|
|
@ -86,6 +86,7 @@ void EmergencyStopControl() |
|
|
{ |
|
|
{ |
|
|
if((GV.MK32_Key.CH8_SE == -1000) && GV.MK32_Key.CH9_SF == -1000) |
|
|
if((GV.MK32_Key.CH8_SE == -1000) && GV.MK32_Key.CH9_SF == -1000) |
|
|
{ |
|
|
{ |
|
|
|
|
|
LS_485Motors_Speed = 0; |
|
|
CurrentMoveState = Move_HALT; |
|
|
CurrentMoveState = Move_HALT; |
|
|
CurrentFrontEndState = HALT_STATE; |
|
|
CurrentFrontEndState = HALT_STATE; |
|
|
StrongGrindingMachineCurrentState = STRONG_GRINDING_MACHINE_HALT_STATE; |
|
|
StrongGrindingMachineCurrentState = STRONG_GRINDING_MACHINE_HALT_STATE; |
|
|
@ -446,9 +447,19 @@ int Knife_Detection(void) |
|
|
|
|
|
|
|
|
extern int32_t KinfePosition; |
|
|
extern int32_t KinfePosition; |
|
|
extern int StrongSingleShotFlag; |
|
|
extern int StrongSingleShotFlag; |
|
|
|
|
|
int Frontend_Speed = 0; |
|
|
|
|
|
|
|
|
void Frontend_Control() |
|
|
void Frontend_Control() |
|
|
{ |
|
|
{ |
|
|
|
|
|
int val = GV.MK32_Key.CH11_RD1 + 1000; |
|
|
|
|
|
if (val > 0 && val <= 286) Frontend_Speed = 120; |
|
|
|
|
|
else if (val > 286 && val <= 571) Frontend_Speed = 180; |
|
|
|
|
|
else if (val > 571 && val <= 857) Frontend_Speed = 240; |
|
|
|
|
|
else if (val > 857 && val <= 1143) Frontend_Speed = 300; |
|
|
|
|
|
else if (val > 1143 && val <= 1429) Frontend_Speed = 360; |
|
|
|
|
|
else if (val > 1429 && val <= 1714) Frontend_Speed = 420; |
|
|
|
|
|
else if (val > 1714 && val <= 2000) Frontend_Speed = 480; |
|
|
|
|
|
|
|
|
int angle; |
|
|
int angle; |
|
|
angle = atan2(GV.MK32_Key.CH1_RY_V, GV.MK32_Key.CH0_RY_H) * 180 / M_PI; |
|
|
angle = atan2(GV.MK32_Key.CH1_RY_V, GV.MK32_Key.CH0_RY_H) * 180 / M_PI; |
|
|
|
|
|
|
|
|
@ -466,7 +477,7 @@ void Frontend_Control() |
|
|
// 右转
|
|
|
// 右转
|
|
|
if((angle >= -45) && (angle <=45)) |
|
|
if((angle >= -45) && (angle <=45)) |
|
|
{ |
|
|
{ |
|
|
LS_485Motors_Speed = 100; |
|
|
LS_485Motors_Speed = Frontend_Speed; |
|
|
return; |
|
|
return; |
|
|
} |
|
|
} |
|
|
} |
|
|
} |
|
|
@ -479,7 +490,7 @@ void Frontend_Control() |
|
|
// 左转
|
|
|
// 左转
|
|
|
else if(((angle > 135) && (angle <= 180)) || (angle < -135) && (angle >= -179)) |
|
|
else if(((angle > 135) && (angle <= 180)) || (angle < -135) && (angle >= -179)) |
|
|
{ |
|
|
{ |
|
|
LS_485Motors_Speed = -100; |
|
|
LS_485Motors_Speed = -Frontend_Speed; |
|
|
return; |
|
|
return; |
|
|
} |
|
|
} |
|
|
else { |
|
|
else { |
|
|
@ -579,7 +590,7 @@ void IV_Control() |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
IV.kinfe_position = (KinfePosition == 1000) ? KinfePosition : (GV.LS_FrontEnd_Motor.Real_Position - KinfePosition);// 这里上传的是原始数据,需要APP上缩小20000倍
|
|
|
IV.kinfe_position = (KinfePosition == 1000) ? KinfePosition : (GV.LS_FrontEnd_Motor.Real_Position - KinfePosition);// 这里上传的是原始数据,需要APP上缩小20000倍
|
|
|
IV.max_laser_sensor = MaxLaserSensor; |
|
|
IV.max_laser_sensor = Frontend_Speed*100/120; |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
void Robot_Control() |
|
|
void Robot_Control() |
|
|
|