|
|
|
@ -35,6 +35,7 @@ transition_t MoveTransitions[] = |
|
|
|
{ Move_HALT, HALT_State_Do }, |
|
|
|
{ Manual_State, Manual_State_Do }, |
|
|
|
{ low_Speed_Manual_State, low_Speed_Manual_State_Do}, |
|
|
|
{ AUTO_FORWARD, auto_forward_state_do} |
|
|
|
|
|
|
|
}; |
|
|
|
|
|
|
|
@ -43,6 +44,8 @@ transition_t FrontendMoveTransitions[] = |
|
|
|
{ HALT_STATE, FrontEnd_Halt_State_Do}, |
|
|
|
{ Manual_UP_STATE, Manual_Up_State_Do}, |
|
|
|
{ Manual_DOWN_STATE, Manual_Down_State_Do}, |
|
|
|
{ MANUAL_LOW_SPEED_UP_STATE, Manual_Low_Speed_Up_State_Do}, |
|
|
|
{ MANUAL_LOW_SPEED_DOWN_STATE, Manual_low_speed_Down_State_Do}, |
|
|
|
|
|
|
|
}; |
|
|
|
|
|
|
|
@ -54,8 +57,6 @@ transition_t StrongGrindingMachineTransitions[] = |
|
|
|
}; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void Fsm_Init() |
|
|
|
{ |
|
|
|
GF_BSP_Interrupt_Add_CallBack( |
|
|
|
@ -67,16 +68,12 @@ void GF_Dispatch() |
|
|
|
{ |
|
|
|
IV_Control(); |
|
|
|
|
|
|
|
// 机器人本体控制
|
|
|
|
Robot_Control(); |
|
|
|
|
|
|
|
// 前端控制
|
|
|
|
Frontend_Control(); |
|
|
|
|
|
|
|
//强磨机控制
|
|
|
|
Strong_Grinding_Machine_Control(); |
|
|
|
|
|
|
|
Mode_Control(); |
|
|
|
|
|
|
|
// 限位安全检测
|
|
|
|
Limit_Detection(); |
|
|
|
|
|
|
|
action_perfrom(MoveTransitions,sizeof(MoveTransitions) / sizeof(transition_t), CurrentMoveState); |
|
|
|
action_perfrom(FrontendMoveTransitions, |
|
|
|
@ -85,11 +82,153 @@ void GF_Dispatch() |
|
|
|
sizeof(StrongGrindingMachineTransitions) / sizeof(transition_t), StrongGrindingMachineCurrentState); |
|
|
|
} |
|
|
|
|
|
|
|
uint8_t upper_limit_position = 0; |
|
|
|
uint8_t lower_limit_position = 0; |
|
|
|
|
|
|
|
void Limit_Detection() |
|
|
|
{ |
|
|
|
// upper_limit_position = GF_BSP_GPIO_ReadIO(2);
|
|
|
|
// lower_limit_position = GF_BSP_GPIO_ReadIO(3);
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
uint8_t Mode_Select_State = MANUAL_OPERATION; |
|
|
|
|
|
|
|
void Mode_Control() |
|
|
|
{ |
|
|
|
Mode_Select_State = GV.PV.robot_operation_mode; |
|
|
|
|
|
|
|
switch(Mode_Select_State) |
|
|
|
{ |
|
|
|
case MANUAL_OPERATION: |
|
|
|
// 机器人本体控制
|
|
|
|
Robot_Control(); |
|
|
|
|
|
|
|
// 前端雷赛电机控制
|
|
|
|
Frontend_Control(); |
|
|
|
|
|
|
|
// 前端强磨机控制
|
|
|
|
Strong_Grinding_Machine_Control(); |
|
|
|
break; |
|
|
|
case AUTO_OPERATION: |
|
|
|
|
|
|
|
// 自动作业任务
|
|
|
|
Automatic_Operation(); |
|
|
|
|
|
|
|
break; |
|
|
|
default: |
|
|
|
CurrentMoveState = Move_HALT; |
|
|
|
CurrentFrontEndState = HALT_STATE; |
|
|
|
StrongGrindingMachineCurrentState = STRONG_GRINDING_MACHINE_HALT_STATE; |
|
|
|
break; |
|
|
|
} |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
void IO_Control() |
|
|
|
{ |
|
|
|
GF_BSP_GPIO_SetIO(4, 1); |
|
|
|
} |
|
|
|
|
|
|
|
uint8_t IsAllowRotation_AutoMode = 0; |
|
|
|
int32_t knife_descent_height = 0; |
|
|
|
// 自动作业停止后默认上升高度为5mm
|
|
|
|
uint8_t knife_rising_height = 5; |
|
|
|
int32_t knife_current_altitude = 0; |
|
|
|
uint16_t delay_counts = 0; |
|
|
|
uint8_t auto_work_mode_states = 5; |
|
|
|
uint8_t auto_work_halt_states = 10; |
|
|
|
|
|
|
|
void Automatic_Operation() |
|
|
|
{ |
|
|
|
knife_descent_height = GV.PV.knife_descending_height; |
|
|
|
|
|
|
|
if(GV.MK32_Key.CH7_SD == -1000) |
|
|
|
{ |
|
|
|
switch(auto_work_mode_states) |
|
|
|
{ |
|
|
|
case 5: |
|
|
|
knife_current_altitude = GV.ZHR29_200_measure_results.laser_sensor_1_measure_distance; |
|
|
|
auto_work_mode_states = 10; |
|
|
|
break; |
|
|
|
case 10: |
|
|
|
CurrentFrontEndState = Manual_DOWN_STATE; |
|
|
|
|
|
|
|
if(knife_current_altitude - GV.ZHR29_200_measure_results.laser_sensor_1_measure_distance |
|
|
|
>= knife_descent_height) |
|
|
|
{ |
|
|
|
CurrentFrontEndState = HALT_STATE; |
|
|
|
auto_work_halt_states = 10; |
|
|
|
auto_work_mode_states = 20; |
|
|
|
} |
|
|
|
break; |
|
|
|
case 20: |
|
|
|
IsAllowRotation_AutoMode = Knife_Detection(); |
|
|
|
if(IsAllowRotation_AutoMode == 1) |
|
|
|
{ |
|
|
|
StrongGrindingMachineCurrentState = STRONG_GRINDING_MACHINE_MOTION_STATE; |
|
|
|
delay_counts = delay_counts + 1; |
|
|
|
if(delay_counts >= 500) |
|
|
|
{ |
|
|
|
delay_counts = 0; |
|
|
|
CurrentMoveState = AUTO_FORWARD; |
|
|
|
} |
|
|
|
} |
|
|
|
else{ |
|
|
|
StrongGrindingMachineCurrentState = STRONG_GRINDING_MACHINE_HALT_STATE; |
|
|
|
} |
|
|
|
|
|
|
|
break; |
|
|
|
case 30: |
|
|
|
break; |
|
|
|
default: |
|
|
|
CurrentMoveState = Move_HALT; |
|
|
|
CurrentFrontEndState = HALT_STATE; |
|
|
|
StrongGrindingMachineCurrentState = STRONG_GRINDING_MACHINE_HALT_STATE; |
|
|
|
break; |
|
|
|
|
|
|
|
} |
|
|
|
} |
|
|
|
else{ |
|
|
|
delay_counts = 0; |
|
|
|
CurrentMoveState = Move_HALT; |
|
|
|
StrongGrindingMachineCurrentState = STRONG_GRINDING_MACHINE_HALT_STATE; |
|
|
|
|
|
|
|
if(auto_work_mode_states == 10) |
|
|
|
{ |
|
|
|
CurrentFrontEndState = HALT_STATE; |
|
|
|
auto_work_mode_states = 5; |
|
|
|
} |
|
|
|
else if(auto_work_mode_states == 20) |
|
|
|
{ |
|
|
|
switch(auto_work_halt_states) |
|
|
|
{ |
|
|
|
case 10: |
|
|
|
knife_current_altitude = GV.ZHR29_200_measure_results.laser_sensor_1_measure_distance; |
|
|
|
auto_work_halt_states = 20; |
|
|
|
break; |
|
|
|
case 20: |
|
|
|
CurrentFrontEndState = Manual_UP_STATE; |
|
|
|
|
|
|
|
if(GV.ZHR29_200_measure_results.laser_sensor_1_measure_distance - knife_current_altitude |
|
|
|
>= knife_rising_height) |
|
|
|
{ |
|
|
|
auto_work_mode_states = 5; |
|
|
|
CurrentFrontEndState = HALT_STATE; |
|
|
|
} |
|
|
|
break; |
|
|
|
} |
|
|
|
} |
|
|
|
else{ |
|
|
|
auto_work_mode_states = 5; |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
uint8_t IsAllowRotation = 0; |
|
|
|
void Strong_Grinding_Machine_Control() |
|
|
|
{ |
|
|
|
GF_BSP_GPIO_SetIO(4, 1); |
|
|
|
IsAllowRotation = Knife_Detection(); |
|
|
|
if(IsAllowRotation == 1) |
|
|
|
{ |
|
|
|
@ -99,11 +238,15 @@ void Strong_Grinding_Machine_Control() |
|
|
|
} |
|
|
|
else{ |
|
|
|
StrongGrindingMachineCurrentState = STRONG_GRINDING_MACHINE_HALT_STATE; |
|
|
|
// 只有在刀到位且不转的时候和刀未到位的时候可以控制
|
|
|
|
IO_Control(); |
|
|
|
} |
|
|
|
|
|
|
|
} |
|
|
|
else{ |
|
|
|
StrongGrindingMachineCurrentState = STRONG_GRINDING_MACHINE_HALT_STATE; |
|
|
|
// 只有在刀到位且不转的时候和刀未到位的时候可以控制
|
|
|
|
IO_Control(); |
|
|
|
} |
|
|
|
|
|
|
|
} |
|
|
|
@ -141,6 +284,9 @@ void Frontend_Control() |
|
|
|
int angle; |
|
|
|
angle = atan2(GV.MK32_Key.CH1_RY_V, GV.MK32_Key.CH0_RY_H) * 180 / M_PI; |
|
|
|
|
|
|
|
// 正常速度控制前端雷赛电机
|
|
|
|
if(GV.MK32_Key.CH5_SB == 0) |
|
|
|
{ |
|
|
|
// 前进
|
|
|
|
if((angle >= 45) && (angle <= 135)) |
|
|
|
{ |
|
|
|
@ -171,6 +317,47 @@ void Frontend_Control() |
|
|
|
else { |
|
|
|
CurrentFrontEndState = HALT_STATE; |
|
|
|
} |
|
|
|
} |
|
|
|
// 低速控制前端雷赛电机
|
|
|
|
else if(GV.MK32_Key.CH5_SB == -1000) |
|
|
|
{ |
|
|
|
// 前进
|
|
|
|
if((angle >= 45) && (angle <= 135)) |
|
|
|
{ |
|
|
|
CurrentFrontEndState = MANUAL_LOW_SPEED_UP_STATE; |
|
|
|
return; |
|
|
|
} |
|
|
|
else if(P_MK32->CH3_LY_H > 100) |
|
|
|
{ |
|
|
|
// 右转
|
|
|
|
if((angle >= -45) && (angle <=45)) |
|
|
|
{ |
|
|
|
|
|
|
|
return; |
|
|
|
} |
|
|
|
} |
|
|
|
// 后退
|
|
|
|
else if((angle >= -135) && (angle < -45)) |
|
|
|
{ |
|
|
|
CurrentFrontEndState = MANUAL_LOW_SPEED_DOWN_STATE; |
|
|
|
return; |
|
|
|
} |
|
|
|
// 左转
|
|
|
|
else if(((angle > 135) && (angle <= 180)) || (angle < -135) && (angle >= -179)) |
|
|
|
{ |
|
|
|
|
|
|
|
return; |
|
|
|
} |
|
|
|
else { |
|
|
|
CurrentFrontEndState = HALT_STATE; |
|
|
|
} |
|
|
|
|
|
|
|
} |
|
|
|
else{ |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
@ -181,6 +368,7 @@ void IV_Control() |
|
|
|
real_speed = (Act_Speed*360/100/101/6*3.1415926*0.325); |
|
|
|
IV.Robot_Move_Speed = real_speed; |
|
|
|
IV.RF_Angle_Roll = SP_MSP_RF_TL720D_Parameters_In->RF_Angle_Roll; |
|
|
|
IV.Remote_Status = GV.MK32_Key.IsOnline; |
|
|
|
IV.laser_sensor_1_measure_distance = GV.ZHR29_200_measure_results.laser_sensor_1_measure_distance; |
|
|
|
IV.laser_sensor_2_measure_distance = GV.ZHR29_200_measure_results.laser_sensor_2_measure_distance; |
|
|
|
IV.laser_sensor_3_measure_distance = GV.ZHR29_200_measure_results.laser_sensor_3_measure_distance; |
|
|
|
|
HJB\13752
3 months ago