/* * fsm.c * * Created on: Oct 18, 2024 * Author: akeguo */ //平面区域自动作业 Region_Automated_Task_Func_Alternately_Plane();//一米只行驶95cm， #include "fsm.h" #include #include "BHBF_ROBOT.h" #include "BSP/bsp_include.h" #include "MSP/msp_PID.h" #include "robot_state.h" #include "MSP/msp_MK32_1.h" #include "motors.h" #include "math.h" #include "msp_Blast_Machine.h" int index_counter = 0; double Error_Cont_Thresh=0.5; int Forward_Flag_Robot_Manual=1; //int Markkk = 0; int32_t GetLaneChangeDistance(); int32_t GetVehicleSpeed(); void Vertical_Left_Right_Lane_Change_Control(int *LaneChangeCmd, int left_or_right); void Horizontal_Up_Down_Lane_Change_Control(int *LaneChangeCmd, int up_or_down, int head_to_left_or_right); int32_t *RobotSpeed; int32_t *RobotLaneChangeDistance; int LaneChangeCommand; int LastLaneChangeCommand; int LaneChangeCommandState; char is_upper_computer_take_over_control=0; int var0; int var1; int Plane_Lane_Change_Falg=0; int Region_Task_Falg=0; double Horzi_Der_Angle_Deg=0; double CV_Robot_Deri_Angle_Deg_Grity=0; double CV_Robot_Deri_Angle_Deg_Plane=0; int Robot_Platform_Back_Flag_Y=0; double Move_Base_Speed_Count_Meter_Min=201.7*7.64;//天太电机1m/min double Swing_Deg_Count=11014; double Swing_Speed_Deg_Sencond=201.7;//HJ32-121 double System_time_SR=0.1; double Vertical_Der_Angle_Deg=0; double Vertical_Change_Road_Time=5000; double Horizontal_Change_Road_Time=5000; double Plane_Change_Road_Time=5000; int16_t GV_Gyro_MFOG40_Gyro_Angular=0; int Change_Road_Jude_Flag=0; int Swing_Limit_Flag=0; int Robot_Platform_Back_Flag=0; int Auto_Job_Back_Time_MS_Count=0; PushRod_STATE_t Current_PushRod_STATE;//推杆运动 MoveSTATE_t CurrentMoveState;//机器人运动 SwingSTATE_t CurrentSwingState;//摆臂运动 int GV_Robot_Move_Speed=0; // 2 double GV_Robot_Change_Lane_Distance=0; // 3 int GV_Swing_Speed=0; // 4 int GV_Robot_symmetricalOrNot=0; // 5 int GV_Robot_Swing_Range_Angle=0; // 6 int GV_Robot_asymmetricalAngleSetValue=0; // 7 int GV_Robot_backMode=0; // 8 double GV_Robot_Back_Distance=0; // 9 double GV_Robot_Back_Speed=0; // 10 int GV_Robot_Press_Set=0; // 11 double GV_Robot_Vertical_Adjust = 0; // 12 int GV_Robot_Length_Homework=0; // 13 int GV_Robot_Width_Homework=0; // 14 int GV_Operation_Mode; double Robot_Speed_Base=0; double Robot_Alternately_Speed=0; double Robot_Countinus_Speed=0; double Change_Road_Speed=201.7*7.64*3; double Deri_Speed_Robot_MAX=201.7*7.64*3*1.5; double Rock_Angle=0; double GV_Work_Mode_Speed=0; int ctl_flag = 0; int ctl_flag_1 = 0; int Blast_Machine_Flag_0=10; int Blast_Count_1=0; int Blast_Count_2=0; int Blast_Count_3 = 0; int Blast_Machine_Flag=0; int Blast_Machine_Flag_1 = 0; int Blast_Machine_Open_counts = 0; typedef enum { Halt_Mode=0, Manual_Mode=1, Horizontal_Mode=2, Flat_Mode=3, Vertical_Mode_Left=4, Vertical_Mode_Right=5, Regional_Horizontal_Automatic_Task=6, Regional_Flat_Automatic_Task=7, } GV_Robot_Operation_Mode; typedef enum { Serial_Mode_Config = 0, // 串口模式确定 Action_Execute = 1, // 动作执行 Action_Shut_down = 2, // 动作关闭 } GV_Main_Core_Mode; typedef enum { Forward_Attitude_Judge=1, Forward_Attitude_Adjust=2, Forward_Motion=3, Work_Way=4, Fight_Alternately=5, Fight_retreating=6, } GV_Robot_Forward_Falg; typedef enum { Parameter_Calculation=0, Backward_Task_X=1, Turn_To_Y=2, Lane_Change_Y=3, Turn_Back_X=4, Region_Auto_Close=5, } Robot_Region_Auto_Falg; typedef enum { RE_Horizontal_Parameter_Calculation=0, RE_Horizontal_Turn_To_Desire_Angle=1, RE_Horizontal_Backward_Task_X=2, RE_Horizontal_Turn_To_Y=3, RE_Horizontal_Lane_Change_Y=4, RE_Horizontal_Turn_Back_X=5, RE_Horizontal_Region_Auto_Close=6, } Robot_Region_Auto_Falg_Horizontal; typedef enum { Regional_Parameter_Determination=0, Regional_Parameter_Calculation=1, Regional_Alternately=2, Regional_Continuous=3, } Robot_Region_Flag_State; typedef enum { Expected_Angle_Confir=0, Turn_To_Y_C=1, Back_Runing_Y=2, Turn_To_X=3, Change_Road_Completed=4, } Plane_Change_Road_Falg; typedef enum { Parameter_Calculation_Elevation=1, Turn_To_Desired_Angle=2, Backward_Task_X_Elevation=3, Lane_Change_Y_Elevation=4, Region_Auto_Close_Elevation=5, } Robot_Region_Auto_Falg_Elevation; transition_t Set_PushRod_States[]= { { PushRod_HALT, Move_PushRod_Halt_Func_Do }, { PushRod_UP, Move_PushRod_Up_Func_Do }, { PushRod_Down, Move_PushRod_Down_Func_Do }, }; transition_t Set_Swing_States[]= { {Swing_Left,Move_Swing_Left_Func_Do}, {Swing_Right,Move_Swing_Right_Func_Do}, {Swing_Halt,Move_Swing_Halt_Func_Do}, }; //手动下 机器人运动 transition_t MoveTransitions[] = { { Move_Forwards, Forwards_State_Do },//前进 { Move_Backwards, Backwards_State_Do },//后退 { Move_TurnLeft, TurnLeft_State_Do },//左转 { Move_TurnRight, TurnRight_State_Do },//右转 { Move_HALT, HALT_State_Do },//暂停 { Move_Vertical_Task_Forwards, Move_Horizontal_Vertical_Task_Forwards_Do_Forwards },//竖直前向运动 { Move_Vertical_Task_Backwards, Move_Horizontal_Vertical_Task_Backwards_Do_Backward },//竖直后退运动 { Move_Horizontal_Task_Forwards, Move_Horizontal_Vertical_Task_Forwards_Do_Forwards },//水平前向运动 { Move_Horizontal_Task_Backwards, Move_Horizontal_Vertical_Task_Backwards_Do_Backward },//水平后退运动 }; void Fsm_Init() { GF_BSP_Interrupt_Add_CallBack(DF_BSP_InterCall_TIM8_2ms_PeriodElapsedCallback, GF_Dispatch); } char Switch_Off_Flag = 0; int Robot_Main_Flag=0;// double Robot_Angle_G_P_Deg=0; //double MF40G_Angle_Add_Count_Fact=0.000005;//0.00001 double MF40G_Angle_Add_Count_Fact=0.000005+0.00000333;//0.00000333的计算方法为；以0.5速度后退5米，反底偏差为8cm，8cm对应的直角角度为1度，1/10/60/500=0.00000333； double MF40G_Angle_Add_Deg=0; int MF40G_Angle_Add_Count=0; int stop_flag = 0; void GF_Dispatch() { if(P_MK32->CH7_SD != -1000) { Blast_Machine_Control_Fun(); } MF40G_Angle_Add_Count++; MF40G_Angle_Add_Deg=GV.MFOG40_Plane.gyro_angular-MF40G_Angle_Add_Count*MF40G_Angle_Add_Count_Fact; if(MF40G_Angle_Add_Deg>180) { MF40G_Angle_Add_Deg=MF40G_Angle_Add_Deg-360; } else if(MF40G_Angle_Add_Deg<-180) { MF40G_Angle_Add_Deg=MF40G_Angle_Add_Deg+360; } //EmergencyStop_Hardware_Communication_Detection_1(); EmergencyStop_Hardware_Communication_Detection(); // if(GV.SystemErrorData.ErrorCode != 0 && GV.SystemErrorData.ErrorCode != 2) // { // Blast_Machine_Control_Fun(); // } switch (Robot_Main_Flag) { case Serial_Mode_Config://读取串口模式，推杆手动控制在其中，机器人运动则进入下一模式中； // Markkk = 0; // GV.SwingMotor.Position_immediately1_Lag2=AAAAA; // GV.SwingMotor.Tar_Position_count=BBBBB; // GV.SwingMotor.Tar_Position_Velcity_RPM=CCCCC; GV_Operation_Mode=(int)CV.PV.Robot_Operation_Mode; if(ctl_flag == 1)//自动喷砂结束关闭喷砂机 { Blast_Machine_Close_Fun(); ctl_flag = 0; } // PushRod_Contronl();//推杆上下停止控制 double y_value = P_MK32->CH2_LY_V; // 假设 Y 是纵向（前后） double x_value = P_MK32->CH3_LY_H; // 假设 X 是横向（左右） double w_value = P_MK32->CH0_RY_H; // 假设 X 是横向（左右） double z_value = P_MK32->CH1_RY_V; // 假设 X 是横向（左右） if((fabs(y_value)>=300)||(fabs(x_value)>=300)||(P_MK32->CH7_SD==-1000)||(P_MK32->CH4_SA!=0) ||(fabs(w_value)>=300)||(fabs(z_value)>=300)||(P_MK32->CH5_SB!=0)) { Robot_Main_Flag++; } //计划将机器人运动参数计算，需要放在此处，但暂时还没做 PV_Data_Reading(); Move_Speed_Define(); Horiz_Angle_Judge(); Robot_Halt_Mode(); Swing_Mode_Determination(); Blast_and_Blower_Control_Fun(); //控制喷砂机 break; case Action_Execute: switch (GV_Operation_Mode) { case Halt_Mode: Robot_Halt_Mode(); Robot_Angle_G_P_Deg=0; break; case Manual_Mode: Robot_Manual_Operation_Mode();//手动操作//已检查无问题 Robot_Angle_G_P_Deg=0; break; case Horizontal_Mode: Horizontal_Operatin_Main_Func(); Robot_Angle_G_P_Deg=GV.TL720DParameters.RF_Angle_Roll; break; case Flat_Mode: Plane_Operatin_Main_Func(); // Markkk = 333; Robot_Angle_G_P_Deg=MF40G_Angle_Add_Deg*100; break; case Vertical_Mode_Left: Vertical_Operatin_Main_Func_Left(); Robot_Angle_G_P_Deg=GV.TL720DParameters.RF_Angle_Roll; break; case Vertical_Mode_Right: Vertical_Operatin_Main_Func_Right();//今日检查 Robot_Angle_G_P_Deg=GV.TL720DParameters.RF_Angle_Roll; break; case Regional_Horizontal_Automatic_Task: Regional_Horizontal_Automatic_Functionc();//待检查 Robot_Angle_G_P_Deg=GV.TL720DParameters.RF_Angle_Roll; break; case Regional_Flat_Automatic_Task: Regional_Plane_Automatic_Functionc(); Robot_Angle_G_P_Deg=MF40G_Angle_Add_Deg*100; break; default: Robot_Main_Flag=Serial_Mode_Config; Robot_Halt_Mode(); break; } break; case Action_Shut_down: Robot_Halt_Mode(); Robot_Main_Flag=Serial_Mode_Config; break; } // IV_control(); if(MF40G_Angle_Add_Count%10==0) { IV_control_1(); } } void action_perfrom(transition_t transitions[], int length, int state) { for (index_counter = 0; index_counter < length; index_counter++) { if (transitions[index_counter].State == state) { if (transitions[index_counter].robotRun != NULL) { transitions[index_counter].robotRun(); } break; //return; } } } void Robot_Halt_Mode() { GV.LeftMotor.Target_Velcity = 0; GV.RightMotor.Target_Velcity = 0; Move_Swing_Halt_Func_Do(); } void Robot_Manual_Operation_Mode() { // PushRod_Contronl();//推杆上下停止控制 Robot_Manual_Operation_Function(); Robot_Main_Mode_Jude(); UltraStopReverse_Manually_Backward(); PushRod_Contronl(); // Pressure_Adaptive_Function_Uptata(200); // PushRod_Contronl();//推杆上下停止控制 } void UltraStopReverse(int Flag_Current) { static int UltraStop_Flag_1; static int Flag_Last; static int Stop_Flag; if(P_MK32->CH10_LD1<=100) { UltraStop_Flag_1=3; } else { // UltraStop_Flag_1=0; } switch(UltraStop_Flag_1) { case 0: if((GV.Robot_To_Wall_mm.KS206_1_Measuring_Distance>=250)||(GV.Robot_To_Wall_mm.KS206_2_Measuring_Distance>=250)) { Stop_Flag++; } else { Stop_Flag=0; } if(Stop_Flag>10) { Stop_Flag=0; UltraStop_Flag_1++; } break; case 1: GV.LeftMotor.Target_Velcity =0; GV.RightMotor.Target_Velcity =0; if(Flag_Last!=Flag_Current) { UltraStop_Flag_1=0; } break; case 3: UltraStop_Flag_1=0; break; } Flag_Last=Flag_Current; } void UltraStopReverse_Manually_Backward() { // static int UltraStop_Flag; // switch(UltraStop_Flag) // { // case 0: // if((GV.Robot_To_Wall_mm.KS206_1_Measuring_Distance>=205)||(GV.Robot_To_Wall_mm.KS206_2_Measuring_Distance>=205)) // { // UltraStop_Flag++; // } // break; // case 1: // if(P_MK32->CH2_LY_V<=-300) // { // GV.LeftMotor.Target_Velcity =0; // GV.RightMotor.Target_Velcity =0; // } // else // { // UltraStop_Flag=0; // } // break; // } } void PushRod_Contronl() { if (P_MK32->CH1_RY_V >= 300) { Current_PushRod_STATE = PushRod_UP; } else if (P_MK32->CH1_RY_V <= -300) { Current_PushRod_STATE = PushRod_Down; } else { Current_PushRod_STATE = PushRod_HALT; } action_perfrom(Set_PushRod_States, sizeof(Set_PushRod_States) / sizeof(transition_t), Current_PushRod_STATE); } //void Blast_Machine_Control() //{ // if(P_MK32->CH6_SC==-1000) // { // Blast_Machine_Flag=1; // } // else if(P_MK32->CH6_SC==1000) // { // Blast_Machine_Flag=4; // } // else // { // Move_PushRod_Halt_Func_Do_1(); // Move_PushRod_Halt_Func_Do(); // Blast_Count_3 = 0; // Blast_Machine_Flag = 0; // Blast_Machine_Flag_1 = 0; // Blast_Machine_Open_counts = 0; // // ctl_flag = 0; // ctl_flag_1 = 0; // stop_flag = 0; // // } // // switch(Blast_Machine_Flag) // { // case 1: // if(GV.SystemErrorData.ErrorCode == 0 || GV.SystemErrorData.ErrorCode == 2) // { // Blast_Machine_Open(); // } // break; // case 4: // Blast_Machine_Close(); // break; // } //} // //void Blast_Machine_Open() //{ // Move_PushRod_Halt_Func_Do_1(); // switch(Blast_Machine_Flag_1) // { // case 0: // Blast_Count_3 ++; // Move_PushRod_Up_Func_Do(); // if(Blast_Count_3 >= 250) // { // Blast_Count_3 = 0; // Blast_Machine_Flag_1 = 10; // } // break; // // case 10: // Blast_Count_3 ++; // Move_PushRod_Halt_Func_Do(); // if(Blast_Count_3 >= 60) // { // Blast_Count_3 = 0; // Blast_Machine_Flag_1 = 1; // } // break; // // case 1: // Blast_Count_3 ++; // Move_PushRod_Down_Func_Do(); // if(Blast_Count_3 >= 250) // { // Blast_Count_3 = 0; // Blast_Machine_Flag_1 = 2; // Blast_Machine_Open_counts = Blast_Machine_Open_counts + 1; // } // break; // case 2: // if(Blast_Machine_Open_counts == 10) // { // Blast_Machine_Flag_1 = 3; // Blast_Machine_Open_counts = 0; // } // else // { // Blast_Count_3 ++; // Move_PushRod_Halt_Func_Do(); // if(Blast_Count_3 >= 60) // { // Blast_Count_3 = 0; // Blast_Machine_Flag_1 = 0; // } // } // break; // case 3: // Move_PushRod_Halt_Func_Do(); // break; // } //} // //void Blast_Machine_Close() //{ // Move_PushRod_Halt_Func_Do(); // switch(Blast_Machine_Flag_1) // { // case 0: // Blast_Count_3 ++; // Move_PushRod_Up_Func_Do_1(); // if(Blast_Count_3 >= 250) // { // Blast_Count_3 = 0; // Blast_Machine_Flag_1 = 10; // } // break; // // case 10: // Blast_Count_3 ++; // Move_PushRod_Halt_Func_Do_1(); // if(Blast_Count_3 >= 60) // { // Blast_Count_3 = 0; // Blast_Machine_Flag_1 = 1; // } // break; // case 1: // Blast_Count_3 ++; // Move_PushRod_Down_Func_Do_1(); // if(Blast_Count_3 >= 250) // { // Blast_Count_3 = 0; // Blast_Machine_Flag_1 = 2; // Blast_Machine_Open_counts = Blast_Machine_Open_counts + 1; // } // break; // case 2: // if(Blast_Machine_Open_counts == 10) // { // Blast_Machine_Flag_1 = 3; // Blast_Machine_Open_counts = 0; // } // else // { // Blast_Count_3 ++; // Move_PushRod_Halt_Func_Do_1(); // if(Blast_Count_3 >= 60) // { // Blast_Count_3 = 0; // Blast_Machine_Flag_1 = 0; // } // } // break; // case 3: // Move_PushRod_Halt_Func_Do_1(); // ctl_flag = 0; // stop_flag = 0; // break; // } //} void Robot_Manual_Operation_Function() { double y_value = P_MK32->CH2_LY_V; // 假设 Y 是纵向（前后） double x_value = P_MK32->CH3_LY_H; // 假设 X 是横向（左右） double w_value = P_MK32->CH0_RY_H; // 假设 X 是横向（左右） double z_value = P_MK32->CH1_RY_V; // 假设 X 是横向（左右） static int Horizontal_Manual_Flag ; // 静态局部变量 if((fabs(y_value)>=300)||(fabs(x_value)>=300)||(fabs(w_value)>=300)||(fabs(z_value)>=300)) { switch(Horizontal_Manual_Flag) { case 0: Horizontal_Manual_Flag++; break; case 1: if((fabs(y_value)>=300)||(fabs(x_value)>=300)) { double Rock_Angle= atan2(y_value, x_value); Rock_Angle=Rock_Angle/3.1415926*180; if((Rock_Angle>=-20)&&(Rock_Angle<=20)) { TurnRight_State_Do(); Forward_Flag_Robot_Manual=1; } else if((Rock_Angle>=70)&&(Rock_Angle<=110)) { Forwards_State_Do(); } else if((Rock_Angle>=-110)&&(Rock_Angle<=-70)) { Backwards_State_Do(); } else if(fabs(Rock_Angle)>=160) { TurnLeft_State_Do(); Forward_Flag_Robot_Manual=1; } else { HALT_State_Do(); } } else { HALT_State_Do(); } Robot_Swing_Operation_Function();//摆臂运动函数 break; } } else { Horizontal_Manual_Flag=0; HALT_State_Do(); //Move_Swing_Halt_F //unc_Do(); Forward_Flag_Robot_Manual=1; } } void Robot_Swing_Operation_Function() { if(P_MK32->CH0_RY_H>300) { Move_Swing_Right_Func_Do(); } else if(P_MK32->CH0_RY_H<-300) { Move_Swing_Left_Func_Do(); } else { Move_Swing_Halt_Func_Do(); } /* *Swing_Speed = CV.PV.Swing_Speed;*/ // action_perfrom(Set_Swing_States,sizeof(Set_Swing_States) / sizeof(transition_t), CurrentSwingState); } void Robot_Swing_Operation_Function_Position() { } int Forward_Flag_Robot_Manual_Count=0; // 静态局部变量 int Forward_Flag_Robot_Auto=0; void Horizontal_Operatin_Main_Func() { Blast_Machine_Control_Fun(); if(P_MK32->CH7_SD==-1000)//自动行驶 { Move_Horizontal_Auto_Sub_Func();//自动后退 Forward_Flag_Robot_Auto UltraStopReverse(Forward_Flag_Robot_Auto); Change_Road_Jude_Flag=0; Forward_Flag_Robot_Manual=1; Pressure_Adaptive_Function_Uptata(200); } else if(P_MK32->CH4_SA==-1000)//换道 { Change_Road_Down_Left_Right(); Forward_Flag_Robot_Auto=1; Forward_Flag_Robot_Manual=1; } else if(P_MK32->CH5_SB==-1000)//前进自动巡航 { Move_Horizontal_Manual_Sub_Func_Forwards(); Forward_Flag_Robot_Auto=1; Change_Road_Jude_Flag=0; } else if(P_MK32->CH5_SB==1000)//后退自动巡航 { Move_Horizontal_Manual_Sub_Func_Backwards(); Forward_Flag_Robot_Auto=1; Change_Road_Jude_Flag=0; UltraStopReverse(Forward_Flag_Robot_Manual); } else { Horizontal_Manual_Operation_Func(); //Forward_Flag_Robot_Manual Move_Horizontal_Manual_Sub_Func(); Forward_Flag_Robot_Auto=1; Change_Road_Jude_Flag=0; Robot_Main_Mode_Jude(); Swing_Mode_Determination();//摆臂方式确定 PushRod_Contronl(); UltraStopReverse_Manually_Backward(); } } void Move_Horizontal_Auto_Sub_Func() { switch (Forward_Flag_Robot_Auto) { case Forward_Attitude_Judge: Horiz_Angle_Judge(); Forward_Flag_Robot_Auto++; break; case Forward_Attitude_Adjust: Robot_Posture_Adjus_Gravity(); if(Angle_Error_LLL0) { Change_Road_Jude_Flag=1; } else { Change_Road_Jude_Flag=2; } CRLU_Flag=0; break; case 1: Change_Road_Left(); break; case 2: Change_Road_Right(); break; } // UltraStopReverse(CRLU_Flag); } void Horizontal_Manual_Operation_Func() { double y_value = P_MK32->CH2_LY_V; // 假设 Y 是纵向（前后） double x_value = P_MK32->CH3_LY_H; // 假设 X 是横向（左右） double w_value = P_MK32->CH0_RY_H; // 假设 X 是横向（左右） double z_value = P_MK32->CH1_RY_V; // 假设 X 是横向（左右） static int Horizontal_Manual_Flag ; // 静态局部变量 if((fabs(y_value)>=300)||(fabs(x_value)>=300)||(fabs(w_value)>=300)||(fabs(z_value)>=300)) { switch(Horizontal_Manual_Flag) { case 0: Horizontal_Manual_Flag++; break; case 1: if((fabs(y_value)>=300)||(fabs(x_value)>=300)) { Rock_Angle= atan2(y_value, x_value); Rock_Angle=Rock_Angle/3.1415926*180; if((Rock_Angle>=-30)&&(Rock_Angle<=30)) { TurnRight_State_Do(); Forward_Flag_Robot_Manual=1; } else if((Rock_Angle>=60)&&(Rock_Angle<=120)) { Move_Horizontal_Manual_Sub_Func_Forwards(); } else if((Rock_Angle>=-120)&&(Rock_Angle<=-60)) { Move_Horizontal_Manual_Sub_Func_Backwards(); } else if(fabs(Rock_Angle)>=150) { TurnLeft_State_Do(); Forward_Flag_Robot_Manual=1; } else { HALT_State_Do(); Forward_Flag_Robot_Manual=1; } } else { HALT_State_Do(); Forward_Flag_Robot_Manual=1; } Robot_Swing_Operation_Function();//摆臂运动函数 break; } } else { Horizontal_Manual_Flag=0; HALT_State_Do(); Forward_Flag_Robot_Manual=1; Move_Swing_Halt_Func_Do(); } } void Vertical_Operatin_Main_Func_Left() { Blast_Machine_Control_Fun(); if(P_MK32->CH7_SD==-1000)//自动行驶 { Move_Vertical_Auto_Sub_Func(); UltraStopReverse(Forward_Flag_Robot_Auto); CRLU_Flag=0; Forward_Flag_Robot_Manual=1; Forward_Flag_Robot_Manual_Count=0; Pressure_Adaptive_Function_Uptata(200); } else if(P_MK32->CH4_SA==-1000)//从右向左换道 { Change_Road_Down_Up_Right_To_Left(); Forward_Flag_Robot_Auto=1; Forward_Flag_Robot_Manual=1; Forward_Flag_Robot_Manual_Count=0; } else if(P_MK32->CH5_SB==-1000)//前进自动巡航 { if(Forward_Flag_Robot_Manual_Count<1) { Forward_Flag_Robot_Manual_Count++; Forward_Flag_Robot_Manual=1; } Move_Vertical_Manual_Sub_Func_Forward(); CRLU_Flag=0; } else if(P_MK32->CH5_SB==1000)//后退自动巡航 { if(Forward_Flag_Robot_Manual_Count<1) { Forward_Flag_Robot_Manual_Count++; Forward_Flag_Robot_Manual=1; } Move_Vertical_Manual_Sub_Func_Backward(); UltraStopReverse(Forward_Flag_Robot_Manual); CRLU_Flag=0; } else { Vertical_Manual_Operation_Func(); CRLU_Flag=0; Forward_Flag_Robot_Auto=1; Robot_Platform_Back_Flag=0; Robot_Main_Mode_Jude(); Swing_Limit_Flag=0; Forward_Flag_Robot_Manual_Count=0; UltraStopReverse_Manually_Backward(); PushRod_Contronl(); } } void Vertical_Operatin_Main_Func_Right() { Blast_Machine_Control_Fun(); if(P_MK32->CH7_SD==-1000)//自动行驶 { Move_Vertical_Auto_Sub_Func(); UltraStopReverse(Forward_Flag_Robot_Auto); CRLU_Flag=0; Forward_Flag_Robot_Manual=1; Forward_Flag_Robot_Manual_Count=0; Pressure_Adaptive_Function_Uptata(200); } // else if(P_MK32->CH4_SA==-1000)//从右向左换道 // { // Change_Road_Down_Up_Right_To_Left(); // Forward_Flag_Robot_Auto=1; // Forward_Flag_Robot_Manual=1; // Forward_Flag_Robot_Manual_Count=0; // } else if(P_MK32->CH4_SA==-1000)//从左到右换道 { Change_Road_Down_Up_Left_To_Right(); Forward_Flag_Robot_Auto=1; Forward_Flag_Robot_Manual=1; Forward_Flag_Robot_Manual_Count=0; } else if(P_MK32->CH5_SB==-1000)//前进自动巡航 { if(Forward_Flag_Robot_Manual_Count<1) { Forward_Flag_Robot_Manual_Count++; Forward_Flag_Robot_Manual=1; } Move_Vertical_Manual_Sub_Func_Forward(); CRLU_Flag=0; } else if(P_MK32->CH5_SB==1000)//ho { if(Forward_Flag_Robot_Manual_Count<1) { Forward_Flag_Robot_Manual_Count++; Forward_Flag_Robot_Manual=1; } Move_Vertical_Manual_Sub_Func_Backward(); UltraStopReverse(Forward_Flag_Robot_Manual); CRLU_Flag=0; } else { Vertical_Manual_Operation_Func(); CRLU_Flag=0; Forward_Flag_Robot_Auto=1; Robot_Platform_Back_Flag=0; Robot_Main_Mode_Jude(); Swing_Limit_Flag=0; Forward_Flag_Robot_Manual_Count=0; UltraStopReverse_Manually_Backward(); PushRod_Contronl();//推杆上下停止控制 } } void Move_Vertical_Manual_Sub_Func_Forward() { switch (Forward_Flag_Robot_Manual) { case Forward_Attitude_Judge: Vertical_Angle_Judge(); Forward_Flag_Robot_Manual++; break; case Forward_Attitude_Adjust: Robot_Posture_Adjus_Gravity(); if(Angle_Error_LLLCH4_SA==-1000)//换道 { Swing_Limit_Contrl();//摆臂限位控制 Forward_Flag_Robot_Auto=1; // Change_Road_Plane(); Change_Road_Plane_Continuous_FanDi_Left(); Region_Task_Falg=0; } else if(P_MK32->CH4_SA==1000)//边打边换道//与边打边退速度相同； { Swing_Limit_Contrl();//摆臂限位控制 Forward_Flag_Robot_Auto=1; Change_Road_Plane_Continuous_FanDi_Right(); Region_Task_Falg=0; } else if(P_MK32->CH7_SD==-1000)//自动行驶 { Move_Plane_Auto_Sub_Func(); UltraStopReverse(Forward_Flag_Robot_Auto); CRLU_Flag=0; Region_Task_Falg=0; } else if(P_MK32->CH5_SB==-1000)//前进巡航 { Move_Plane_Manual_Sub_Func_Fordwards(); //Region_Automated_Task_Func_Alternately_Horizontal(); } else if(P_MK32->CH5_SB==1000)//后退巡航 { Move_Plane_Manual_Sub_Func_Backwards(); UltraStopReverse(Forward_Flag_Robot_Manual); //Region_Automated_Task_Func_Alternately_Horizontal(); } else { Plane_Manual_Operation_Func(); Forward_Flag_Robot_Auto=1; CRLU_Flag=0; Robot_Main_Mode_Jude(); UltraStopReverse_Manually_Backward(); } } void Move_Plane_Auto_Sub_Func() { switch (Forward_Flag_Robot_Auto) { case Forward_Attitude_Judge: Plane_Angle_Judge(); Forward_Flag_Robot_Auto++; break; case Forward_Attitude_Adjust: Forward_Flag_Robot_Auto++; Auto_Job_Back_Time_MS_Count=0; Robot_Platform_Back_Flag=0; Swing_Limit_Flag=0; break; case Forward_Motion: // Move_Plane_Task_Backwards_Do(); Forward_Flag_Robot_Auto++;//默认模式为打一道退一次 break; case Work_Way: if(GV_Robot_backMode==1) { Forward_Flag_Robot_Auto=Fight_Alternately; } else if(GV_Robot_backMode==2) { Forward_Flag_Robot_Auto=Fight_retreating; } break; case Fight_Alternately: Fight_Alternately_Function_Plane(); break; case Fight_retreating: Fight_Countinus_Function_Plane(); break; default: HALT_State_Do(); break; } } void Plane_Angle_Judge() { CV_Robot_Deri_Angle_Deg_Plane=MF40G_Angle_Add_Deg; } int Backward_Num_X_Count; //X后退次数 int Lane_Change_Backward_Count_Y; int Plane_Complete_Total_XY=0; int Plane_Complete_Count=0; double Plane_Turn_Angle_Deg; double Deri_Angle_Deg_X[2];//X方向两个期望角度 double X_Deri_Angle[2]; //输入给机器人的期望角度 int Backward_Count_X; //X向后退时间 double Backward_Num_X_Total; //X向后退总次数 double Lane_Change_Num_Total_Y; //Y向后退次数； void Region_Automated_Task_Func_Alternately_Plane() { switch (Region_Task_Falg) { case Parameter_Calculation: Deri_Angle_Deg_X[0]=MF40G_Angle_Add_Deg; Plane_Turn_Angle_Deg=MF40G_Angle_Add_Deg+90; Deri_Angle_Deg_X[1]=Deri_Angle_Deg_X[0]+180; if(Deri_Angle_Deg_X[1]>180) { Deri_Angle_Deg_X[1]=Deri_Angle_Deg_X[1]-360; } Plane_X_Backward_Time_Calculation(&Backward_Num_X_Total); Plane_Change_Road_Backward_Num_Calculation(&Lane_Change_Num_Total_Y); Plane_Y_Lane_Change_Time_Calculation(&Plane_Complete_Total_XY); Region_Task_Falg++; Backward_Count_X=0; X_Deri_Angle[0]=Deri_Angle_Deg_X[0]; Swing_Limit_Flag=0; Backward_Num_X_Count=0; Plane_Complete_Count=0; Robot_Platform_Back_Flag=0; Auto_Job_Back_Time_MS_Count=0; break; case Backward_Task_X: CV_Robot_Deri_Angle_Deg_Plane=X_Deri_Angle[0]; Swing_Limit_Contrl();//摆臂限位控制 Robot_Platform_Back_Contronl_Plane(); if(Backward_Num_X_Count>=Backward_Num_X_Total) { Auto_Job_Back_Time_MS_Count=0; Robot_Platform_Back_Flag=0; Region_Task_Falg++; Plane_Complete_Count++; int i=0; i=Plane_Complete_Count%2; if(i==0) { X_Deri_Angle[0]=Deri_Angle_Deg_X[0]; X_Deri_Angle[1]=Deri_Angle_Deg_X[1]; } else { X_Deri_Angle[0]=Deri_Angle_Deg_X[1]; X_Deri_Angle[1]=Deri_Angle_Deg_X[0]; } if(Plane_Complete_Count>=Plane_Complete_Total_XY) { ctl_flag_1 = 1; Region_Task_Falg=Region_Auto_Close; } Backward_Num_X_Count=0; } Robot_Platform_Back_Flag_Y=0; break; case Turn_To_Y: Swing_Limit_Contrl();//摆臂限位控制 CV_Robot_Deri_Angle_Deg_Plane=Plane_Turn_Angle_Deg; Robot_Posture_Adjus_Plane(); if(Angle_Error_LLL=Lane_Change_Num_Total_Y) { Region_Task_Falg++; Backward_Num_X_Count=0; } break; case Turn_Back_X: CV_Robot_Deri_Angle_Deg_Plane=X_Deri_Angle[0]; Swing_Limit_Contrl(); Robot_Posture_Adjus_Plane(); if(Angle_Error_LLL=Backward_Num_X_Total) { Auto_Job_Back_Time_MS_Count=0; Robot_Platform_Back_Flag=2; Region_Task_Falg++; Plane_Complete_Count++; int i=0; i=Plane_Complete_Count%2; if(i==0) { X_Deri_Angle[0]=Deri_Angle_Deg_X[0]; X_Deri_Angle[1]=Deri_Angle_Deg_X[1]; } else { X_Deri_Angle[0]=Deri_Angle_Deg_X[1]; X_Deri_Angle[1]=Deri_Angle_Deg_X[0]; } if(Plane_Complete_Count>=Horizontal_Complete_Total_XY) { ctl_flag_1 = 1; Region_Task_Falg=RE_Horizontal_Region_Auto_Close; } Backward_Num_X_Count=0; } Robot_Platform_Back_Flag_Y=0; break; case RE_Horizontal_Turn_To_Y: Swing_Limit_Contrl();//摆臂限位控制 CV_Robot_Deri_Angle_Deg_Grity=Horizontal_Turn_Angle_Deg; Robot_Posture_Adjus_Gravity(); if(Angle_Error_LLL=Lane_Change_Num_Total_Y) { Region_Task_Falg++; Backward_Num_X_Count=0; } break; case RE_Horizontal_Turn_Back_X: CV_Robot_Deri_Angle_Deg_Grity=X_Deri_Angle[0]; Swing_Limit_Contrl(); Robot_Posture_Adjus_Gravity(); if(Angle_Error_LLLCH2_LY_V; // 假设 Y 是纵向（前后） double x_value = P_MK32->CH3_LY_H; // 假设 X 是横向（左右） double w_value = P_MK32->CH0_RY_H; // 假设 X 是横向（左右） double z_value = P_MK32->CH1_RY_V; // 假设 X 是横向（左右） static int Horizontal_Manual_Flag ; // 静态局部变量 if((fabs(y_value)>=300)||(fabs(x_value)>=300)||(fabs(w_value)>=300)||(fabs(z_value)>=300)) { switch(Horizontal_Manual_Flag) { case 0: Horizontal_Manual_Flag++; break; case 1: if((fabs(y_value)>=300)||(fabs(x_value)>=300)) { Rock_Angle= atan2(y_value, x_value); Rock_Angle=Rock_Angle/3.1415926*180; if((Rock_Angle>=-30)&&(Rock_Angle<=30)) { TurnRight_State_Do(); Forward_Flag_Robot_Manual=1; } else if((Rock_Angle>=60)&&(Rock_Angle<=120)) { Move_Vertical_Manual_Sub_Func_Forward(); } else if((Rock_Angle>=-120)&&(Rock_Angle<=-60)) { Move_Vertical_Manual_Sub_Func_Backward(); } else if(fabs(Rock_Angle)>=150) { TurnLeft_State_Do(); Forward_Flag_Robot_Manual=1; } else { HALT_State_Do(); } } else { HALT_State_Do(); } Robot_Swing_Operation_Function();//摆臂运动函数 break; } } else { Horizontal_Manual_Flag=0; HALT_State_Do(); Move_Swing_Halt_Func_Do(); Forward_Flag_Robot_Manual=1; } } void Plane_Manual_Operation_Func() { double y_value = P_MK32->CH2_LY_V; // 假设 Y 是纵向（前后） double x_value = P_MK32->CH3_LY_H; // 假设 X 是横向（左右） double w_value = P_MK32->CH0_RY_H; // 假设 X 是横向（左右） double z_value = P_MK32->CH1_RY_V; // 假设 X 是横向（左右） static int Horizontal_Manual_Flag ; // 静态局部变量 if((fabs(y_value)>=300)||(fabs(x_value)>=300)||(fabs(w_value)>=300)||(fabs(z_value)>=300)) { switch(Horizontal_Manual_Flag) { case 0: Horizontal_Manual_Flag++; Forward_Flag_Robot_Manual=1; break; case 1: if((fabs(y_value)>=300)||(fabs(x_value)>=300)) { double Rock_Angle= atan2(y_value, x_value); Rock_Angle=Rock_Angle/3.1415926*180; if((Rock_Angle>=-30)&&(Rock_Angle<=30)) { TurnLeft_State_Do_Plane(); Forward_Flag_Robot_Manual=1; } else if((Rock_Angle>=60)&&(Rock_Angle<=120)) { Move_Plane_Manual_Sub_Func_Fordwards(); } else if((Rock_Angle>=-120)&&(Rock_Angle<=-60)) { Move_Plane_Manual_Sub_Func_Backwards(); } else if(fabs(Rock_Angle)>=150) { TurnRight_State_Do_Plane(); Forward_Flag_Robot_Manual=1; } else { HALT_State_Do(); Forward_Flag_Robot_Manual=1; } } else { HALT_State_Do(); } Robot_Swing_Operation_Function();//摆臂运动函数 break; } } else { Horizontal_Manual_Flag=0; HALT_State_Do(); Move_Swing_Halt_Func_Do(); Forward_Flag_Robot_Manual=1; } } void Move_Plane_Manual_Sub_Func_Fordwards() { switch (Forward_Flag_Robot_Manual) { case Forward_Attitude_Judge: Plane_Angle_Judge(); Forward_Flag_Robot_Manual++; break; case Forward_Attitude_Adjust: Forward_Flag_Robot_Manual++; break; case Forward_Motion: Move_Plane_Task_Fordwards_Do_Update(Robot_Speed_Base, CV_Robot_Deri_Angle_Deg_Plane); break; default: HALT_State_Do(); break; } } void Move_Plane_Manual_Sub_Func_Backwards() { switch (Forward_Flag_Robot_Manual) { case Forward_Attitude_Judge: Plane_Angle_Judge(); Forward_Flag_Robot_Manual++; break; case Forward_Attitude_Adjust: Forward_Flag_Robot_Manual++; break; case Forward_Motion: // Move_Plane_Task_Fordwards_Do(); Move_Plane_Task_Backwards_Distance_Do_Update(Robot_Speed_Base, CV_Robot_Deri_Angle_Deg_Plane); break; default: HALT_State_Do(); break; } // UltraStopReverse(Forward_Flag_Robot_Manual); } void Plane_X_Backward_Time_Calculation(double *Num) { *Num=((double)GV_Robot_Length_Homework*100)/GV_Robot_Back_Distance; } void Plane_X_Backward_Time_Calculation_Continuous(int *Time) { *Time=((double)GV_Robot_Length_Homework)/GV_Robot_Back_Speed*60*500;//长度/后退速度=分钟 分钟=60*500毫秒 } void Horizontal_X_Backward_Num_Calculation(double *Num) { *Num=((double)GV_Robot_Length_Homework*100)/GV_Robot_Back_Distance; } void Horizontal_X_Backward_Time_Calculation_Continuous(int *Time) { *Time=((double)GV_Robot_Length_Homework)/GV_Robot_Back_Speed*60*500;//长度/后退速度=分钟 分钟=60*500毫秒 } void Plane_Y_Lane_Change_Time_Calculation(int *Num) { *Num=GV_Robot_Width_Homework*100/GV_Robot_Change_Lane_Distance; } void Horizontal_Y_Lane_Change_Time_Calculation(int *Num) { *Num=GV_Robot_Width_Homework*100/GV_Robot_Change_Lane_Distance; } void Plane_Change_Road_Backward_Num_Calculation(double *Num) { *Num=((double)GV_Robot_Change_Lane_Distance)/GV_Robot_Back_Distance; } void Plane_Change_Road_Backward_Time_Calculation_Continuous(int *Time) { *Time=((double)GV_Robot_Change_Lane_Distance/100)/GV_Robot_Back_Speed*60*500;//换道距离/后退速度=分钟 分钟=60*500毫秒 } void Horizontal_Change_Road_Backward_Num_Calculation(double *Num) { *Num=((double)GV_Robot_Change_Lane_Distance)/GV_Robot_Back_Distance; } void Horizontal_Change_Road_Backward_Time_Calculation_Continuous(int *Time) { *Time=((double)GV_Robot_Change_Lane_Distance/100)/GV_Robot_Back_Speed*60*500;//换道距离/后退速度=分钟 分钟=60*500毫秒 } void Plane_Lane_Change_Func(double *Turn_Angle_Deg_1, double *Turn_Angle_Plane,double *Backward_Time) { //将Turn_Angle_Deg_1的值赋给AA和BB static int Back_Time_Count; switch (Plane_Lane_Change_Falg) { case 0: //转向角度0 CV_Robot_Deri_Angle_Deg_Plane=*Turn_Angle_Plane; Robot_Posture_Adjus_Plane(); if(Angle_Error_LLL*Backward_Time) { Plane_Lane_Change_Falg++; } break; case 2: //转向期望角度1 CV_Robot_Deri_Angle_Deg_Plane=Turn_Angle_Deg_1[0]; Robot_Posture_Adjus_Plane(); if(Angle_Error_LLLCH14_LT>300) { Left_Compensation_Count++; if(Left_Compensation_Count>200) { GV.Left_Compensation=GV.Left_Compensation+25; Left_Compensation_Count=0; } if(GV.Left_Compensation>1000) { GV.Left_Compensation=1000; } } else if(P_MK32->CH14_LT<-300) { Left_Compensation_Count--; if(Left_Compensation_Count<-200) { GV.Left_Compensation=GV.Left_Compensation-25; Left_Compensation_Count=0; if(GV.Left_Compensation<-1000) { GV.Left_Compensation=-1000; } } } else { Left_Compensation_Count=0; } if(P_MK32->CH15_RT>300) { Right_Compensation_Count++; if(Right_Compensation_Count>200) { GV.Right_Compensation=GV.Right_Compensation+25; Right_Compensation_Count=0; if(GV.Right_Compensation>1000) { GV.Right_Compensation=1000; } } } else if(P_MK32->CH15_RT<-300) { Right_Compensation_Count--; if(Right_Compensation_Count<-200) { GV.Right_Compensation=GV.Right_Compensation-25; Right_Compensation_Count=0; if(GV.Right_Compensation<-1000) { GV.Right_Compensation=-1000; } } } else { Right_Compensation_Count=0; } double Robot_Grity=((double )GV.TL720DParameters.RF_Angle_Roll)/100; if((Robot_Grity>-180)&&(Robot_Grity<0)) { Horzi_Der_Angle_Deg=-90+((double)GV.Right_Compensation)/100; } else { Horzi_Der_Angle_Deg=90+((double)GV.Left_Compensation)/100; } CV_Robot_Deri_Angle_Deg_Grity=Horzi_Der_Angle_Deg; } void Horiz_Angle_Judge_4_Direction() { static int Left_Compensation_Count; static int Right_Compensation_Count; if(P_MK32->CH14_LT>300) { Left_Compensation_Count++; if(Left_Compensation_Count>200) { GV.Left_Compensation=GV.Left_Compensation+25; Left_Compensation_Count=0; } if(GV.Left_Compensation>1000) { GV.Left_Compensation=1000; } } else if(P_MK32->CH14_LT<-300) { Left_Compensation_Count--; if(Left_Compensation_Count<-200) { GV.Left_Compensation=GV.Left_Compensation-25; Left_Compensation_Count=0; if(GV.Left_Compensation<-1000) { GV.Left_Compensation=-1000; } } } else { Left_Compensation_Count=0; } if(P_MK32->CH15_RT>300) { Right_Compensation_Count++; if(Right_Compensation_Count>200) { GV.Right_Compensation=GV.Right_Compensation+25; Right_Compensation_Count=0; if(GV.Right_Compensation>1000) { GV.Right_Compensation=1000; } } } else if(P_MK32->CH15_RT<-300) { Right_Compensation_Count--; if(Right_Compensation_Count<-200) { GV.Right_Compensation=GV.Right_Compensation-25; Right_Compensation_Count=0; if(GV.Right_Compensation<-1000) { GV.Right_Compensation=-1000; } } } else { Right_Compensation_Count=0; } double Robot_Grity=((double )GV.TL720DParameters.RF_Angle_Roll)/100; if((Robot_Grity>-180)&&(Robot_Grity<=0)&&((P_MK32->CH2_LY_V<-300)||P_MK32->CH5_SB==1000)) { Horzi_Der_Angle_Deg=-90+((double)GV.Right_Compensation)/100; } else if((Robot_Grity>-180)&&(Robot_Grity<=0)&&((P_MK32->CH2_LY_V>300)||P_MK32->CH5_SB==-1000)) { Horzi_Der_Angle_Deg=-90-((double)GV.Right_Compensation)/100; } else if ((Robot_Grity>0)&&(Robot_Grity<180)&&((P_MK32->CH2_LY_V<-300)||P_MK32->CH5_SB==1000)) { Horzi_Der_Angle_Deg=90+((double)GV.Left_Compensation)/100; } else if((Robot_Grity>0)&&(Robot_Grity<180)&&((P_MK32->CH2_LY_V>300)||P_MK32->CH5_SB==-1000)) { Horzi_Der_Angle_Deg=90-((double)GV.Left_Compensation)/100; } CV_Robot_Deri_Angle_Deg_Grity=Horzi_Der_Angle_Deg; } void Horiz_Angle_Judge_Region() { if(Deri_Angle_Deg_X[0]>0) { Deri_Angle_Deg_X[0]= 90+((double)GV.Left_Compensation)/100; Deri_Angle_Deg_X[1]=-90+((double)GV.Right_Compensation)/100; } else { Deri_Angle_Deg_X[0]= -90+((double)GV.Right_Compensation)/100; Deri_Angle_Deg_X[1]= 90+((double)GV.Left_Compensation)/100; } if(X_Deri_Angle[0]>0) { X_Deri_Angle[0]=90+((double)GV.Left_Compensation)/100; } else { X_Deri_Angle[0]=-90+((double)GV.Right_Compensation)/100; } } void Move_Horizontal_Vertical_Task_Forwards_Do_Forwards(double robotMoveSpeed, double robotDeriAngleDegGrity) { double Deri_Speed_Robot_MAX = robotMoveSpeed * 1.5; double Robot_Speed[4] = {0, 0, 0, 0}; double Robot_Grity = ((double)GV.TL720DParameters.RF_Angle_Roll) / 100; GF_MSP_PID_Now_Der_adj_Com_Horizon(Robot_Grity, robotDeriAngleDegGrity, robotMoveSpeed, Deri_Speed_Robot_MAX, System_time_SR, Robot_Speed); GV.LeftMotor.Target_Velcity = Robot_Speed[0]; GV.RightMotor.Target_Velcity = -Robot_Speed[1]; } void Move_Horizontal_Task_Change_Road_Backwards_Do() { double Robot_Speed[4]={0,0,0,0}; double Robot_Grity=((double )GV.TL720DParameters.RF_Angle_Roll)/100; GF_MSP_PID_Now_Der_adj_Com_Horizon(Robot_Grity, CV_Robot_Deri_Angle_Deg_Grity, Change_Road_Speed, Deri_Speed_Robot_MAX, System_time_SR , Robot_Speed); GV.LeftMotor.Target_Velcity =-Robot_Speed[1]; GV.RightMotor.Target_Velcity =Robot_Speed[0]; } void Move_Plane_Task_Change_Road_Backwards_Do() { double Robot_Speed[4]={0,0,0,0}; double Robot_Grity=MF40G_Angle_Add_Deg; GF_MSP_PID_Now_Der_adj_Com_Horizon(Robot_Grity, CV_Robot_Deri_Angle_Deg_Plane, Change_Road_Speed, Deri_Speed_Robot_MAX, System_time_SR , Robot_Speed); GV.LeftMotor.Target_Velcity =-Robot_Speed[0]; GV.RightMotor.Target_Velcity =Robot_Speed[1]; } void Move_Speed_Define() { Robot_Speed_Base= ((double)GV_Robot_Move_Speed) *Move_Base_Speed_Count_Meter_Min/10;//机器人车体速度 Robot_Countinus_Speed=((double)GV_Robot_Back_Speed)*Move_Base_Speed_Count_Meter_Min;//机器人边打边退速度 } void Robot_Posture_Adjus_Gravity() { double Sign_Flag=0; double Speed_Factor=0; int Speed_Base=201.7*7.64*1; double Robot_Grity=((double )GV.TL720DParameters.RF_Angle_Roll)/100; Angle_Error_LLL= CV_Robot_Deri_Angle_Deg_Grity-Robot_Grity; if(Angle_Error_LLL>=180) { Angle_Error_LLL=Angle_Error_LLL-360; } else if(Angle_Error_LLL<=-180) { Angle_Error_LLL=Angle_Error_LLL+360; } if(Angle_Error_LLL>=0) { Sign_Flag=1; } else { Sign_Flag=-1; } Angle_Error_LLL=fabs(Angle_Error_LLL); if(Angle_Error_LLL>5) { Speed_Factor=2; } else { Speed_Factor=0.4; } GV.LeftMotor.Target_Velcity=Speed_Factor*Sign_Flag*Speed_Base; GV.RightMotor.Target_Velcity=Speed_Factor*Sign_Flag*Speed_Base; } void Robot_Posture_Adjus_Plane() { double Sign_Flag=0; double Speed_Factor=0; int Speed_Base=201.7*7.64*1; double Robot_Plane=MF40G_Angle_Add_Deg; Angle_Error_LLL= CV_Robot_Deri_Angle_Deg_Plane-Robot_Plane; if(Angle_Error_LLL>=180) { Angle_Error_LLL=Angle_Error_LLL-360; } else if(Angle_Error_LLL<=-180) { Angle_Error_LLL=Angle_Error_LLL+360; } if(Angle_Error_LLL>=0) { Sign_Flag=-1; } else { Sign_Flag=1; } Angle_Error_LLL=fabs(Angle_Error_LLL); if(Angle_Error_LLL>5) { Speed_Factor=2; } else { Speed_Factor=0.4; } GV.LeftMotor.Target_Velcity=-Speed_Factor*Sign_Flag*Speed_Base; GV.RightMotor.Target_Velcity=-Speed_Factor*Sign_Flag*Speed_Base; } void Vertical_Angle_Judge() { double Robot_Grity=((double )GV.TL720DParameters.RF_Angle_Roll)/100; Vertical_Der_Angle_Deg= GV_Robot_Vertical_Adjust; // if((Robot_Grity>-90)&&(Robot_Grity<=90)&&((P_MK32->CH2_LY_V<-300)||P_MK32->CH5_SB==1000)) // { // Vertical_Der_Angle_Deg=-GV_Robot_Vertical_Adjust; // } // else if((Robot_Grity>-90)&&(Robot_Grity<=90)&&((P_MK32->CH2_LY_V>300)||P_MK32->CH5_SB==-1000)) // { // Vertical_Der_Angle_Deg=GV_Robot_Vertical_Adjust; // } // else if ((fabs(Robot_Grity)>90)&&((P_MK32->CH2_LY_V<-300)||P_MK32->CH5_SB==1000)) // { // Vertical_Der_Angle_Deg=180+GV_Robot_Vertical_Adjust; // if(Vertical_Der_Angle_Deg>180) // { // Vertical_Der_Angle_Deg=Vertical_Der_Angle_Deg-360; // } // } // else if((fabs(Robot_Grity)>90)&&((P_MK32->CH2_LY_V>300)||P_MK32->CH5_SB==-1000)) // { // Vertical_Der_Angle_Deg=180-GV_Robot_Vertical_Adjust; // if(Vertical_Der_Angle_Deg>180) // { // Vertical_Der_Angle_Deg=Vertical_Der_Angle_Deg-360; // } // } CV_Robot_Deri_Angle_Deg_Grity=Vertical_Der_Angle_Deg; } void Vertical_Angle_Judge_Uptata_1() { double Robot_Grity=((double )GV.TL720DParameters.RF_Angle_Roll)/100; // if((Robot_Grity>-90)&&(Robot_Grity<=90)) // { // Vertical_Der_Angle_Deg=-GV_Robot_Vertical_Adjust; // } // else if((Robot_Grity>-90)&&(Robot_Grity<=90)) // { // Vertical_Der_Angle_Deg=GV_Robot_Vertical_Adjust; // } Vertical_Der_Angle_Deg=GV_Robot_Vertical_Adjust; CV_Robot_Deri_Angle_Deg_Grity=Vertical_Der_Angle_Deg; } void Change_Road_Down_Up_Right_To_Left() { static int Change_R_L_Count; double Der_Angle_Down_Up[2]={90,0}; switch(CRLU_Flag) { case 0://两轮速度相同转向 CV_Robot_Deri_Angle_Deg_Grity=Der_Angle_Down_Up[0]; Robot_Posture_Adjus_Gravity(); if(Angle_Error_LLLVertical_Change_Road_Time) { CRLU_Flag++; Change_R_L_Count=0; } break; case 3://后退指定距离 CV_Robot_Deri_Angle_Deg_Grity=Der_Angle_Down_Up[1]; Robot_Posture_Adjus_Gravity(); if(Angle_Error_LLLVertical_Change_Road_Time) { CRLU_Flag++; Change_R_L_Count=0; } break; case 3://后退指定距离 CV_Robot_Deri_Angle_Deg_Grity=Der_Angle_Down_Up[1]; Robot_Posture_Adjus_Gravity(); if(Angle_Error_LLL180) { Der_Angle_Plane[0]=Der_Angle_Plane[0]-360; } else if(Der_Angle_Plane[0]<-180) { Der_Angle_Plane[0]=Der_Angle_Plane[0]+360; } Der_Angle_Plane[1]=MF40G_Angle_Add_Deg+180; if(Der_Angle_Plane[1]>180) { Der_Angle_Plane[1]=Der_Angle_Plane[1]-360; } else if(Der_Angle_Plane[1]<-180) { Der_Angle_Plane[1]=Der_Angle_Plane[1]+360; } CRLU_Flag++; break; case Turn_To_Y_C://转向到期望角度 CV_Robot_Deri_Angle_Deg_Plane=Der_Angle_Plane[0]; Robot_Posture_Adjus_Plane(); if(Angle_Error_LLLPlane_Change_Road_Time) { CRLU_Flag++; Change_R_L_Count=0; } break; case Turn_To_X://后退指定距离 CV_Robot_Deri_Angle_Deg_Plane=Der_Angle_Plane[1]; Robot_Posture_Adjus_Plane(); if(Angle_Error_LLL180) { Der_Angle_Plane[0]=Der_Angle_Plane[0]-360; } else if(Der_Angle_Plane[0]<-180) { Der_Angle_Plane[0]=Der_Angle_Plane[0]+360; } Der_Angle_Plane[1]=MF40G_Angle_Add_Deg-180; if(Der_Angle_Plane[1]>180) { Der_Angle_Plane[1]=Der_Angle_Plane[1]-360; } else if(Der_Angle_Plane[1]<-180) { Der_Angle_Plane[1]=Der_Angle_Plane[1]+360; } CRLU_Flag++; break; case Turn_To_Y_C://转向到期望角度 CV_Robot_Deri_Angle_Deg_Plane=Der_Angle_Plane[0]; Robot_Posture_Adjus_Plane(); if(Angle_Error_LLLPlane_Change_Road_Time) { CRLU_Flag++; Change_R_L_Count=0; } break; case Turn_To_X://后退指定距离 CV_Robot_Deri_Angle_Deg_Plane=Der_Angle_Plane[1]; Robot_Posture_Adjus_Plane(); if(Angle_Error_LLL180) { Der_Angle_Plane[0]=Der_Angle_Plane[0]-360; } else if(Der_Angle_Plane[0]<-180) { Der_Angle_Plane[0]=Der_Angle_Plane[0]+360; } Der_Angle_Plane[1]=MF40G_Angle_Add_Deg+180; if(Der_Angle_Plane[1]>180) { Der_Angle_Plane[1]=Der_Angle_Plane[1]-360; } else if(Der_Angle_Plane[1]<-180) { Der_Angle_Plane[1]=Der_Angle_Plane[1]+360; } CRLU_Flag++; break; case Turn_To_Y_C://转向到期望角度 CV_Robot_Deri_Angle_Deg_Plane=Der_Angle_Plane[0]; Robot_Posture_Adjus_Plane(); if(Angle_Error_LLLPlane_Change_Road_Time) { CRLU_Flag++; Change_R_L_Count=0; } break; case Turn_To_X://后退指定距离 CV_Robot_Deri_Angle_Deg_Plane=Der_Angle_Plane[1]; Robot_Posture_Adjus_Plane(); if(Angle_Error_LLLCH3_LY_H<-300) { Turn_Count++; if(Turn_Count>1000) { Robot_Speed[1]=Robot_Speed[1]+Robot_Speed[1]*1; CV_Robot_Deri_Angle_Deg_Plane=MF40G_Angle_Add_Deg; } } else if(P_MK32->CH3_LY_H>300) { Turn_Count++; if(Turn_Count>1000) { Robot_Speed[0]=Robot_Speed[0]+Robot_Speed[0]*1; CV_Robot_Deri_Angle_Deg_Plane=MF40G_Angle_Add_Deg; } } else { Turn_Count=0; } GV.LeftMotor.Target_Velcity =-Robot_Speed[1]; GV.RightMotor.Target_Velcity =Robot_Speed[0]; } void Move_Plane_Task_Backwards_Distance_Do_Update_Turn_Origin(double robotMoveSpeed, double robotDeriAngleDegPlane) { double Deri_Speed_Robot_MAX=robotMoveSpeed*1.5; double Robot_Speed[4]={0,0,0,0}; double Robot_Plane_Angle=MF40G_Angle_Add_Deg; GF_MSP_PID_Now_Der_adj_Com_Horizon(Robot_Plane_Angle, robotDeriAngleDegPlane, robotMoveSpeed, Deri_Speed_Robot_MAX, System_time_SR , Robot_Speed); static int Turn_Count=0; if(P_MK32->CH3_LY_H<-300) { Turn_Count++; if(Turn_Count>1000) { Robot_Speed[1]=Robot_Speed[1]+Robot_Speed[1]*1; CV_Robot_Deri_Angle_Deg_Plane=MF40G_Angle_Add_Deg; if(X_Deri_Angle[0]==Deri_Angle_Deg_X[0]) { Deri_Angle_Deg_X[0]=CV_Robot_Deri_Angle_Deg_Plane; Plane_Turn_Angle_Deg=Deri_Angle_Deg_X[0]+90; Deri_Angle_Deg_X[1]=Deri_Angle_Deg_X[0]+180; X_Deri_Angle[0]=Deri_Angle_Deg_X[0]; if( Deri_Angle_Deg_X[1]>180) { Deri_Angle_Deg_X[1]= Deri_Angle_Deg_X[1]-360; } } else { Deri_Angle_Deg_X[1]=CV_Robot_Deri_Angle_Deg_Plane; Plane_Turn_Angle_Deg=Deri_Angle_Deg_X[1]-90; Deri_Angle_Deg_X[0]=Deri_Angle_Deg_X[1]-180; X_Deri_Angle[0]=Deri_Angle_Deg_X[1]; if( Deri_Angle_Deg_X[1]>180) { Deri_Angle_Deg_X[1]= Deri_Angle_Deg_X[1]-360; } } } } else if(P_MK32->CH3_LY_H>300) { Turn_Count++; if(Turn_Count>1000) { Robot_Speed[0]=Robot_Speed[0]+Robot_Speed[0]*1; CV_Robot_Deri_Angle_Deg_Plane=MF40G_Angle_Add_Deg; if(X_Deri_Angle[0]==Deri_Angle_Deg_X[0]) { Deri_Angle_Deg_X[0]=CV_Robot_Deri_Angle_Deg_Plane; Plane_Turn_Angle_Deg=Deri_Angle_Deg_X[0]+90; Deri_Angle_Deg_X[1]=Deri_Angle_Deg_X[0]+180; X_Deri_Angle[0]=Deri_Angle_Deg_X[0]; if( Deri_Angle_Deg_X[1]>180) { Deri_Angle_Deg_X[1]= Deri_Angle_Deg_X[1]-360; } } else { Deri_Angle_Deg_X[1]=CV_Robot_Deri_Angle_Deg_Plane; Plane_Turn_Angle_Deg=Deri_Angle_Deg_X[1]-90; Deri_Angle_Deg_X[0]=Deri_Angle_Deg_X[1]-180; X_Deri_Angle[0]=Deri_Angle_Deg_X[1]; if( Deri_Angle_Deg_X[0]<-180) { Deri_Angle_Deg_X[0]= Deri_Angle_Deg_X[1]+360; } } } } else { Turn_Count=0; } GV.LeftMotor.Target_Velcity =-Robot_Speed[1]; GV.RightMotor.Target_Velcity =Robot_Speed[0]; } void Move_Plane_Task_Backwards_Swing_Back() { double Deri_Speed_Robot_MAX=((double)GV.Robot_Move_Speed)*1.5; double Robot_Speed[4]={0,0,0,0}; double Robot_Grity=((double )GV.TL720DParameters.RF_Angle_Roll)/100; GF_MSP_PID_Now_Der_adj_Com(Robot_Grity, CV_Robot_Deri_Angle_Deg_Plane, GV.Robot_Move_Speed, Deri_Speed_Robot_MAX, System_time_SR , Robot_Speed); GV.LeftMotor.Target_Velcity =-Robot_Speed[1]; GV.RightMotor.Target_Velcity =Robot_Speed[0]; } int Change_R_L_Count_1; void Change_Road_Left() { double Der_Angle_Left[2]={0,-90}; switch(CRLU_Flag) { case 0://两轮速度相同转向 CV_Robot_Deri_Angle_Deg_Grity=Der_Angle_Left[0]; Robot_Posture_Adjus_Gravity(); if(Angle_Error_LLLHorizontal_Change_Road_Time) { CRLU_Flag++; Change_R_L_Count_1=0; } break; case 3://后退指定距离 CV_Robot_Deri_Angle_Deg_Grity=Der_Angle_Left[1]; Robot_Posture_Adjus_Gravity(); if(Angle_Error_LLLHorizontal_Change_Road_Time) { CRLU_Flag++; Change_R_L_Count=0; } break; case 3://后退指定距离 CV_Robot_Deri_Angle_Deg_Grity=Der_Angle_Right[1]; Robot_Posture_Adjus_Gravity(); if(Angle_Error_LLL20) { GV_Robot_Change_Lane_Distance_Updata=(GV_Robot_Change_Lane_Distance-1.7)/1.046; } else { GV_Robot_Change_Lane_Distance_Updata=GV_Robot_Change_Lane_Distance; } Horizontal_Change_Road_Time=((double)GV_Robot_Change_Lane_Distance_Updata)*0.01/3*60*500;//一米的时候超出12cm } void Vertical_Change_Road_Back_Time_Compute() { Vertical_Change_Road_Time=((double)GV_Robot_Change_Lane_Distance)*0.01/3*60*500;//一米的时候超出12cm } //打一道退一次 void Fight_Alternately_Function_Horizontal() { Swing_Limit_Contrl();//摆臂限位控制 Robot_Platform_Back_Contronl_Horizontal(); } //打退连续 void Fight_Countinus_Function_Horizontal() { Swing_Limit_Contrl();//摆臂限位控制 Move_Horizontal_Vertical_Task_Backwards_Do_Backward(Robot_Countinus_Speed,CV_Robot_Deri_Angle_Deg_Grity); } //打一道退一次 void Fight_Alternately_Function_Vertical() { Swing_Limit_Contrl();//摆臂限位控制 Robot_Platform_Back_Contronl_Vertical(); } void Fight_Alternately_Function_Plane() { Swing_Limit_Contrl();//摆臂限位控制 Robot_Platform_Back_Contronl_Plane(); } void Fight_Countinus_Function_Plane() { Swing_Limit_Contrl();//摆臂限位控制 // Move_Plane_Task_Backwards_Distance_Do_Update(Robot_Countinus_Speed, CV_Robot_Deri_Angle_Deg_Plane); Move_Plane_Task_Backwards_Distance_Do_Update_Turn(Robot_Countinus_Speed, CV_Robot_Deri_Angle_Deg_Plane); } int Swing_Time_Count=0; double Swing_Time_All=0; double Swing_Time_Threshold=0; double Left_Target_Pos;//左侧边界 double Right_Target_Pos;//右侧边界 double Left_Threshold_Pos=0; double Right_Threshold_Pos=0; double Auto_Job_Back_Time_MS_Horizontal=0; double Auto_Job_Back_Time_MS_Vertical=0; double Horizontal_Back_Speed_Value=0; double Vertical_Back_Speed_Value=0; double Plane_Back_Speed_Value=0; void Swing_Limit_Contrl() { //Case 1 向左摆臂 switch(Swing_Limit_Flag) { case 0: Back_Para_Compute(); if(GV.SwingMotor.Real_Position>=Left_Target_Pos-25000) { Swing_Limit_Flag=2; } else if(GV.SwingMotor.Real_Position<=Right_Target_Pos+25000) { Swing_Limit_Flag=1; } else { Swing_Limit_Flag++; } break; case 1://已经到了右边，现在向左运动到限位； Swing_Time_Count++; GV.SwingMotor.Position_immediately1_Lag2=1; GV.SwingMotor.Tar_Position_count=Left_Target_Pos; GV.SwingMotor.Tar_Position_Velcity_RPM=((double)GV_Swing_Speed)*201.7; if(GV.SwingMotor.Real_Position>Left_Target_Pos-25000) { Swing_Limit_Flag=3; Swing_Time_Count=0; } break; case 2://已经到了左右边，现在向右左运动到限位； Swing_Time_Count++; GV.SwingMotor.Position_immediately1_Lag2=1; GV.SwingMotor.Tar_Position_count=Right_Target_Pos; GV.SwingMotor.Tar_Position_Velcity_RPM=((double)GV_Swing_Speed)*201.7; if(GV.SwingMotor.Real_PositionLeft_Target_Pos-20000)||(Swing_Time_Count>Swing_Time_All)) { Swing_Time_Count=0; Swing_Limit_Flag++; GV.SwingMotor.Position_immediately1_Lag2=2; GV.SwingMotor.Tar_Position_count=Right_Target_Pos; GV.SwingMotor.Tar_Position_Velcity_RPM=((double)GV_Swing_Speed)*201.7; } break; case 4: Swing_Time_Count++; if((GV.SwingMotor.Real_PositionSwing_Time_All)) { Swing_Time_Count=0; Swing_Limit_Flag=3; GV.SwingMotor.Position_immediately1_Lag2=2; GV.SwingMotor.Tar_Position_count=Left_Target_Pos; GV.SwingMotor.Tar_Position_Velcity_RPM=((double)GV_Swing_Speed)*201.7; } break; } } double Swi_Range_Auto=0; void Back_Para_Compute() { double Sw_Rust_Mid_Posi=-6753;//需检测修改 double Swi_Speed_Auto=((double)CV.PV.Robot_Swing_Speed)*201.7; double Back_Angle_Deg=5; double Turn_Angle_Back=TT_One_Deg_Count*Back_Angle_Deg; //总后退3S，1.5S在左边，1.5S在右边 // Left_Target_Pos=Sw_Rust_Mid_Posi+Swi_Range_Auto;//左侧边界 // Right_Target_Pos=Sw_Rust_Mid_Posi-Swi_Range_Auto;//右侧边界 Left_Threshold_Pos=Left_Target_Pos-Turn_Angle_Back;//正向5度阈值 Right_Threshold_Pos=Right_Target_Pos+Turn_Angle_Back;//逆向5度阈值 Swing_Time_All=((double)Swi_Range_Auto)/((double)GV_Swing_Speed)*500+500;//全程摆臂时间计算 Swing_Time_Threshold=((double)Swi_Range_Auto-5)/((double)GV_Swing_Speed)*500+500;//阈值摆臂时间计算 } void Robot_Platform_Back_Contronl_Horizontal() { //计算后退时间及后退速度 switch(Robot_Platform_Back_Flag) { case 0: Platform_Back_Para_Compute_Horizontal(); Auto_Job_Back_Time_MS_Count=0; Robot_Platform_Back_Flag++; break; case 1: Auto_Job_Back_Time_MS_Count++; if(GV.SwingMotor.Real_Position>Left_Threshold_Pos) { Robot_Platform_Back_Flag++; Auto_Job_Back_Time_MS_Count=0; } GV.LeftMotor.Target_Velcity =0; GV.RightMotor.Target_Velcity =0; break; case 2: Auto_Job_Back_Time_MS_Count++; if((GV.SwingMotor.Real_Position>Left_Threshold_Pos)||(GV.SwingMotor.Real_PositionSwing_Time_All) { GV.LeftMotor.Target_Velcity =0; GV.RightMotor.Target_Velcity =0; GV.SwingMotor.Target_Velcity =0; } GV.LeftMotor.Target_Velcity =0; GV.RightMotor.Target_Velcity =0; break; case 3: Move_Horizontal_Vertical_Task_Backwards_Do_Backward(Horizontal_Back_Speed_Value,CV_Robot_Deri_Angle_Deg_Grity); Auto_Job_Back_Time_MS_Count++; if(Auto_Job_Back_Time_MS_Count>Auto_Job_Back_Time_MS_Horizontal) { Robot_Platform_Back_Flag++; Auto_Job_Back_Time_MS_Count=0; } break; case 4: if((GV.SwingMotor.Real_PositionRight_Threshold_Pos)) { Backward_Num_X_Count++; Robot_Platform_Back_Flag=2; Auto_Job_Back_Time_MS_Count=0; } GV.LeftMotor.Target_Velcity =0; GV.RightMotor.Target_Velcity =0; break; } } void Robot_Platform_Back_Contronl_Vertical() { //计算后退时间及后退速度 switch(Robot_Platform_Back_Flag) { case 0: Platform_Back_Para_Compute_Vertical(); Auto_Job_Back_Time_MS_Count=0; Robot_Platform_Back_Flag++; break; case 1: Auto_Job_Back_Time_MS_Count++; if(GV.SwingMotor.Real_Position>Left_Threshold_Pos) { Robot_Platform_Back_Flag++; Auto_Job_Back_Time_MS_Count=0; } GV.LeftMotor.Target_Velcity =0; GV.RightMotor.Target_Velcity =0; break; case 2: Auto_Job_Back_Time_MS_Count++; if((GV.SwingMotor.Real_Position>Left_Threshold_Pos)||(GV.SwingMotor.Real_PositionAuto_Job_Back_Time_MS_Vertical) { Robot_Platform_Back_Flag++; Auto_Job_Back_Time_MS_Count=0; } break; case 4: if((GV.SwingMotor.Real_PositionRight_Threshold_Pos)) { Backward_Num_X_Count++; Robot_Platform_Back_Flag=2; Auto_Job_Back_Time_MS_Count=0; } GV.LeftMotor.Target_Velcity =0; GV.RightMotor.Target_Velcity =0; break; } } void Robot_Plane_Auto_Job_Region_Change_Road() { //计算后退时间及后退速度 Platform_Back_Para_Compute(); switch(Robot_Platform_Back_Flag_Y) { case 0://转向到期望角度 CV_Robot_Deri_Angle_Deg_Plane=Plane_Turn_Angle_Deg; Robot_Posture_Adjus_Plane(); if(Angle_Error_LLLLeft_Threshold_Pos) { Robot_Platform_Back_Flag++; Auto_Job_Back_Time_MS_Count=0; } GV.LeftMotor.Target_Velcity =0; GV.RightMotor.Target_Velcity =0; break; case 2: Auto_Job_Back_Time_MS_Count++; if((GV.SwingMotor.Real_Position>Left_Threshold_Pos)||(GV.SwingMotor.Real_PositionAuto_Job_Back_Time_MS_Horizontal) { Robot_Platform_Back_Flag++; Auto_Job_Back_Time_MS_Count=0; } break; case 4: if((GV.SwingMotor.Real_PositionRight_Threshold_Pos)) { Backward_Num_X_Count++; Robot_Platform_Back_Flag=2; Auto_Job_Back_Time_MS_Count=0; } GV.LeftMotor.Target_Velcity =0; GV.RightMotor.Target_Velcity =0; break; } } void Platform_Back_Para_Compute_Horizontal() { Auto_Job_Back_Time_MS_Horizontal=(5.0/((double)GV_Swing_Speed)*1000+250)*1.025;//5的含义为，摆臂期望角度为90，在85度的时候就要开始后退。 double Speed_Base=201.7*7.64;//其中201.7为电机1m/°的速度值//201.7*7.64（）为天太电机1m/min的数据，以该数据为基础进行1/2/3/4/5/6/7/8/9/14m/min double Deg_Sencend_Compute=((double)GV_Robot_Back_Distance)/100/(Auto_Job_Back_Time_MS_Horizontal/1000)*60; Horizontal_Back_Speed_Value=Deg_Sencend_Compute*Speed_Base/2; } void Platform_Back_Para_Compute_Vertical() { Auto_Job_Back_Time_MS_Vertical=(5.0/((double)GV_Swing_Speed)*1000+100)*1.025;//5的含义为，摆臂期望角度为90，在85度的时候就要开始后退。 double Speed_Base=201.7*7.64;//其中201.7为电机1m/°的速度值//201.7*7.64（）为天太电机1m/min的数据，以该数据为基础进行1/2/3/4/5/6/7/8/9/14m/min double Deg_Sencend_Compute=((double)GV_Robot_Back_Distance)/100/(Auto_Job_Back_Time_MS_Vertical/1000)*60; Vertical_Back_Speed_Value=Deg_Sencend_Compute*Speed_Base/2; } void Platform_Back_Para_Compute_Plane() { Auto_Job_Back_Time_MS_Horizontal=5.0/((double)GV_Swing_Speed)*1000;//5的含义为，摆臂期望角度为90，在85度的时候就要开始后退。 double Speed_Base=201.7*7.64;//其中201.7为电机1m/°的速度值//201.7*7.64（）为天太电机1m/min的数据，以该数据为基础进行1/2/3/4/5/6/7/8/9/14m/min double Deg_Sencend_Compute=((double)GV_Robot_Back_Distance)/100/(Auto_Job_Back_Time_MS_Horizontal/1000)*60; //计算单位1m/min=1000/60=16.67mm/s // double Deg_Sencend_Compute=MM_Senct/16.67; Plane_Back_Speed_Value=Deg_Sencend_Compute*Speed_Base/2; } void Platform_Back_Change_Road_Para_Compute() { Auto_Job_Back_Time_MS_Horizontal=5.0/((double)GV_Swing_Speed)*1000+250;//5的含义为，摆臂期望角度为90，在85度的时候就要开始后退。 double Speed_Base=201.7*7.64;//其中201.7为电机1m/°的速度值//201.7*7.64（）为天太电机1m/min的数据，以该数据为基础进行1/2/3/4/5/6/7/8/9/14m/min double Deg_Sencend_Compute=((double)GV_Robot_Back_Distance)/100/(Auto_Job_Back_Time_MS_Horizontal/1000)*60; //计算单位1m/min=1000/60=16.67mm/s // double Deg_Sencend_Compute=MM_Senct/16.67; Horizontal_Back_Speed_Value=Deg_Sencend_Compute*Speed_Base/2; } void IV_control() { IV.Robot_Gyro = Robot_Angle_G_P_Deg; // IV.RunMode = IV_MCU_Run_State; IV.LeftCompensation = GV.Left_Compensation; IV.RightCompensation = GV.Right_Compensation; // IV.CurrentAngle = GV.Robot_Angle; if(GV.LeftMotor.Target_Velcity>0) { IV.Robot_Move_Deri_Speed = (int)(((double)GV.LeftMotor.Target_Velcity)/Move_Base_Speed_Count_Meter_Min*10)+1; } else if(GV.LeftMotor.Target_Velcity<0) { IV.Robot_Move_Deri_Speed = (int)(((double)GV.LeftMotor.Target_Velcity)/Move_Base_Speed_Count_Meter_Min*10)-1; } else { IV.Robot_Move_Deri_Speed =0; } //IV.Robot_Move_Deri_Speed = (int)(((double)GV.LeftMotor.Target_Velcity)/Move_Base_Speed_Count_Meter_Min*10)+1; IV.SystemError= GV.SystemErrorData.ErrorCode; IV.Left_Motor_Err= GV.LeftMotor.TT_Motor_Fault; IV.Right_Motor_Err= GV.RightMotor.TT_Motor_Fault; IV.Swing_Motor_Err= GV.SwingMotor.TT_Motor_Fault; if((GV.Robot_To_Wall_mm.KS206_1_Measuring_Distance==20)&&(GV.Robot_To_Wall_mm.KS206_2_Measuring_Distance==20)) { IV.Distance_Sensor=200; } else if((GV.Robot_To_Wall_mm.KS206_1_Measuring_Distance!=20)&&(GV.Robot_To_Wall_mm.KS206_2_Measuring_Distance!=20)) { IV.Distance_Sensor=(GV.Robot_To_Wall_mm.KS206_1_Measuring_Distance+GV.Robot_To_Wall_mm.KS206_2_Measuring_Distance)/2; } else if(GV.Robot_To_Wall_mm.KS206_1_Measuring_Distance==20) { IV.Distance_Sensor=(200+GV.Robot_To_Wall_mm.KS206_2_Measuring_Distance)/2; } else if(GV.Robot_To_Wall_mm.KS206_2_Measuring_Distance==20) { IV.Distance_Sensor=(200+GV.Robot_To_Wall_mm.KS206_1_Measuring_Distance)/2; } else { IV.Distance_Sensor=0; } IV.Is_Online=P_MK32->IsOnline; } void IV_control_1() { IV.Robot_Gyro = Robot_Angle_G_P_Deg; // IV.RunMode = IV_MCU_Run_State; IV.LeftCompensation = GV.Left_Compensation; IV.RightCompensation = GV.Right_Compensation; // IV.CurrentAngle = GV.Robot_Angle; if(GV.LeftMotor.Target_Velcity>0) { IV.Robot_Move_Deri_Speed = (int)(((double)GV.LeftMotor.Target_Velcity)/Move_Base_Speed_Count_Meter_Min*10)+1; } else if(GV.LeftMotor.Target_Velcity<0) { IV.Robot_Move_Deri_Speed = (int)(((double)GV.LeftMotor.Target_Velcity)/Move_Base_Speed_Count_Meter_Min*10)-1; } else { IV.Robot_Move_Deri_Speed =0; } //IV.Robot_Move_Deri_Speed = (int)(((double)GV.LeftMotor.Target_Velcity)/Move_Base_Speed_Count_Meter_Min*10)+1; IV.SystemError= GV.SystemErrorData.ErrorCode; IV.Left_Motor_Err= GV.LeftMotor.TT_Motor_Fault; IV.Right_Motor_Err= GV.RightMotor.TT_Motor_Fault; IV.Swing_Motor_Err= GV.SwingMotor.TT_Motor_Fault; IV.Distance_Sensor=-GV.Force_Value_mN.CMCU_Measuring_value*10; IV.Is_Online=P_MK32->IsOnline; } void PV_Data_Reading() { // int32 Robot_Operation_Mode= 1; //操作模式 手动模式1 2 3 4 5 自动模式6 7 // int32 Robot_Move_Speed=2; //机器人移动速度 // int32 Robot_Change_Lane_Distance= 3; //换道距离 // int32 Robot_Swing_Speed= 4; //摆臂速度 // int32 Robot_symmetricalOrNot=5; //摆臂角度界面中的对称or非对称 1 对称 2 非对称 // int32 Robot_Swing_Range_Angle=6; //对称下的摆臂角度 // int32 Robot_asymmetricalAngleSetValue=7; //非对称条件下下 1左侧 2 右侧 // int32 Robot_backMode=8; //后退设置模式 1 打退交替 2 边打边退 // int32 Robot_Back_Distance= 9; //打退交替条件下的后退距离 // int32 Robot_Back_Speed=10; //边打边退条件下的后退速度 // int32 Robot_Press_Set= 11; //压力设置 // int32 Robot_Vertical_Adjust= 12; //竖直微调 // int32 Robot_Length_Homework=13; //自动模式下的作业长度 // int32 Robot_Width_Homework=14; //自动模式下的作业宽度 // GV_Swing_Range_Angle=(int)CV.PV.Robot_Operation_Mode; // 1 GV_Robot_Move_Speed=(int)CV.PV.Robot_Move_Speed; // 2 GV_Robot_Change_Lane_Distance=(double)CV.PV.Robot_Change_Lane_Distance; // 3 GV_Swing_Speed=(int)CV.PV.Robot_Swing_Speed; // 4 if(GV_Swing_Speed<=2) { GV_Swing_Speed=2; } GV_Robot_symmetricalOrNot=(int)CV.PV.Robot_symmetricalOrNot; // 5 GV_Robot_Swing_Range_Angle=(int)CV.PV.Robot_Swing_Range_Angle; // 6 GV_Robot_asymmetricalAngleSetValue=(int)CV.PV.Robot_asymmetricalAngleSetValue; // 7 GV_Robot_backMode=(int)CV.PV.Robot_backMode; // 8 GV_Robot_Back_Distance=((double)CV.PV.Robot_Back_Distance)/10; // 9 GV_Robot_Back_Speed=((double)CV.PV.Robot_Back_Speed)/10; // 10 GV_Robot_Press_Set=(int)CV.PV.Robot_Press_Set; // 11 GV_Robot_Vertical_Adjust=((double)CV.PV.Robot_Vertical_Adjust)/10; // 12 GV_Robot_Length_Homework=(int)CV.PV.Robot_Length_Homework; // 13 GV_Robot_Width_Homework=(int)CV.PV.Robot_Width_Homework; // 14 GV.blast_open_or_close=CV.PV.blast_status; GV.blower_open_or_close=CV.PV.blower_status; } void Robot_Main_Mode_Jude() { const double DEADZONE_THRESHOLD = 200.0; double y_value = P_MK32->CH2_LY_V; // 假设 Y 是纵向（前后） double x_value = P_MK32->CH3_LY_H; // 假设 X 是横向（左右） double w_value = P_MK32->CH0_RY_H; // 假设 X 是横向（左右） double z_value = P_MK32->CH1_RY_V; // 假设 X 是横向（左右） uint8_t all_in_deadzone = (fabs(y_value) <= DEADZONE_THRESHOLD) & (fabs(x_value) <= DEADZONE_THRESHOLD) & (fabs(w_value) <= DEADZONE_THRESHOLD) & (fabs(z_value) <= DEADZONE_THRESHOLD); // 根据判断结果更新标志 if (all_in_deadzone) { Robot_Main_Flag++; } } void EmergencyStop_Hardware_Communication_Detection() { // (Get_BIT(SystemErrorCode, ComError_MK32_Serial) == 1) || if((Get_BIT(SystemErrorCode, ComError_TL720D) == 1) ||(Get_BIT(SystemErrorCode, ComError_ZQ_CAN_ID3_SwingMotor) == 1) ||(Get_BIT(SystemErrorCode, ComError_Force_Sensor) == 1) ||(Get_BIT(SystemErrorCode, ComError_Mfog40) == 1) ||(Get_BIT(SystemErrorCode, ComError_Ultrasonic_Sensor) == 1) ) { // CV.PV.Robot_Operation_Mode=1; // GV_Operation_Mode=1; } if ((Get_BIT(SystemErrorCode, ComError_Mk32_SBus) == 1) ||(Get_BIT(SystemErrorCode, ComError_ZQ_CAN_ID1_LeftMotor) == 1) ||(Get_BIT(SystemErrorCode, ComError_ZQ_CAN_ID2_RightMotor) == 1)) { CV.PV.Robot_Operation_Mode=1; GV_Operation_Mode=1; Robot_Main_Flag=0; } if((GV.LeftMotor.TT_Motor_Fault==0x0302)||(GV.RightMotor.TT_Motor_Fault==0x0302)) { CV.PV.Robot_Operation_Mode=1; GV_Operation_Mode=1; } static int Initial_Power_Record; if(Initial_Power_Record==0) { if (P_MK32->RxIndex > 0 && P_MK32->CH4_SA == 0 && P_MK32->CH5_SB == 0 && P_MK32->CH6_SC == 0 && P_MK32->CH7_SD == 0) { Initial_Power_Record=1; SET_BIT_0(SystemErrorCode, ComError_MK32_InitialState); } else { SET_BIT_1(SystemErrorCode, ComError_MK32_InitialState); Robot_Main_Flag=0; } } if(P_MK32->IsOnline!=1) { Robot_Halt_Mode(); CV.PV.Robot_Operation_Mode=0; GV_Operation_Mode=0; } if((P_MK32->CH8_SE==-1000)&&(P_MK32->CH9_SF==-1000)) { Switch_Off_Flag=1; Robot_Halt_Mode(); TT_Motor_Need_To_Activate = 0; TT_Motor_Need_To_Activate_1=0; GF_BSP_GPIO_SetIO(Motor_Power_IO_CTL, K_OFF); // Move_PushRod_Halt_Func_Do(); // Blast_Machine_Close(); CV.PV.Robot_Operation_Mode=1; GV_Operation_Mode=1; Robot_Main_Flag=0; } else { GF_BSP_GPIO_SetIO(Motor_Power_IO_CTL, K_ON); if (Switch_Off_Flag == 1) { Switch_Off_Flag = 0; TT_Motor_Need_To_Activate = 1; TT_Motor_Need_To_Activate_1=1; Initial_Power_Record=0; } } if((IV.SystemError!=0)||(IV.Left_Motor_Err!=0)||(IV.Right_Motor_Err!=0)||(IV.Swing_Motor_Err!=0)) { Blast_Machine_Close_Fun(); } } void EmergencyStop_Hardware_Communication_Detection_1() { if ((Get_BIT(SystemErrorCode, ComError_Mk32_SBus) == 1) ||(Get_BIT(SystemErrorCode, ComError_ZQ_CAN_ID1_LeftMotor) == 1) ||(Get_BIT(SystemErrorCode, ComError_ZQ_CAN_ID2_RightMotor) == 1)) { CV.PV.Robot_Operation_Mode=1; GV_Operation_Mode=1; Robot_Main_Flag=0; } if((GV.LeftMotor.TT_Motor_Fault==0x0302)||(GV.RightMotor.TT_Motor_Fault==0x0302)) { CV.PV.Robot_Operation_Mode=1; GV_Operation_Mode=1; } static int Initial_Power_Record; if(Initial_Power_Record==0) { if (P_MK32->RxIndex > 0 && P_MK32->CH4_SA == 0 && P_MK32->CH5_SB == 0 && P_MK32->CH6_SC == 0 && P_MK32->CH7_SD == 0) { Initial_Power_Record=1; SET_BIT_0(SystemErrorCode, ComError_MK32_InitialState); } else { SET_BIT_1(SystemErrorCode, ComError_MK32_InitialState); Robot_Main_Flag=0; } } if(P_MK32->IsOnline!=1) { Robot_Halt_Mode(); CV.PV.Robot_Operation_Mode=0; GV_Operation_Mode=0; } if((P_MK32->CH8_SE==-1000)&&(P_MK32->CH9_SF==-1000)) { Switch_Off_Flag=1; Robot_Halt_Mode(); TT_Motor_Need_To_Activate = 0; TT_Motor_Need_To_Activate_1=0; GF_BSP_GPIO_SetIO(Motor_Power_IO_CTL, K_OFF); // Move_PushRod_Halt_Func_Do(); Blast_Machine_Close(); CV.PV.Robot_Operation_Mode=1; GV_Operation_Mode=1; Robot_Main_Flag=0; } else { GF_BSP_GPIO_SetIO(Motor_Power_IO_CTL, K_ON); if (Switch_Off_Flag == 1) { Switch_Off_Flag = 0; TT_Motor_Need_To_Activate = 1; TT_Motor_Need_To_Activate_1=1; Initial_Power_Record=0; } } } int Regional_Automatic_Flag=0; void Regional_Horizontal_Automatic_Functionc() { if(P_MK32->CH7_SD==-1000)//自动行驶 { if(GV.SystemErrorData.ErrorCode == 0 || GV.SystemErrorData.ErrorCode == 2) { if(ctl_flag_1 != 1) { Blast_Machine_Open_Fun();//打开喷砂机hjb } } switch (Regional_Automatic_Flag) { case Regional_Parameter_Determination://区域自动作业参数确认 Regional_Automatic_Flag++; break; case Regional_Parameter_Calculation: if(GV_Robot_backMode==1) { Regional_Automatic_Flag=Regional_Alternately; } else if(GV_Robot_backMode==2) { Regional_Automatic_Flag=Regional_Continuous; } else { } break; case Regional_Alternately: Region_Automated_Task_Func_Alternately_Horizontal(); break; case Regional_Continuous: Region_Automated_Task_Func_Continuous_Horizontal(); break; } UltraStopReverse(Regional_Automatic_Flag); } else { Regional_Automatic_Flag=Regional_Parameter_Determination; Region_Task_Falg=Parameter_Calculation; ctl_flag = 1; ctl_flag_1 = 0; Robot_Manual_Operation_Function(); Robot_Main_Mode_Jude(); UltraStopReverse_Manually_Backward(); } } void Regional_Plane_Automatic_Functionc() { if(P_MK32->CH7_SD==-1000)//自动行驶 { if(GV.SystemErrorData.ErrorCode == 0 || GV.SystemErrorData.ErrorCode == 2) { if(ctl_flag_1 != 1) { Blast_Machine_Open_Fun();//打开喷砂机hjb } } switch(Regional_Automatic_Flag) { case Regional_Parameter_Determination://区域自动作业参数确认 Regional_Automatic_Flag++; break; case Regional_Parameter_Calculation: if(GV_Robot_backMode==1) { Regional_Automatic_Flag=Regional_Alternately; } else if(GV_Robot_backMode==2) { Regional_Automatic_Flag=Regional_Continuous; } else { } break; case Regional_Alternately: Region_Automated_Task_Func_Alternately_Plane(); break; case Regional_Continuous: // Region_Automated_Task_Func_Continuous_Plane(); Region_Automated_Task_Func_Continuous_Plane_Uptate_1(); break; } UltraStopReverse(Regional_Automatic_Flag); } else { Regional_Automatic_Flag=0; Region_Task_Falg=0; ctl_flag = 1; ctl_flag_1 = 0; Robot_Manual_Operation_Function(); Robot_Main_Mode_Jude(); // UltraStopReverse_Manually_Backward(); } } void Region_Automated_Task_Func_Continuous_Plane() { static int Backward_Time_X_Total; static int Backward_Time_Count; static int Lane_Change_Time_Total_Y; switch (Region_Task_Falg) { case Parameter_Calculation: Deri_Angle_Deg_X[0]=MF40G_Angle_Add_Deg; Plane_Turn_Angle_Deg=MF40G_Angle_Add_Deg+90; Deri_Angle_Deg_X[1]=Deri_Angle_Deg_X[0]+180; Plane_X_Backward_Time_Calculation_Continuous(&Backward_Time_X_Total); Plane_Change_Road_Backward_Time_Calculation_Continuous(&Lane_Change_Time_Total_Y); Plane_Y_Lane_Change_Time_Calculation(&Plane_Complete_Total_XY); Backward_Count_X=0; X_Deri_Angle[0]=Deri_Angle_Deg_X[0]; Swing_Limit_Flag=0; Backward_Num_X_Count=0; Plane_Complete_Count=0; Robot_Platform_Back_Flag=0; Auto_Job_Back_Time_MS_Count=0; Region_Task_Falg++; GV.Robot_Move_Speed=CV.PV.Robot_Back_Speed; Backward_Time_Count=0; break; case Backward_Task_X: CV_Robot_Deri_Angle_Deg_Plane=X_Deri_Angle[0]; Swing_Limit_Contrl();//摆臂限位控制 Move_Plane_Task_Backwards_Distance_Do_Update(Robot_Countinus_Speed, CV_Robot_Deri_Angle_Deg_Plane); Backward_Time_Count++; if(Backward_Time_Count>=Backward_Time_X_Total) { Backward_Time_Count=0; Region_Task_Falg++; Plane_Complete_Count++; int i=0; i=Plane_Complete_Count%2; if(i==0) { X_Deri_Angle[0]=Deri_Angle_Deg_X[0]; X_Deri_Angle[1]=Deri_Angle_Deg_X[1]; } else { X_Deri_Angle[0]=Deri_Angle_Deg_X[1]; X_Deri_Angle[1]=Deri_Angle_Deg_X[0]; } if(Plane_Complete_Count>=Plane_Complete_Total_XY) { Region_Task_Falg=Region_Auto_Close; } } break; case Turn_To_Y: Swing_Limit_Contrl();//摆臂限位控制 CV_Robot_Deri_Angle_Deg_Plane=Plane_Turn_Angle_Deg; Robot_Posture_Adjus_Plane(); if(Angle_Error_LLL=Lane_Change_Time_Total_Y) { Region_Task_Falg++; Backward_Time_Count=0; } break; case Turn_Back_X: CV_Robot_Deri_Angle_Deg_Plane=X_Deri_Angle[0]; Swing_Limit_Contrl(); Robot_Posture_Adjus_Plane(); if(Angle_Error_LLL180) { Deri_Angle_Deg_X[1]= Deri_Angle_Deg_X[1]-360; } Plane_X_Backward_Time_Calculation_Continuous(&Backward_Time_X_Total); Plane_Change_Road_Backward_Time_Calculation_Continuous(&Lane_Change_Time_Total_Y); Plane_Y_Lane_Change_Time_Calculation(&Plane_Complete_Total_XY); Backward_Count_X=0; X_Deri_Angle[0]=Deri_Angle_Deg_X[0]; Swing_Limit_Flag=0; Backward_Num_X_Count=0; Plane_Complete_Count=0; Robot_Platform_Back_Flag=0; Auto_Job_Back_Time_MS_Count=0; Region_Task_Falg++; GV.Robot_Move_Speed=CV.PV.Robot_Back_Speed; Backward_Time_Count=0; break; case Backward_Task_X: CV_Robot_Deri_Angle_Deg_Plane=X_Deri_Angle[0]; Swing_Limit_Contrl();//摆臂限位控制 Move_Plane_Task_Backwards_Distance_Do_Update_Turn_Origin(Robot_Countinus_Speed, CV_Robot_Deri_Angle_Deg_Plane); Backward_Time_Count++; if(Backward_Time_Count>=Backward_Time_X_Total) { Backward_Time_Count=0; Region_Task_Falg++; Plane_Complete_Count++; int i=0; i=Plane_Complete_Count%2; if(i==0) { X_Deri_Angle[0]=Deri_Angle_Deg_X[0]; X_Deri_Angle[1]=Deri_Angle_Deg_X[1]; } else { X_Deri_Angle[0]=Deri_Angle_Deg_X[1]; X_Deri_Angle[1]=Deri_Angle_Deg_X[0]; } if(Plane_Complete_Count>=Plane_Complete_Total_XY) { ctl_flag_1 = 1; Region_Task_Falg=Region_Auto_Close; } } break; case Turn_To_Y: Swing_Limit_Contrl();//摆臂限位控制 CV_Robot_Deri_Angle_Deg_Plane=Plane_Turn_Angle_Deg; Robot_Posture_Adjus_Plane(); if(Angle_Error_LLL=Lane_Change_Time_Total_Y) { Region_Task_Falg++; Backward_Time_Count=0; } break; case Turn_Back_X: CV_Robot_Deri_Angle_Deg_Plane=X_Deri_Angle[0]; Swing_Limit_Contrl(); Robot_Posture_Adjus_Plane(); if(Angle_Error_LLL=Backward_Time_X_Total) { Backward_Time_Count=0; Region_Task_Falg++; Plane_Complete_Count++; int i=0; i=Plane_Complete_Count%2; if(i==0) { X_Deri_Angle[0]=Deri_Angle_Deg_X[0]; X_Deri_Angle[1]=Deri_Angle_Deg_X[1]; } else { X_Deri_Angle[0]=Deri_Angle_Deg_X[1]; X_Deri_Angle[1]=Deri_Angle_Deg_X[0]; } if(Plane_Complete_Count>=Horizontal_Complete_Total_XY) { ctl_flag_1 = 1; Region_Task_Falg=RE_Horizontal_Region_Auto_Close; } } Robot_Platform_Back_Flag_Y=0; break; case RE_Horizontal_Turn_To_Y: Swing_Limit_Contrl();//摆臂限位控制 CV_Robot_Deri_Angle_Deg_Grity=Horizontal_Turn_Angle_Deg; Robot_Posture_Adjus_Gravity(); if(Angle_Error_LLL=Lane_Change_Time_Total_Y) { Backward_Time_Count=0; Region_Task_Falg++; } break; case RE_Horizontal_Turn_Back_X: CV_Robot_Deri_Angle_Deg_Grity=X_Deri_Angle[0]; Swing_Limit_Contrl(); Robot_Posture_Adjus_Gravity(); if(Angle_Error_LLL-90)&&(Robot_Grity<=90)&&((P_MK32->CH2_LY_V<-300)||P_MK32->CH5_SB==1000)) { Vertical_Der_Angle_Deg=-GV_Robot_Vertical_Adjust; } else if((Robot_Grity>-90)&&(Robot_Grity<=90)&&((P_MK32->CH2_LY_V>300)||P_MK32->CH5_SB==-1000)) { Vertical_Der_Angle_Deg=GV_Robot_Vertical_Adjust; } else if ((fabs(Robot_Grity)>90)&&((P_MK32->CH2_LY_V<-300)||P_MK32->CH5_SB==1000)) { Vertical_Der_Angle_Deg=180+GV_Robot_Vertical_Adjust; } else if((fabs(Robot_Grity)>90)&&((P_MK32->CH2_LY_V>300)||P_MK32->CH5_SB==-1000)) { Vertical_Der_Angle_Deg=180-GV_Robot_Vertical_Adjust; } CV_Robot_Deri_Angle_Deg_Grity=Vertical_Der_Angle_Deg; } void Swing_Mode_Determination() { static double Left_Position; static double Right_Position; static int S1_Last_Value; static int S2_Last_Value; static int GV_Robot_Swing_Range_Angle_Last; static double Sw_Rust_Mid_Posi; switch(GV_Robot_symmetricalOrNot) { case 1://对称式已经写完啦 if(GV_Robot_Swing_Range_Angle_Last!=GV_Robot_Swing_Range_Angle) { Sw_Rust_Mid_Posi=GV.SwingMotor.Real_Position;//需检测修改 } double Swi_Range=((double)GV_Robot_Swing_Range_Angle)*TT_One_Deg_Count/2; Left_Target_Pos=Sw_Rust_Mid_Posi+Swi_Range;//左侧边界 Right_Target_Pos=Sw_Rust_Mid_Posi-Swi_Range;//右侧边界 Swi_Range_Auto=GV_Robot_Swing_Range_Angle; GV_Robot_Swing_Range_Angle_Last=GV_Robot_Swing_Range_Angle; break; case 2: // Left_Target_Pos=GV.SwingMotor.Real_Position; // Right_Target_Pos=GV.SwingMotor.Real_Position; if(P_MK32->CH12_S1!=S1_Last_Value) { Left_Position=GV.SwingMotor.Real_Position; } else if(P_MK32->CH13_S2!=S2_Last_Value) { Right_Position=GV.SwingMotor.Real_Position; } else { Left_Target_Pos=Left_Position; Right_Target_Pos=Right_Position; double Angle_Difference=(fabs(Left_Position-Right_Position))/TT_One_Deg_Count; if(Angle_Difference>10) { } else { } Swi_Range_Auto=(Left_Target_Pos-Right_Target_Pos)/TT_One_Deg_Count/2; } S1_Last_Value=P_MK32->CH12_S1; S2_Last_Value=P_MK32->CH13_S2; break; } } void Back_Para_Compute_Updata() { double Back_Angle_Deg=5; double Turn_Angle_Back=TT_One_Deg_Count*Back_Angle_Deg; //总后退3S，1.5S在左边，1.5S在右边 Left_Threshold_Pos=Left_Target_Pos-Turn_Angle_Back;//正向5度阈值 Right_Threshold_Pos=Right_Target_Pos+Turn_Angle_Back;//逆向5度阈值 Swing_Time_All=((double)GV_Robot_Swing_Range_Angle)/((double)GV_Swing_Speed)*500+500;//全程摆臂时间计算 Swing_Time_Threshold=((double)GV_Robot_Swing_Range_Angle-5)/((double)GV_Swing_Speed)*500+500;//阈值摆臂时间计算 } double Desired_Presss=200; int PushRod_Flag=0; double D_value=0; double Press_Dis_Value1=200; void Pressure_Adaptive_Function(double Press_Dis_Value) { // double Desired_Presss=(double)CV.PV.Robot_Press_Set; // Desired_Presss=-500; //向下压为负值，向上运动为正值 double Actual_Value=GV.Force_Value_mN.CMCU_Measuring_value; D_value=Actual_Value-Desired_Presss; // -800--500 switch(PushRod_Flag) { case 0: if(D_value>Press_Dis_Value1) { PushRod_Flag=2;//压力过小//向下调整 } else if (D_value<-Press_Dis_Value1) { PushRod_Flag=1;//压力过大//`向上调整 } Current_PushRod_STATE = PushRod_HALT; break; case 1: Current_PushRod_STATE = PushRod_UP; if(D_value>0) { PushRod_Flag=0; } break; case 2: Current_PushRod_STATE = PushRod_Down; if(D_value<0) { PushRod_Flag=0; } break; default: Current_PushRod_STATE = PushRod_HALT; break; } action_perfrom(Set_PushRod_States, sizeof(Set_PushRod_States) / sizeof(transition_t), Current_PushRod_STATE); } void Pressure_Adaptive_Function_Uptata(double Press_Dis_Value) { // double Desired_Presss=(double)CV.PV.Robot_Press_Set; // Desired_Presss=-500; //向下压为负值，向上运动为正值 double Actual_Value=GV.Force_Value_mN.CMCU_Measuring_value; Desired_Presss=-GV_Robot_Press_Set; D_value=Actual_Value-Desired_Presss; // -800--500 switch(PushRod_Flag) { case 0: if(D_value>Press_Dis_Value1) { PushRod_Flag=2;//压力过小//向下调整 } else if (D_value<-Press_Dis_Value1) { PushRod_Flag=1;//压力过大//`向上调整 } Current_PushRod_STATE = PushRod_HALT; break; case 1: Current_PushRod_STATE = PushRod_UP; if(D_value>0) { PushRod_Flag=0; } break; case 2: Current_PushRod_STATE = PushRod_Down; if(D_value<0) { PushRod_Flag=0; } break; default: Current_PushRod_STATE = PushRod_HALT; break; } action_perfrom(Set_PushRod_States, sizeof(Set_PushRod_States) / sizeof(transition_t), Current_PushRod_STATE); }