diff --git a/.settings/language.settings.xml b/.settings/language.settings.xml
index 7c1ad0a..8abf83c 100644
--- a/.settings/language.settings.xml
+++ b/.settings/language.settings.xml
@@ -5,7 +5,7 @@
-
+
@@ -16,7 +16,7 @@
-
+
diff --git a/BASE/Src/BSP/bsp_client_setting.c b/BASE/Src/BSP/bsp_client_setting.c
index fea49bb..b607d57 100644
--- a/BASE/Src/BSP/bsp_client_setting.c
+++ b/BASE/Src/BSP/bsp_client_setting.c
@@ -85,26 +85,40 @@ void UpdateIV_State()//往安卓发的
void UpdateIV_PaintThickness()//往安卓发的
{
- if(GV.Robot_PaintThickness_Count <= LastCount + 1)
+ if(GV.Robot_PaintThickness_Count <= LastCount)
+ {
+ return;
+ }
+ if(GV.Robot_PaintThickness_Count == 1)
{
return;
}
client_setting_Handler->Tx_Buf[0] = 0x01;
client_setting_Handler->Tx_Buf[1] = 0x03;
- client_setting_Handler->Tx_Buf[2] = 0x02;
- client_setting_Handler->TxCount = 7;
- client_setting_Handler->Tx_Buf[3] = (GV.Robot_PaintThickness>>8) &0xff;
- client_setting_Handler->Tx_Buf[4] = GV.Robot_PaintThickness & 0xff;
+ client_setting_Handler->Tx_Buf[2] = 0x04;
+ client_setting_Handler->TxCount = 9;
+ client_setting_Handler->Tx_Buf[3] = ((GV.Robot_PaintThickness_Count - 1)>>8) &0xff;
+ client_setting_Handler->Tx_Buf[4] = (GV.Robot_PaintThickness_Count - 1) & 0xff;
+
- uint16_t crc=MB_CRC16(&(client_setting_Handler->Tx_Buf[0]), 5);
+ client_setting_Handler->Tx_Buf[5] = (GV.Robot_PaintThickness>>8) &0xff;
+ client_setting_Handler->Tx_Buf[6] = GV.Robot_PaintThickness & 0xff;
- client_setting_Handler->Tx_Buf[6] = (crc>>8) &0xff;
- client_setting_Handler->Tx_Buf[5] = crc & 0xff;
+ uint16_t crc=MB_CRC16(&(client_setting_Handler->Tx_Buf[0]), 7);
+
+ client_setting_Handler->Tx_Buf[8] = (crc>>8) &0xff;
+ client_setting_Handler->Tx_Buf[7] = crc & 0xff;
client_setting_Handler->UART_Tx(client_setting_Handler);
+ if(GV.Robot_PaintThickness_Count >= ((uint16_t)pow(2, 31)) - 1)
+ {
+ GV.Robot_PaintThickness_Count = 1;
+ }
+
LastCount = GV.Robot_PaintThickness_Count;
+
}
diff --git a/BASE/Src/MSP/msp_PaintThickness.c b/BASE/Src/MSP/msp_PaintThickness.c
index 25c746c..9f5f0aa 100644
--- a/BASE/Src/MSP/msp_PaintThickness.c
+++ b/BASE/Src/MSP/msp_PaintThickness.c
@@ -38,6 +38,7 @@ void Paint_Thickness_intialize(struct UARTHandler *Handler)
HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"PaintThickness_sensor",0,PaintThickness_sensor);
//uartHandler->Insert_HardWare_Entry_UART
LOG("steering_engine_intialize");
+
}
uint8_t Inquiry_Order_PT[6]={0X2E, 0X64, 0X30, 0X3C, 0X3A, 0X3E};
@@ -53,18 +54,11 @@ void GF_PT_Inquiry()
void decode_PaintThickness(uint8_t *buffer, uint16_t length)
{
+ uint8_t data[2000];
+ memcpy(data,buffer,length);
//memcpy(data_PaintThickness,buffer,length);
LOG("start decoding and the length is %d",length);
- if(length >= PaintThickness_LastLength + 5)
- {
- *PaintThickness_Count = *PaintThickness_Count + 1;
- }
- else
- {
- return;
- }
-
/* 校验 */
//;<:>
uint8_t crc_check[4] = {';','<',':','>'};
@@ -75,6 +69,17 @@ void decode_PaintThickness(uint8_t *buffer, uint16_t length)
&& crc_check[1] == buffer[length - 3]
&& crc_check[0] == buffer[length - 4])
{
+ //判断是否是新数据
+ if(length >= PaintThickness_LastLength + 5)
+ {
+ *PaintThickness_Count = *PaintThickness_Count + 1;
+ }
+ else
+ {
+ return;
+ }
+
+
int startIndex=0;
for(int j=5;j<=14;j++)
diff --git a/Core/Src/FSM.c b/Core/Src/FSM.c
index fe3cbb3..e703649 100644
--- a/Core/Src/FSM.c
+++ b/Core/Src/FSM.c
@@ -379,6 +379,10 @@ void RC_Emergency()
{
if(Current_T_F4_Value.emergency_stop == 1)
{
+ GV.AuTo_Flag = 0;
+ GV.Chg_Flag = 0;
+ Chg_Pa.change_road_finish_flag = 0;
+
GF_BSP_GPIO_SetIO(Out_Spray,1);
GF_BSP_GPIO_SetIO(0,1);
CurrentMoveState = Move_Emergency;
@@ -389,6 +393,110 @@ void RC_Emergency()
}
}
+//运动部分 Move Region
+void Robot_Move()
+{
+ if(Current_T_F4_Value.emergency_stop == 1)
+ {
+ return;
+ }
+ if((Current_T_F4_Value.ChgDown_Ver == 1 || Current_T_F4_Value.ChgUp_Ver == 1
+ || Current_T_F4_Value.ChgLeft_Hor == 1 || Current_T_F4_Value.ChgRight_Hor == 1
+ || Current_T_F4_Value.AutoForWard == 1 || Current_T_F4_Value.AutoBackWard == 1)
+ && (Current_T_F4_Value.robot_forward_backward_speed >= 0X7A
+ && Current_T_F4_Value.robot_forward_backward_speed <= 0X84)
+ && (Current_T_F4_Value.robot_left_right_turn_speed >= 0X7A
+ && Current_T_F4_Value.robot_left_right_turn_speed <= 0X84)
+ && GV.Robot_Ultrasonic.Dym_Value_Flag != 1)
+ {
+ if((Current_T_F4_Value.AutoForWard == 1 || Current_T_F4_Value.AutoBackWard == 1)
+ &&(Current_T_F4_Value.ChgDown_Ver != 1 && Current_T_F4_Value.ChgUp_Ver != 1
+ && Current_T_F4_Value.ChgLeft_Hor != 1 && Current_T_F4_Value.ChgRight_Hor != 1))//自动作业与换道互斥
+ {
+ if(Current_T_F4_Value.AutoForWard == 1)
+ {
+ //如启停喷枪按下后判断前进速度到达后，喷漆自动开启
+ AutoForSpray_On();
+ //喷漆程序结束
+ }
+ else if(Current_T_F4_Value.AutoBackWard == 1)
+ {
+ //如启停喷枪按下后判断前进速度到达后，喷漆自动开启
+ AutoBackSpray_On();
+ //喷漆程序结束
+ }
+ }
+ else if((Current_T_F4_Value.AutoForWard != 1 && Current_T_F4_Value.AutoBackWard != 1)
+ &&(Current_T_F4_Value.ChgDown_Ver == 1 || Current_T_F4_Value.ChgUp_Ver == 1
+ || Current_T_F4_Value.ChgLeft_Hor == 1 || Current_T_F4_Value.ChgRight_Hor == 1))
+ {
+ if(GV.Chg_Flag == 1 && (Current_T_F4_Value.ChgDown_Ver == 1 || Current_T_F4_Value.ChgUp_Ver == 1
+ && Current_T_F4_Value.ChgLeft_Hor == 1 || Current_T_F4_Value.ChgRight_Hor == 1))
+ {
+ CurrentMoveState = Move_ChgFinish;
+ }
+ else if(Current_T_F4_Value.ChgUp_Ver == 1
+ && !(Current_T_F4_Value.ChgDown_Ver == 1
+ || Current_T_F4_Value.ChgLeft_Hor == 1
+ || Current_T_F4_Value.ChgRight_Hor == 1))
+ {
+ CurrentMoveState = Move_ChgUp;
+ }
+ else if(Current_T_F4_Value.ChgDown_Ver == 1
+ && !(Current_T_F4_Value.ChgUp_Ver == 1
+ || Current_T_F4_Value.ChgLeft_Hor == 1
+ || Current_T_F4_Value.ChgRight_Hor == 1))
+ {
+ CurrentMoveState = Move_ChgDown;
+ }
+ else if(Current_T_F4_Value.ChgLeft_Hor == 1
+ && !(Current_T_F4_Value.ChgUp_Ver == 1
+ || Current_T_F4_Value.ChgDown_Ver == 1
+ || Current_T_F4_Value.ChgRight_Hor == 1))
+ {
+ CurrentMoveState = Move_ChgLeft;
+ }
+ else if(Current_T_F4_Value.ChgRight_Hor == 1
+ && !(Current_T_F4_Value.ChgUp_Ver == 1
+ || Current_T_F4_Value.ChgDown_Ver == 1
+ || Current_T_F4_Value.ChgLeft_Hor == 1))
+ {
+ CurrentMoveState = Move_ChgRight;
+ }
+ }
+ }
+ else if((Current_T_F4_Value.ChgDown_Ver != 1 && Current_T_F4_Value.ChgUp_Ver != 1
+ && Current_T_F4_Value.ChgLeft_Hor!= 1 && Current_T_F4_Value.ChgRight_Hor != 1
+ && Current_T_F4_Value.AutoForWard != 1 && Current_T_F4_Value.AutoBackWard != 1)
+ && (Current_T_F4_Value.robot_forward_backward_speed < 0X7A //7A 84
+ || Current_T_F4_Value.robot_forward_backward_speed > 0X84
+ || Current_T_F4_Value.robot_left_right_turn_speed < 0X7A
+ || Current_T_F4_Value.robot_left_right_turn_speed > 0X84))
+ {
+ if (Current_T_F4_Value.robot_forward_backward_speed < 0X7A)//前进
+ {
+ CurrentMoveState = Move_Forwards;
+ }
+ else if (Current_T_F4_Value.robot_forward_backward_speed > 0X84)
+ {
+ CurrentMoveState = Move_Backwards; //后退
+ }
+
+ else if (Current_T_F4_Value.robot_left_right_turn_speed < 0X7A)
+ {
+ CurrentMoveState = Move_TurnLeft; //右转
+ }
+ else if (Current_T_F4_Value.robot_left_right_turn_speed > 0X84)
+ {
+ CurrentMoveState = Move_TurnRight; //左转
+ }
+ }
+ else
+ {
+ Off_AutoSpray();
+ }
+}
+
void GF_Dispatch()//2ms调用一次 给车体速度等赋值
{
//--遥控器急停判断
@@ -399,138 +507,44 @@ void GF_Dispatch()//2ms调用一次 给车体速度等赋值
{
Dangerous_Test();
}
- //--
+
//--气压判断
Dym_Test();
- //--
- //运动部分 Move Region
- if((Current_T_F4_Value.ChgDown_Ver == 1 || Current_T_F4_Value.ChgUp_Ver == 1
- || Current_T_F4_Value.ChgLeft_Hor == 1 || Current_T_F4_Value.ChgRight_Hor == 1
- || Current_T_F4_Value.AutoForWard == 1 || Current_T_F4_Value.AutoBackWard == 1)
- && (Current_T_F4_Value.robot_forward_backward_speed >= 0X7A
- && Current_T_F4_Value.robot_forward_backward_speed <= 0X84)
- && (Current_T_F4_Value.robot_left_right_turn_speed >= 0X7A
- && Current_T_F4_Value.robot_left_right_turn_speed <= 0X84)
- && GV.Robot_Ultrasonic.Dym_Value_Flag != 1)
- {
- if((Current_T_F4_Value.AutoForWard == 1 || Current_T_F4_Value.AutoBackWard == 1)
- &&(Current_T_F4_Value.ChgDown_Ver != 1 && Current_T_F4_Value.ChgUp_Ver != 1
- && Current_T_F4_Value.ChgLeft_Hor != 1 && Current_T_F4_Value.ChgRight_Hor != 1))//自动作业与换道互斥
- {
- if(Current_T_F4_Value.AutoForWard == 1)
- {
- //如启停喷枪按下后判断前进速度到达后，喷漆自动开启
- AutoForSpray_On();
- //喷漆程序结束
- }
- else if(Current_T_F4_Value.AutoBackWard == 1)
- {
- //如启停喷枪按下后判断前进速度到达后，喷漆自动开启
- AutoBackSpray_On();
- //喷漆程序结束
- }
- }
- else if((Current_T_F4_Value.AutoForWard != 1 && Current_T_F4_Value.AutoBackWard != 1)
- &&(Current_T_F4_Value.ChgDown_Ver == 1 || Current_T_F4_Value.ChgUp_Ver == 1
- || Current_T_F4_Value.ChgLeft_Hor == 1 || Current_T_F4_Value.ChgRight_Hor == 1))
- {
- if(GV.Chg_Flag == 1 && (Current_T_F4_Value.ChgDown_Ver == 1 || Current_T_F4_Value.ChgUp_Ver == 1
- && Current_T_F4_Value.ChgLeft_Hor == 1 || Current_T_F4_Value.ChgRight_Hor == 1))
- {
- CurrentMoveState = Move_ChgFinish;
- }
- else if(Current_T_F4_Value.ChgUp_Ver == 1
- && !(Current_T_F4_Value.ChgDown_Ver == 1
- || Current_T_F4_Value.ChgLeft_Hor == 1
- || Current_T_F4_Value.ChgRight_Hor == 1))
- {
- CurrentMoveState = Move_ChgUp;
- }
- else if(Current_T_F4_Value.ChgDown_Ver == 1
- && !(Current_T_F4_Value.ChgUp_Ver == 1
- || Current_T_F4_Value.ChgLeft_Hor == 1
- || Current_T_F4_Value.ChgRight_Hor == 1))
- {
- CurrentMoveState = Move_ChgDown;
- }
- else if(Current_T_F4_Value.ChgLeft_Hor == 1
- && !(Current_T_F4_Value.ChgUp_Ver == 1
- || Current_T_F4_Value.ChgDown_Ver == 1
- || Current_T_F4_Value.ChgRight_Hor == 1))
- {
- CurrentMoveState = Move_ChgLeft;
- }
- else if(Current_T_F4_Value.ChgRight_Hor == 1
- && !(Current_T_F4_Value.ChgUp_Ver == 1
- || Current_T_F4_Value.ChgDown_Ver == 1
- || Current_T_F4_Value.ChgLeft_Hor == 1))
- {
- CurrentMoveState = Move_ChgRight;
- }
- }
- }
- else if((Current_T_F4_Value.ChgDown_Ver != 1 && Current_T_F4_Value.ChgUp_Ver != 1
- && Current_T_F4_Value.ChgLeft_Hor!= 1 && Current_T_F4_Value.ChgRight_Hor != 1
- && Current_T_F4_Value.AutoForWard != 1 && Current_T_F4_Value.AutoBackWard != 1)
- && (Current_T_F4_Value.robot_forward_backward_speed < 0X7A //7A 84
- || Current_T_F4_Value.robot_forward_backward_speed > 0X84
- || Current_T_F4_Value.robot_left_right_turn_speed < 0X7A
- || Current_T_F4_Value.robot_left_right_turn_speed > 0X84))
- {
- if (Current_T_F4_Value.robot_forward_backward_speed < 0X7A)//前进
- {
- CurrentMoveState = Move_Forwards;
- }
- else if (Current_T_F4_Value.robot_forward_backward_speed > 0X84)
- {
- CurrentMoveState = Move_Backwards; //后退
- }
-
- else if (Current_T_F4_Value.robot_left_right_turn_speed < 0X7A)
- {
- CurrentMoveState = Move_TurnLeft; //右转
- }
- else if (Current_T_F4_Value.robot_left_right_turn_speed > 0X84)
- {
- CurrentMoveState = Move_TurnRight; //左转
- }
- }
- else
- {
- Off_AutoSpray();
- }
- //--探边置位消除
- Dangerous_Flag_Delete();
- //--
- /****************************************/
- //LU_Three控制升降
- if(Current_T_F4_Value.Lift_Up == 1)
- {
- GF_BSP_GPIO_SetIO(Out_Lift_2,0);
- GF_BSP_GPIO_SetIO(Out_Lift_1,1);
- }
- else if(Current_T_F4_Value.Lift_Down == 1)
- {
- GF_BSP_GPIO_SetIO(Out_Lift_2,1);
- GF_BSP_GPIO_SetIO(Out_Lift_1,0);
- }
- else
- {
- GF_BSP_GPIO_SetIO(Out_Lift_2,1);
- GF_BSP_GPIO_SetIO(Out_Lift_1,1);
- }
+ Robot_Move();
- //控制电磁阀喷枪
- if(Current_T_F4_Value.FA && AutoSpray_Flag == 0) // && CurrentMoveState != Move_AutoForward&& CurrentMoveState != Move_AutoBackward)
- {
- GF_BSP_GPIO_SetIO(Out_Spray,0);
- }
- else if(!Current_T_F4_Value.FA && AutoSpray_Flag == 0) // && CurrentMoveState != Move_AutoForward && CurrentMoveState != Move_AutoBackward)
- {
- GF_BSP_GPIO_SetIO(Out_Spray,1);
- }
+ //--探边置位消除
+ Dangerous_Flag_Delete();
+ //--
+
+ /****************************************/
+ //LU_Three控制升降
+ if(Current_T_F4_Value.Lift_Up == 1)
+ {
+ GF_BSP_GPIO_SetIO(Out_Lift_2,0);
+ GF_BSP_GPIO_SetIO(Out_Lift_1,1);
+ }
+ else if(Current_T_F4_Value.Lift_Down == 1)
+ {
+ GF_BSP_GPIO_SetIO(Out_Lift_2,1);
+ GF_BSP_GPIO_SetIO(Out_Lift_1,0);
+ }
+ else
+ {
+ GF_BSP_GPIO_SetIO(Out_Lift_2,1);
+ GF_BSP_GPIO_SetIO(Out_Lift_1,1);
+ }
+
+ //控制电磁阀喷枪
+ if(Current_T_F4_Value.FA && AutoSpray_Flag == 0) // && CurrentMoveState != Move_AutoForward&& CurrentMoveState != Move_AutoBackward)
+ {
+ GF_BSP_GPIO_SetIO(Out_Spray,0);
+ }
+ else if(!Current_T_F4_Value.FA && AutoSpray_Flag == 0) // && CurrentMoveState != Move_AutoForward && CurrentMoveState != Move_AutoBackward)
+ {
+ GF_BSP_GPIO_SetIO(Out_Spray,1);
+ }
//计算速度档位m/min
GV.Robot_Speed_mpm = Speed_Judge(&Current_T_F4_Value);
@@ -547,7 +561,8 @@ void GF_Dispatch()//2ms调用一次 给车体速度等赋值
GV.Robot_State = CurrentMoveState;
- if(GV.LeftFrontMotor.ERROR_Flag != 0 || GV.RightFrontMotor.ERROR_Flag != 0)
+ if((GV.LeftFrontMotor.ERROR_Flag != 0 || GV.RightFrontMotor.ERROR_Flag != 0)
+ && Current_T_F4_Value.emergency_stop != 1)
{
GV.Robot_State = Move_Error;
}