YuLin_Zhen
/
FW3.2
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

新增漆膜测厚的序列号；在校验之后再判断新数据；急停信号的UDP正确上传

3.2
LIN\54376 2 months ago
parent
9314843ef2
commit
a124f81105
4 changed files with 181 additions and 147 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 4
      .settings/language.settings.xml
  2. 30
      BASE/Src/BSP/bsp_client_setting.c
  3. 23
      BASE/Src/MSP/msp_PaintThickness.c
  4. 271
      Core/Src/FSM.c

4
.settings/language.settings.xml
View File

@ -5,7 +5,7 @@
<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="1328381156250390778" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="-48266367591953008" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
<language-scope id="org.eclipse.cdt.core.gcc"/>
<language-scope id="org.eclipse.cdt.core.g++"/>
</provider>
@ -16,7 +16,7 @@
<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="1328381156250390778" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="-48266367591953008" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
<language-scope id="org.eclipse.cdt.core.gcc"/>
<language-scope id="org.eclipse.cdt.core.g++"/>
</provider>

30
BASE/Src/BSP/bsp_client_setting.c
View File

@ -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;
}

23
BASE/Src/MSP/msp_PaintThickness.c
View File

@ -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++)

271
Core/Src/FSM.c
View File

@ -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;
}

Write Preview
Loading…
Cancel
Save