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.
169 lines
5.5 KiB
169 lines
5.5 KiB
/*
|
|
* msp_DH_Roughening.c
|
|
*
|
|
* Created on: Nov 22, 2024
|
|
* Author: akeguo
|
|
*/
|
|
|
|
#include "msp_DH_Roughening.h"
|
|
#include "bsp_Error_Detect.h"
|
|
#include "robot_state.h"
|
|
#include "BHBF_ROBOT.h"
|
|
DHRoughening DHRougheningContrller; //遥控器数据
|
|
|
|
|
|
|
|
DispacherController *DHRoughening_dispacherController;
|
|
struct UARTHandler *DHRoughening_Controller_UART_Handler;
|
|
|
|
void DHRoughening_Controller_intialize(struct UARTHandler *Handler) {
|
|
|
|
DHRoughening_Controller_UART_Handler = Handler;
|
|
DHRoughening_Controller_UART_Handler->Wait_time = 20; //等待10ms 最低不要低于4；
|
|
DHRoughening_dispacherController = Handler->dispacherController;
|
|
DHRoughening_dispacherController->Dispacher_Enable=1;
|
|
DHRoughening_dispacherController->DispacherCallTime = 200;
|
|
DHRoughening_dispacherController->Add_Dispatcher_List(
|
|
DHRoughening_dispacherController, read_DHRoughening_Data);
|
|
DHRoughening_dispacherController->Add_Dispatcher_List(
|
|
DHRoughening_dispacherController, send_to_DHRoughening);
|
|
|
|
HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,
|
|
"DHRoughening", 0, DHRougheningController);
|
|
|
|
}
|
|
|
|
void read_DHRoughening_Data(void) {
|
|
LOGFF(DL_ERROR, "read data from DeChi");
|
|
|
|
DHRoughening_Controller_UART_Handler->UART_Decode = decode_DHRoughening;
|
|
MB_ReadInputReg(&DHRoughening_Controller_UART_Handler->Tx_Buf,
|
|
&DHRoughening_Controller_UART_Handler->TxCount, 3, 0, 6);
|
|
|
|
DHRoughening_Controller_UART_Handler->UART_Tx(
|
|
DHRoughening_Controller_UART_Handler);
|
|
|
|
}
|
|
uint8_t data5555[20];
|
|
void decode_DHRoughening(uint8_t *buffer, uint16_t length) {
|
|
|
|
memcpy(data5555,buffer,length);
|
|
uint16_t crc_check = ((buffer[length - 1] << 8) | buffer[length - 2]);
|
|
/* CRC 校验正确 */
|
|
if (crc_check == MB_CRC16(buffer, length - 2))
|
|
{
|
|
HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,
|
|
"DHRoughening", 1);
|
|
|
|
memcpy(&DHRougheningContrller,&buffer[3],12);//16个字节数字
|
|
// *Roughenging_LeftCompensation=(*Roughenging_LeftCompensation_Range)*DHRougheningContrller.Left_Compensation*100/65535;
|
|
// *Roughenging_RightCompensation=(*Roughenging_RightCompensation_Range)*DHRougheningContrller.Right_Compensation*100/65535;
|
|
} else
|
|
{
|
|
|
|
}
|
|
|
|
}
|
|
void send_to_DHRoughening()
|
|
{
|
|
|
|
set_DHRoughening_Display((uint16_t) (GV.Left_Compensation * 100),
|
|
(uint16_t) (GV.Right_Compensation / 6.5 * 100),
|
|
(uint16_t) abs(GV.Robot_Angle.RF_Angle_Roll),
|
|
(uint16_t) abs(GV.Robot_ForceValue ));
|
|
|
|
|
|
|
|
}
|
|
|
|
//左补偿 xxx.xx 度 0x0000—0xFFFF==0—655.35 度
|
|
//右补偿 xxx.xx 度 0x0000—0xFFFF==0—655.35 度
|
|
//当前角度 xxx.xx 度 0x0000—0xFFFF==0—655.35 度
|
|
//移动速度 xxx 米/分钟 0x0000—0xFFFF==0—65535 米/分钟
|
|
MB_REG_DATA mb_reg_value_DHRoughening;
|
|
void set_DHRoughening_Display(uint16_t LeftCompensation,
|
|
uint16_t RightCompensation, uint16_t Angle, uint16_t Speed) {
|
|
|
|
|
|
mb_reg_value_DHRoughening.DATA_10H[0] = (LeftCompensation >> 8) & 0xff;
|
|
mb_reg_value_DHRoughening.DATA_10H[1] = (LeftCompensation) & 0xff;
|
|
|
|
mb_reg_value_DHRoughening.DATA_10H[2] = (RightCompensation >> 8) & 0xff;
|
|
mb_reg_value_DHRoughening.DATA_10H[3] = RightCompensation & 0xff;
|
|
|
|
mb_reg_value_DHRoughening.DATA_10H[4] = (Angle >> 8) & 0xff;
|
|
mb_reg_value_DHRoughening.DATA_10H[5] = Angle & 0xff;
|
|
|
|
mb_reg_value_DHRoughening.DATA_10H[6] = (Speed >> 8) & 0xff;
|
|
mb_reg_value_DHRoughening.DATA_10H[7] = Speed & 0xff;
|
|
|
|
DHRoughening_Controller_UART_Handler->UART_Decode = NULL; //表示不需要解析读回来的数据
|
|
|
|
MB_WriteNumHoldingReg(&DHRoughening_Controller_UART_Handler->Tx_Buf,
|
|
&DHRoughening_Controller_UART_Handler->TxCount, 3, 0, 5,
|
|
mb_reg_value_DHRoughening.DATA_10H);
|
|
|
|
//read from the data;
|
|
DHRoughening_Controller_UART_Handler->UART_Tx(
|
|
DHRoughening_Controller_UART_Handler);
|
|
|
|
}
|
|
|
|
int32_t DHRougheningGetSpeed(DHRoughening DHRougheningContrller)
|
|
{
|
|
// if (DHRougheningContrller.Vehicle_Speed1 == 1) {
|
|
// return *DHRoughening_Speed[0];
|
|
// }
|
|
// if (DHRougheningContrller.Vehicle_Speed2 == 1) {
|
|
// return *DHRoughening_Speed[1];
|
|
// }
|
|
// if (DHRougheningContrller.Vehicle_Speed3 == 1) {
|
|
// return *DHRoughening_Speed[2];
|
|
// }
|
|
// if (DHRougheningContrller.Vehicle_Speed4 == 1) {
|
|
// return *DHRoughening_Speed[3];
|
|
// }
|
|
// if (DHRougheningContrller.Vehicle_Speed5 == 1) {
|
|
// return *DHRoughening_Speed[4];
|
|
// }
|
|
// if (DHRougheningContrller.Vehicle_Speed6 == 1) {
|
|
// return *DHRoughening_Speed[5];
|
|
// }
|
|
// if (DHRougheningContrller.Vehicle_Speed7 == 1) {
|
|
// return *DHRoughening_Speed[6];
|
|
// }
|
|
// if (DHRougheningContrller.Vehicle_Speed8 == 1) {
|
|
// return *DHRoughening_Speed[7];
|
|
// }
|
|
// if (DHRougheningContrller.Vehicle_Speed9 == 1) {
|
|
// return *DHRoughening_Speed[8];
|
|
// }
|
|
// if (DHRougheningContrller.Vehicle_Speed10 == 1) {
|
|
// return *DHRoughening_Speed[9];
|
|
// }
|
|
return 0;
|
|
}
|
|
|
|
int32_t DHRougheningGetLaneChangeDistance(DHRoughening DHRougheningContrller) {
|
|
|
|
// if (DHRougheningContrller.LaneChangeDistance_0 == 1) {
|
|
// return *DHRoughening_LaneChangeDistance[0];
|
|
// }
|
|
//
|
|
// if (DHRougheningContrller.LaneChangeDistance_1 == 1) {
|
|
// return *DHRoughening_LaneChangeDistance[1];
|
|
// }
|
|
// if (DHRougheningContrller.LaneChangeDistance_2 == 1) {
|
|
// return *DHRoughening_LaneChangeDistance[2];
|
|
// }
|
|
// if (DHRougheningContrller.LaneChangeDistance_3 == 1) {
|
|
// return *DHRoughening_LaneChangeDistance[3];
|
|
// }
|
|
// if (DHRougheningContrller.LaneChangeDistance_4 == 1) {
|
|
// return *DHRoughening_LaneChangeDistance[4];
|
|
// }
|
|
// if (DHRougheningContrller.LaneChangeDistance_5 == 1) {
|
|
// return *DHRoughening_LaneChangeDistance[5];
|
|
// }
|
|
return 0;
|
|
}
|
|
|