MaRuihan
/
jiangnan_swing_app
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
江南摆臂机器人app
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
4 Commits
1 Branch
0 Tags
1.5 MiB
 
 
Tree: dbe9c3e792
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from 'dbe9c3e792'
${ noResults }
jiangnan_swing_app/Core/Src/fsm.c

4307 lines
106 KiB
Raw Blame History

/*
* fsm.c
*
* Created on: Oct 18, 2024
* Author: akeguo
*/
//平面区域自动作业 Region_Automated_Task_Func_Alternately_Plane();//一米只行驶95cm，
#include "fsm.h"
#include <math.h>
#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 Pressure_Adaptive_Function_Uptata2(void);
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_Uptata2();
}
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_LLL<Error_Cont_Thresh)
{
Forward_Flag_Robot_Auto++;
}
Robot_Platform_Back_Flag=0;
Swing_Limit_Flag=0;
break;
case Forward_Motion:
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;
}
Robot_Platform_Back_Flag=0;
break;
case Fight_Alternately:
Horiz_Angle_Judge();
Fight_Alternately_Function_Horizontal();
break;
case Fight_retreating:
Horiz_Angle_Judge();
Fight_Countinus_Function_Horizontal();
break;
default:
HALT_State_Do();
break;
}
}
double Robot_Angle_DEG;
void Change_Road_Down_Left_Right()
{
switch(Change_Road_Jude_Flag)
{
case 0:
Robot_Angle_DEG=((double)GV.TL720DParameters.RF_Angle_Roll)/100;
if(Robot_Angle_DEG>0)
{
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_Uptata2();
}
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_Uptata2();
}
// 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_LLL<Error_Cont_Thresh)
{
Forward_Flag_Robot_Manual++;
Robot_Halt_Mode();
}
break;
case Forward_Motion:
// Move_Horizontal_Vertical_Task_Forwards_Do_Forwards();
Vertical_Angle_Judge();
double Robot_Speed_Base_Vertical_Up=Robot_Speed_Base/0.98;
Move_Horizontal_Vertical_Task_Forwards_Do_Forwards(Robot_Speed_Base_Vertical_Up, CV_Robot_Deri_Angle_Deg_Grity);
break;
default:
HALT_State_Do();
break;
}
}
void Move_Vertical_Manual_Sub_Func_Backward()
{
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_LLL<Error_Cont_Thresh)
{
Forward_Flag_Robot_Manual++;
Robot_Halt_Mode();
}
break;
case Forward_Motion:
Vertical_Angle_Judge();
double Robot_Speed_Base_Vertical_Down=Robot_Speed_Base/1.05;
Move_Horizontal_Vertical_Task_Backwards_Do_Backward(Robot_Speed_Base_Vertical_Down,CV_Robot_Deri_Angle_Deg_Grity);
break;
default:
HALT_State_Do();
break;
}
// UltraStopReverse(Forward_Flag_Robot_Manual);
}
void Move_Vertical_Auto_Sub_Func()
{
switch (Forward_Flag_Robot_Auto)
{
case Forward_Attitude_Judge:
// Vertical_Angle_Judge();
Vertical_Angle_Judge_Uptata_1();
Forward_Flag_Robot_Auto++;
break;
case Forward_Attitude_Adjust:
Robot_Posture_Adjus_Gravity();
if(Angle_Error_LLL<Error_Cont_Thresh)
{
Forward_Flag_Robot_Auto++;
// Robot_Halt_Mode();
}
Robot_Platform_Back_Flag=0;
Swing_Limit_Flag=0;
break;
case Forward_Motion:
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;
}
Robot_Platform_Back_Flag=0;
break;
case Fight_Alternately:
Vertical_Angle_Judge();
Fight_Alternately_Function_Vertical();
break;
case Fight_retreating:
Vertical_Angle_Judge();
Fight_Countinus_Function_Horizontal();
break;
default:
HALT_State_Do();
break;
}
}
void Move_Vertical_Auto_Sub_Func_Forward()
{
switch (Forward_Flag_Robot_Auto)
{
case Forward_Attitude_Judge:
Vertical_Angle_Judge();
Forward_Flag_Robot_Auto++;
break;
case Forward_Attitude_Adjust:
Robot_Posture_Adjus_Gravity();
if(Angle_Error_LLL<Error_Cont_Thresh)
{
Forward_Flag_Robot_Auto++;
Robot_Halt_Mode();
}
break;
case Forward_Motion:
Fight_Alternately_Function_Vertical();
Move_Vertical_Task_Forwards_Do();
break;
default:
HALT_State_Do();
break;
}
}
void Plane_Operatin_Main_Func()
{
//PushRod_Contronl();//推杆上下停止控制
Blast_Machine_Control_Fun();
if(P_MK32->CH4_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<Error_Cont_Thresh)
{
Region_Task_Falg++;
}
Auto_Job_Back_Time_MS_Count=0;
Robot_Platform_Back_Flag=0;
break;
case Lane_Change_Y:
Swing_Limit_Contrl();//摆臂限位控制
// Platform_Back_Para_Compute_Plane();
Robot_Platform_Back_Contronl_Plane();
if(Backward_Num_X_Count>=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<Error_Cont_Thresh)
{
Region_Task_Falg=1;
}
break;
case Region_Auto_Close:
HALT_State_Do();
// Blast_Machine_Close();//hjb
Blast_Machine_Close_Fun();//hjb
Plane_Complete_Count=0;
GV.SwingMotor.Target_Velcity =0;
break;
default:
HALT_State_Do();
break;
}
}
double Horizontal_Turn_Angle_Deg;
int Horizontal_Complete_Total_XY=0;
///水平区域自动作业，水平区域自动作业
void Region_Automated_Task_Func_Alternately_Horizontal()
{
switch (Region_Task_Falg)
{
case Parameter_Calculation:
Horiz_Angle_Judge();
Deri_Angle_Deg_X[0]=CV_Robot_Deri_Angle_Deg_Grity;
Horiz_Angle_Judge_Region();
Horizontal_Turn_Angle_Deg=0;
Horizontal_X_Backward_Num_Calculation(&Backward_Num_X_Total);
Horizontal_Change_Road_Backward_Num_Calculation(&Lane_Change_Num_Total_Y);
Horizontal_Y_Lane_Change_Time_Calculation(&Horizontal_Complete_Total_XY);
Region_Task_Falg++;
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 RE_Horizontal_Turn_To_Desire_Angle:
CV_Robot_Deri_Angle_Deg_Grity=X_Deri_Angle[0];
Robot_Posture_Adjus_Gravity();
if(Angle_Error_LLL<Error_Cont_Thresh)
{
Region_Task_Falg++;
Auto_Job_Back_Time_MS_Count=0;
Robot_Platform_Back_Flag=0;
}
break;
case RE_Horizontal_Backward_Task_X:
Horiz_Angle_Judge();
Horiz_Angle_Judge_Region();
Swing_Limit_Contrl();//摆臂限位控制
Robot_Platform_Back_Contronl_Horizontal();
if(Backward_Num_X_Count>=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<Error_Cont_Thresh)
{
Region_Task_Falg++;
}
Auto_Job_Back_Time_MS_Count=0;
Robot_Platform_Back_Flag=0;
Backward_Num_X_Count=0;
break;
case RE_Horizontal_Lane_Change_Y:
Swing_Limit_Contrl();//摆臂限位控制
Robot_Platform_Back_Contronl_Horizontal();
if(Backward_Num_X_Count>=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_LLL<Error_Cont_Thresh)
{
Region_Task_Falg=RE_Horizontal_Backward_Task_X;
}
break;
case RE_Horizontal_Region_Auto_Close:
HALT_State_Do();
Blast_Machine_Close_Fun();//hjb
Plane_Complete_Count=0;
GV.SwingMotor.Target_Velcity =0;
break;
default:
HALT_State_Do();
break;
}
}
void Vertical_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_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<Error_Cont_Thresh)
{
Plane_Lane_Change_Falg++;
}
break;
case 1: //后退
Back_Time_Count++;
Move_Plane_Task_Backwards_Do();
if(Back_Time_Count>*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_LLL<Error_Cont_Thresh)
{
Plane_Lane_Change_Falg++;
}
break;
default:
HALT_State_Do();
break;
}
}
void Move_Horizontal_Manual_Sub_Func_Forwards()
{
switch (Forward_Flag_Robot_Manual)
{
case Forward_Attitude_Judge:
Horiz_Angle_Judge_4_Direction();
Forward_Flag_Robot_Manual++;
break;
case Forward_Attitude_Adjust:
Robot_Posture_Adjus_Gravity();
if(Angle_Error_LLL<Error_Cont_Thresh)
{
Forward_Flag_Robot_Manual++;
Robot_Halt_Mode();
}
break;
case Forward_Motion:
Horiz_Angle_Judge_4_Direction();
Move_Horizontal_Vertical_Task_Forwards_Do_Forwards(Robot_Speed_Base, CV_Robot_Deri_Angle_Deg_Grity);
break;
default:
HALT_State_Do();
break;
}
}
void Move_Horizontal_Manual_Sub_Func_Backwards()
{
switch (Forward_Flag_Robot_Manual)
{
case Forward_Attitude_Judge:
Horiz_Angle_Judge_4_Direction();
Forward_Flag_Robot_Manual++;
break;
case Forward_Attitude_Adjust:
Robot_Posture_Adjus_Gravity();
if(Angle_Error_LLL<Error_Cont_Thresh)
{
Forward_Flag_Robot_Manual++;
Robot_Halt_Mode();
}
break;
case Forward_Motion:
Horiz_Angle_Judge_4_Direction();
Move_Horizontal_Vertical_Task_Backwards_Do_Backward(Robot_Speed_Base,CV_Robot_Deri_Angle_Deg_Grity);
break;
default:
HALT_State_Do();
break;
}
}
void Horiz_Angle_Judge()
{
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))
{
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_LLL<Error_Cont_Thresh)
{
CRLU_Flag++;
Change_R_L_Count=0;
}
break;
case 1://计算换道后退时间，设定后退速度
Vertical_Change_Road_Back_Time_Compute();
CRLU_Flag++;
break;
case 2://PID转向期望角度1
Change_R_L_Count++;
Move_Horizontal_Task_Change_Road_Backwards_Do();
if(Change_R_L_Count>Vertical_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_LLL<Error_Cont_Thresh)
{
CRLU_Flag++;
}
break;
case 4://到达位置
GV.LeftMotor.Target_Velcity=0;
GV.RightMotor.Target_Velcity=0;
break;
default:
HALT_State_Do();
break;
}
// UltraStopReverse(CRLU_Flag);
}
void Change_Road_Down_Up_Left_To_Right()
{
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_LLL<Error_Cont_Thresh)
{
CRLU_Flag++;
Change_R_L_Count=0;
}
break;
case 1://计算换道后退时间，设定后退速度
Vertical_Change_Road_Back_Time_Compute();
CRLU_Flag++;
break;
case 2://PID转向期望角度1
Change_R_L_Count++;
Move_Horizontal_Task_Change_Road_Backwards_Do();
if(Change_R_L_Count>Vertical_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_LLL<Error_Cont_Thresh)
{
CRLU_Flag++;
}
break;
case 4://到达位置
GV.LeftMotor.Target_Velcity=0;
GV.RightMotor.Target_Velcity=0;
break;
default:
HALT_State_Do();
break;
}
// UltraStopReverse(CRLU_Flag);
}
double Der_Angle_Plane[2]={0,0};
void Change_Road_Plane()
{
static int Change_R_L_Count;
switch(CRLU_Flag)
{
case Expected_Angle_Confir://两轮速度相同转向
Der_Angle_Plane[0]=MF40G_Angle_Add_Deg+90;
if(Der_Angle_Plane[0]>180)
{
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_LLL<Error_Cont_Thresh)
{
Plane_Change_Road_Back_Time_Compute();//后退时间计算
CRLU_Flag++;
Change_R_L_Count=0;
}
break;
case Back_Runing_Y://PID转向期望角度1
Change_R_L_Count++;
Move_Plane_Task_Backwards_Distance_Do_Update(Change_Road_Speed, CV_Robot_Deri_Angle_Deg_Plane);
if(Change_R_L_Count>Plane_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_LLL<Error_Cont_Thresh)
{
CRLU_Flag++;
Change_R_L_Count=0;
}
break;
case Change_Road_Completed://到达位置
GV.LeftMotor.Target_Velcity=0;
GV.RightMotor.Target_Velcity=0;
GV.SwingMotor.Target_Velcity=0;
break;
default:
HALT_State_Do();
break;
}
// UltraStopReverse(CRLU_Flag);
}
void Change_Road_Plane_Continuous_FanDi_Left()
{
static int Change_R_L_Count;
switch(CRLU_Flag)
{
case Expected_Angle_Confir://两轮速度相同转向
Swing_Limit_Flag=0;
Der_Angle_Plane[0]=MF40G_Angle_Add_Deg-90;
if(Der_Angle_Plane[0]>180)
{
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_LLL<Error_Cont_Thresh)
{
Plane_Change_Road_Back_Time_Countinus();//后退时间计算
CRLU_Flag++;
Change_R_L_Count=0;
}
break;
case Back_Runing_Y://PID转向期望角度1
Change_R_L_Count++;
Move_Plane_Task_Backwards_Distance_Do_Update(Robot_Countinus_Speed, CV_Robot_Deri_Angle_Deg_Plane);
if(Change_R_L_Count>Plane_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_LLL<Error_Cont_Thresh)
{
CRLU_Flag++;
Change_R_L_Count=0;
}
break;
case Change_Road_Completed://到达位置
GV.LeftMotor.Target_Velcity=0;
GV.RightMotor.Target_Velcity=0;
GV.SwingMotor.Target_Velcity=0;
break;
default:
HALT_State_Do();
break;
}
// UltraStopReverse(CRLU_Flag);
}
void Change_Road_Plane_Continuous_FanDi_Right()
{
static int Change_R_L_Count;
switch(CRLU_Flag)
{
case Expected_Angle_Confir://两轮速度相同转向
Swing_Limit_Flag=0;
Der_Angle_Plane[0]=MF40G_Angle_Add_Deg+90;
if(Der_Angle_Plane[0]>180)
{
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_LLL<Error_Cont_Thresh)
{
Plane_Change_Road_Back_Time_Countinus();//后退时间计算
CRLU_Flag++;
Change_R_L_Count=0;
}
break;
case Back_Runing_Y://PID转向期望角度1
Change_R_L_Count++;
Move_Plane_Task_Backwards_Distance_Do_Update(Robot_Countinus_Speed, CV_Robot_Deri_Angle_Deg_Plane);
if(Change_R_L_Count>Plane_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_LLL<Error_Cont_Thresh)
{
CRLU_Flag++;
Change_R_L_Count=0;
}
break;
case Change_Road_Completed://到达位置
GV.LeftMotor.Target_Velcity=0;
GV.RightMotor.Target_Velcity=0;
GV.SwingMotor.Target_Velcity=0;
break;
default:
HALT_State_Do();
break;
}
// UltraStopReverse(CRLU_Flag);
}
void Move_Plane_Task_Fordwards_Do()
{
double Deri_Speed_Robot_MAX=((double)GV.Robot_Move_Speed)*1.5;
double Robot_Speed[4]={0,0,0,0};
double Robot_Plane=MF40G_Angle_Add_Deg;
GF_MSP_PID_Now_Der_adj_Com_Horizon(Robot_Plane, 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];
}
void Move_Plane_Task_Fordwards_Do_Update(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);
GV.LeftMotor.Target_Velcity =Robot_Speed[0];
GV.RightMotor.Target_Velcity =-Robot_Speed[1];
}
void Move_Plane_Task_Backwards_Do()
{
double Deri_Speed_Robot_MAX=((double)GV.Robot_Move_Speed)*1.5;
double Robot_Speed[4]={0,0,0,0};
double Robot_Plane=MF40G_Angle_Add_Deg;
GF_MSP_PID_Now_Der_adj_Com_Horizon(Robot_Plane, 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[0];
GV.RightMotor.Target_Velcity =Robot_Speed[1];
}
void Move_Plane_Task_Backwards_Distance_Do()
{
double Deri_Speed_Robot_MAX=((double)GV.Robot_Move_Speed)*1.5;
double Robot_Speed[4]={0,0,0,0};
double Robot_Plane=MF40G_Angle_Add_Deg;
GF_MSP_PID_Now_Der_adj_Com_Horizon(Robot_Plane, 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[0];
GV.RightMotor.Target_Velcity =Robot_Speed[1];
}
void Move_Horizontal_Vertical_Task_Backwards_Do_Backward(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[1];
GV.RightMotor.Target_Velcity = Robot_Speed[0];
}
void Move_Plane_Task_Backwards_Distance_Do_Update(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);
GV.LeftMotor.Target_Velcity =-Robot_Speed[1];
GV.RightMotor.Target_Velcity =Robot_Speed[0];
}
void Move_Plane_Task_Backwards_Distance_Do_Update_Turn(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;
}
}
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_LLL<Error_Cont_Thresh)
{
CRLU_Flag++;
Change_R_L_Count_1=0;
}
break;
case 1: //计算后退时间
Horizontal_Change_Road_Back_Time_Compute();
CRLU_Flag++;
break;
case 2://PID转向期望角度1
Change_R_L_Count_1++;
// Move_Vertical_Task_Change_Road_Backwards_Do();
Move_Horizontal_Vertical_Task_Backwards_Do_Backward(Change_Road_Speed,CV_Robot_Deri_Angle_Deg_Grity);
if(Change_R_L_Count_1>Horizontal_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_LLL<Error_Cont_Thresh)
{
CRLU_Flag++;
}
break;
case 4://到达位置
GV.LeftMotor.Target_Velcity=0;
GV.RightMotor.Target_Velcity=0;
break;
default:
HALT_State_Do();
break;
}
}
void Change_Road_Right()
{
static int Change_R_L_Count;
double Der_Angle_Right[2]={0,90};
switch(CRLU_Flag)
{
case 0://两轮速度相同转向
CV_Robot_Deri_Angle_Deg_Grity=Der_Angle_Right[0];
Robot_Posture_Adjus_Gravity();
if(Angle_Error_LLL<Error_Cont_Thresh)
{
CRLU_Flag++;
Change_R_L_Count=0;
}
break;
case 1://计算后退时间，设定后退速度
CRLU_Flag++;
Horizontal_Change_Road_Back_Time_Compute();
break;
case 2://PID转向期望角度1
Change_R_L_Count++;
// Move_Vertical_Task_Change_Road_Backwards_Do();
Move_Horizontal_Vertical_Task_Backwards_Do_Backward(Change_Road_Speed,CV_Robot_Deri_Angle_Deg_Grity);
if(Change_R_L_Count>Horizontal_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_LLL<Error_Cont_Thresh)
{
CRLU_Flag++;
}
break;
case 4://到达位置
GV.LeftMotor.Target_Velcity=0;
GV.RightMotor.Target_Velcity=0;
break;
default:
HALT_State_Do();
break;
}
}
void Forwards_State_Do(void)
{
GV.LeftMotor.Target_Velcity = Robot_Speed_Base;
GV.RightMotor.Target_Velcity = -Robot_Speed_Base;
}
void Backwards_State_Do(void)
{
GV.LeftMotor.Target_Velcity = -Robot_Speed_Base;
GV.RightMotor.Target_Velcity = Robot_Speed_Base;
}
void TurnLeft_State_Do(void)
{
GV.LeftMotor.Target_Velcity =-Robot_Speed_Base/8;
GV.RightMotor.Target_Velcity =-Robot_Speed_Base/8;
}
void TurnRight_State_Do(void)
{
GV.LeftMotor.Target_Velcity =Robot_Speed_Base/8;
GV.RightMotor.Target_Velcity =Robot_Speed_Base/8;
}
void TurnRight_State_Do_Plane(void)
{
GV.LeftMotor.Target_Velcity =-Robot_Speed_Base/8;
GV.RightMotor.Target_Velcity =-Robot_Speed_Base/8;
}
void TurnLeft_State_Do_Plane(void)
{
GV.LeftMotor.Target_Velcity =Robot_Speed_Base/8;
GV.RightMotor.Target_Velcity =Robot_Speed_Base/8;
}
int Left_Limtit_Position=95*TT_One_Deg_Count;
int Right_Limtit_Position=-95*TT_One_Deg_Count;
void Move_Swing_Left_Func_Do(void)
{
GV.SwingMotor.Position_immediately1_Lag2=1;
GV.SwingMotor.Tar_Position_count=Left_Limtit_Position;
GV.SwingMotor.Tar_Position_Velcity_RPM=GV_Swing_Speed*Swing_Speed_Deg_Sencond;
// GV.SwingMotor.Target_Velcity = GV_Swing_Speed*Swing_Speed_Deg_Sencond;
}
void Move_Swing_Right_Func_Do(void)
{
GV.SwingMotor.Position_immediately1_Lag2=1;
GV.SwingMotor.Tar_Position_count=Right_Limtit_Position;
GV.SwingMotor.Tar_Position_Velcity_RPM=GV_Swing_Speed*Swing_Speed_Deg_Sencond;
// GV.SwingMotor.Target_Velcity =-GV_Swing_Speed*Swing_Speed_Deg_Sencond;
}
void Move_Swing_Halt_Func_Do(void)
{
GV.SwingMotor.Position_immediately1_Lag2=1;
GV.SwingMotor.Tar_Position_count=0;
GV.SwingMotor.Tar_Position_Velcity_RPM=0;
// GV.SwingMotor.Target_Velcity = 0;
}
void Plane_Change_Road_Back_Time_Compute()
{
// double Change_Road_Speed=201.7*7.64*3;//换道速度设定为3米每分钟
// double Change_Road_Speed_MAX=201.7*7.64*3*1.5;//换道最高速度设定为4.5米每分钟
Plane_Change_Road_Time=((double)GV_Robot_Change_Lane_Distance)*0.01/3*60*500;//一米的时候超出12cm
}
void Plane_Change_Road_Back_Time_Countinus()
{
// double Change_Road_Speed=201.7*7.64*3;//换道速度设定为3米每分钟
// double Change_Road_Speed_MAX=201.7*7.64*3*1.5;//换道最高速度设定为4.5米每分钟
Plane_Change_Road_Time=((double)GV_Robot_Change_Lane_Distance)*0.01/GV_Robot_Back_Speed*60*500;//一米的时候超出12cm
}
void Horizontal_Change_Road_Back_Time_Compute()
{
// double Change_Road_Speed=201.7*7.64*3;//换道速度设定为3米每分钟
// double Change_Road_Speed_MAX=201.7*7.64*3*1.5;//换道最高速度设定为4.5米每分钟
double GV_Robot_Change_Lane_Distance_Updata;
if(GV_Robot_Change_Lane_Distance>20)
{
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_Position<Right_Target_Pos+25000)
{
Swing_Limit_Flag=4;
Swing_Time_Count=0;
}
break;
case 3: //向右旋转
Swing_Time_Count++;
if((GV.SwingMotor.Real_Position>Left_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_Position<Right_Target_Pos+20000)||(Swing_Time_Count>Swing_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_Position<Right_Threshold_Pos))
{
Robot_Platform_Back_Flag++;
Auto_Job_Back_Time_MS_Count=0;
}
if(Auto_Job_Back_Time_MS_Count>Swing_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_Position<Left_Threshold_Pos)&&(GV.SwingMotor.Real_Position>Right_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_Position<Right_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 3:
Move_Horizontal_Vertical_Task_Backwards_Do_Backward(Vertical_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_Vertical)
{
Robot_Platform_Back_Flag++;
Auto_Job_Back_Time_MS_Count=0;
}
break;
case 4:
if((GV.SwingMotor.Real_Position<Left_Threshold_Pos)&&(GV.SwingMotor.Real_Position>Right_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_LLL<Error_Cont_Thresh)
{
Robot_Platform_Back_Flag_Y++;
}
Auto_Job_Back_Time_MS_Count=0;
Robot_Platform_Back_Flag=0;
break;
case 1://后退指定次数
Robot_Platform_Back_Contronl_Plane();
break;
case 2:
break;
}
}
void Robot_Platform_Back_Contronl_Plane()
{
switch(Robot_Platform_Back_Flag)
{
case 0:
Platform_Back_Para_Compute_Plane();//计算后退时间及后退速度
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_Position<Right_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 3:
Move_Plane_Task_Backwards_Distance_Do_Update(Plane_Back_Speed_Value, CV_Robot_Deri_Angle_Deg_Plane);
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_Position<Left_Threshold_Pos)&&(GV.SwingMotor.Real_Position>Right_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<Error_Cont_Thresh)
{
Region_Task_Falg++;
}
Auto_Job_Back_Time_MS_Count=0;
Robot_Platform_Back_Flag=0;
Backward_Time_Count=0;
break;
case Lane_Change_Y:
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>=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<Error_Cont_Thresh)
{
Region_Task_Falg=1;
}
break;
case Region_Auto_Close:
HALT_State_Do();
Plane_Complete_Count=0;
GV.SwingMotor.Target_Velcity =0;
break;
default:
HALT_State_Do();
break;
}
}
void Region_Automated_Task_Func_Continuous_Plane_Uptate_1()
{
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;
if( Deri_Angle_Deg_X[1]>180)
{
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<Error_Cont_Thresh)
{
Region_Task_Falg++;
}
Auto_Job_Back_Time_MS_Count=0;
Robot_Platform_Back_Flag=0;
Backward_Time_Count=0;
break;
case Lane_Change_Y:
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>=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<Error_Cont_Thresh)
{
Region_Task_Falg=1;
}
break;
case Region_Auto_Close:
HALT_State_Do();
Blast_Machine_Close_Fun();//hjb
Plane_Complete_Count=0;
GV.SwingMotor.Target_Velcity =0;
break;
default:
HALT_State_Do();
break;
}
}
void Region_Automated_Task_Func_Continuous_Horizontal()
{
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:
Horiz_Angle_Judge();
Deri_Angle_Deg_X[0]=CV_Robot_Deri_Angle_Deg_Grity;
Horiz_Angle_Judge_Region();
Horizontal_Turn_Angle_Deg=0;
Horizontal_X_Backward_Time_Calculation_Continuous(&Backward_Time_X_Total);
Horizontal_Change_Road_Backward_Time_Calculation_Continuous(&Lane_Change_Time_Total_Y);
Horizontal_Y_Lane_Change_Time_Calculation(&Horizontal_Complete_Total_XY);
Region_Task_Falg++;
X_Deri_Angle[0]=Deri_Angle_Deg_X[0];
Swing_Limit_Flag=0;
break;
case RE_Horizontal_Turn_To_Desire_Angle:
CV_Robot_Deri_Angle_Deg_Grity=X_Deri_Angle[0];
Robot_Posture_Adjus_Gravity();
if(Angle_Error_LLL<Error_Cont_Thresh)
{
Region_Task_Falg++;
}
Backward_Time_Count=0;
break;
case RE_Horizontal_Backward_Task_X:
Horiz_Angle_Judge();
Horiz_Angle_Judge_Region();
Swing_Limit_Contrl();//摆臂限位控制
CV_Robot_Deri_Angle_Deg_Grity=X_Deri_Angle[0];
Move_Horizontal_Vertical_Task_Backwards_Do_Backward(Robot_Countinus_Speed,CV_Robot_Deri_Angle_Deg_Grity);
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>=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<Error_Cont_Thresh)
{
Region_Task_Falg++;
}
break;
case RE_Horizontal_Lane_Change_Y:
Swing_Limit_Contrl();//摆臂限位控制
CV_Robot_Deri_Angle_Deg_Grity=Horizontal_Turn_Angle_Deg;
Move_Horizontal_Vertical_Task_Backwards_Do_Backward(Robot_Countinus_Speed,CV_Robot_Deri_Angle_Deg_Grity);
Backward_Time_Count++;
if(Backward_Time_Count>=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<Error_Cont_Thresh)
{
Region_Task_Falg=RE_Horizontal_Backward_Task_X;
}
break;
case RE_Horizontal_Region_Auto_Close:
HALT_State_Do();
Blast_Machine_Close_Fun();//hjb
Plane_Complete_Count=0;
GV.SwingMotor.Target_Velcity =0;
break;
default:
HALT_State_Do();
break;
}
}
void Vertical_Angle_Judge_4_Direction()
{
// GV_Robot_Vertical_Adjust
double Robot_Grity=((double )GV.TL720DParameters.RF_Angle_Roll)/100;
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;
}
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_Uptata2(void)
{
// 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;
if(abs(Actual_Value)>abs(Desired_Presss))
{
Current_PushRod_STATE = PushRod_HALT;
}
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);
}