diff --git a/AUTO_CHASSIS.ioc b/AUTO_CHASSIS.ioc
index c075c1e..6f7d7c5 100644
--- a/AUTO_CHASSIS.ioc
+++ b/AUTO_CHASSIS.ioc
@@ -28,7 +28,8 @@ Dma.Request2=USART3_RX
Dma.Request3=USART6_RX
Dma.Request4=USART1_TX
Dma.Request5=USART1_RX
-Dma.RequestsNb=6
+Dma.Request6=USART6_TX
+Dma.RequestsNb=7
Dma.SPI1_RX.0.Direction=DMA_PERIPH_TO_MEMORY
Dma.SPI1_RX.0.FIFOMode=DMA_FIFOMODE_DISABLE
Dma.SPI1_RX.0.Instance=DMA2_Stream2
@@ -89,6 +90,16 @@ Dma.USART6_RX.3.PeriphDataAlignment=DMA_PDATAALIGN_BYTE
Dma.USART6_RX.3.PeriphInc=DMA_PINC_DISABLE
Dma.USART6_RX.3.Priority=DMA_PRIORITY_VERY_HIGH
Dma.USART6_RX.3.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode
+Dma.USART6_TX.6.Direction=DMA_MEMORY_TO_PERIPH
+Dma.USART6_TX.6.FIFOMode=DMA_FIFOMODE_DISABLE
+Dma.USART6_TX.6.Instance=DMA2_Stream6
+Dma.USART6_TX.6.MemDataAlignment=DMA_MDATAALIGN_BYTE
+Dma.USART6_TX.6.MemInc=DMA_MINC_ENABLE
+Dma.USART6_TX.6.Mode=DMA_NORMAL
+Dma.USART6_TX.6.PeriphDataAlignment=DMA_PDATAALIGN_BYTE
+Dma.USART6_TX.6.PeriphInc=DMA_PINC_DISABLE
+Dma.USART6_TX.6.Priority=DMA_PRIORITY_LOW
+Dma.USART6_TX.6.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode
FREERTOS.INCLUDE_pcTaskGetTaskName=1
FREERTOS.INCLUDE_uxTaskGetStackHighWaterMark2=1
FREERTOS.INCLUDE_vTaskCleanUpResources=1
@@ -178,6 +189,7 @@ NVIC.DMA2_Stream1_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
NVIC.DMA2_Stream2_IRQn=true\:5\:0\:true\:false\:true\:false\:false\:true\:true
NVIC.DMA2_Stream3_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
NVIC.DMA2_Stream5_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
+NVIC.DMA2_Stream6_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
NVIC.DMA2_Stream7_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
NVIC.EXTI0_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
diff --git a/Core/Inc/stm32f4xx_it.h b/Core/Inc/stm32f4xx_it.h
index 2c650e5..de1767c 100644
--- a/Core/Inc/stm32f4xx_it.h
+++ b/Core/Inc/stm32f4xx_it.h
@@ -69,6 +69,7 @@ void CAN2_RX0_IRQHandler(void);
void CAN2_RX1_IRQHandler(void);
void OTG_FS_IRQHandler(void);
void DMA2_Stream5_IRQHandler(void);
+void DMA2_Stream6_IRQHandler(void);
void DMA2_Stream7_IRQHandler(void);
void USART6_IRQHandler(void);
/* USER CODE BEGIN EFP */
diff --git a/Core/Src/dma.c b/Core/Src/dma.c
index 4c8e202..16a09df 100644
--- a/Core/Src/dma.c
+++ b/Core/Src/dma.c
@@ -59,6 +59,9 @@ void MX_DMA_Init(void)
/* DMA2_Stream5_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA2_Stream5_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(DMA2_Stream5_IRQn);
+ /* DMA2_Stream6_IRQn interrupt configuration */
+ HAL_NVIC_SetPriority(DMA2_Stream6_IRQn, 5, 0);
+ HAL_NVIC_EnableIRQ(DMA2_Stream6_IRQn);
/* DMA2_Stream7_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA2_Stream7_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(DMA2_Stream7_IRQn);
diff --git a/Core/Src/stm32f4xx_it.c b/Core/Src/stm32f4xx_it.c
index 05c6ecb..c681352 100644
--- a/Core/Src/stm32f4xx_it.c
+++ b/Core/Src/stm32f4xx_it.c
@@ -69,6 +69,7 @@ extern DMA_HandleTypeDef hdma_usart1_tx;
extern DMA_HandleTypeDef hdma_usart1_rx;
extern DMA_HandleTypeDef hdma_usart3_rx;
extern DMA_HandleTypeDef hdma_usart6_rx;
+extern DMA_HandleTypeDef hdma_usart6_tx;
extern UART_HandleTypeDef huart1;
extern UART_HandleTypeDef huart6;
/* USER CODE BEGIN EV */
@@ -418,6 +419,20 @@ void DMA2_Stream5_IRQHandler(void)
/* USER CODE END DMA2_Stream5_IRQn 1 */
}
+/**
+ * @brief This function handles DMA2 stream6 global interrupt.
+ */
+void DMA2_Stream6_IRQHandler(void)
+{
+ /* USER CODE BEGIN DMA2_Stream6_IRQn 0 */
+
+ /* USER CODE END DMA2_Stream6_IRQn 0 */
+ HAL_DMA_IRQHandler(&hdma_usart6_tx);
+ /* USER CODE BEGIN DMA2_Stream6_IRQn 1 */
+
+ /* USER CODE END DMA2_Stream6_IRQn 1 */
+}
+
/**
* @brief This function handles DMA2 stream7 global interrupt.
*/
diff --git a/Core/Src/usart.c b/Core/Src/usart.c
index 8baef17..f133fad 100644
--- a/Core/Src/usart.c
+++ b/Core/Src/usart.c
@@ -31,6 +31,7 @@ DMA_HandleTypeDef hdma_usart1_tx;
DMA_HandleTypeDef hdma_usart1_rx;
DMA_HandleTypeDef hdma_usart3_rx;
DMA_HandleTypeDef hdma_usart6_rx;
+DMA_HandleTypeDef hdma_usart6_tx;
/* USART1 init function */
@@ -278,6 +279,24 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle)
__HAL_LINKDMA(uartHandle,hdmarx,hdma_usart6_rx);
+ /* USART6_TX Init */
+ hdma_usart6_tx.Instance = DMA2_Stream6;
+ hdma_usart6_tx.Init.Channel = DMA_CHANNEL_5;
+ hdma_usart6_tx.Init.Direction = DMA_MEMORY_TO_PERIPH;
+ hdma_usart6_tx.Init.PeriphInc = DMA_PINC_DISABLE;
+ hdma_usart6_tx.Init.MemInc = DMA_MINC_ENABLE;
+ hdma_usart6_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
+ hdma_usart6_tx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
+ hdma_usart6_tx.Init.Mode = DMA_NORMAL;
+ hdma_usart6_tx.Init.Priority = DMA_PRIORITY_LOW;
+ hdma_usart6_tx.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
+ if (HAL_DMA_Init(&hdma_usart6_tx) != HAL_OK)
+ {
+ Error_Handler();
+ }
+
+ __HAL_LINKDMA(uartHandle,hdmatx,hdma_usart6_tx);
+
/* USART6 interrupt Init */
HAL_NVIC_SetPriority(USART6_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(USART6_IRQn);
@@ -352,6 +371,7 @@ void HAL_UART_MspDeInit(UART_HandleTypeDef* uartHandle)
/* USART6 DMA DeInit */
HAL_DMA_DeInit(uartHandle->hdmarx);
+ HAL_DMA_DeInit(uartHandle->hdmatx);
/* USART6 interrupt Deinit */
HAL_NVIC_DisableIRQ(USART6_IRQn);
diff --git a/MDK-ARM/AUTO_CHASSIS.uvoptx b/MDK-ARM/AUTO_CHASSIS.uvoptx
index 564be7f..8492b49 100644
--- a/MDK-ARM/AUTO_CHASSIS.uvoptx
+++ b/MDK-ARM/AUTO_CHASSIS.uvoptx
@@ -158,32 +158,27 @@
0
1
- UP,0x0A
+ GO_motor_info
1
1
- GO_motor_info
+ aaa
2
1
- dr16,0x0A
+ UP,0x0A
3
1
- cmd,0x0A
+ aaa
4
1
- rc_ctrl,0x0A
-
-
- 5
- 1
- aaa
+ motor_send_data,0x0A
diff --git a/MDK-ARM/ELE_CHASSIS/AUTO_CHASSIS.axf b/MDK-ARM/ELE_CHASSIS/AUTO_CHASSIS.axf
deleted file mode 100644
index 9794aa3..0000000
Binary files a/MDK-ARM/ELE_CHASSIS/AUTO_CHASSIS.axf and /dev/null differ
diff --git a/User/Module/config.c b/User/Module/config.c
index 26fb7e9..018fbc1 100644
--- a/User/Module/config.c
+++ b/User/Module/config.c
@@ -66,29 +66,19 @@ static const ConfigParam_t param_chassis ={
.out_limit = 3000.0f
},
-.go_param[0]={
- .rev = 0,
- .T=0.79,
- .W=0.1,
- .K_P=0.2,
- .K_W=0.1,
-},
-.go_param[1]={
- .rev = 0,
- .T=0,
- .W=0,
- .K_P=1.0f,
- .K_W=0.1,
-},
-.go1_position_param={
- .p =0.1f,
- .i =0.1f,
- .d =0.0f,
- .i_limit = 2.0f,
- .out_limit = 10.0f
+.go_cmd={
+ .id =0,
+ .mode = 1,
+ .K_P = 1.0f,
+ .K_W = 0.05,
+ .Pos = 0,
+ .W = 0,
+ .T = 0,
},
+
+
/*上层其他参数*/
/*运球*/
.DribbleConfig_Config = {
@@ -103,8 +93,8 @@ static const ConfigParam_t param_chassis ={
.PitchConfig_Config = {
.m2006_init_angle =-150,
.m2006_trigger_angle =0,
- .go1_init_position = -500,
- .go1_release_threshold =-1900,
+ .go1_init_position = 0,
+ .go1_release_threshold =0,
.m2006_Screw_init=0
},
diff --git a/User/Module/up.c b/User/Module/up.c
index a50dc90..2a137a6 100644
--- a/User/Module/up.c
+++ b/User/Module/up.c
@@ -19,8 +19,7 @@
int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
{
u->param = param; /*初始化参数 */
- /*go电机初始化*/
- GO_M8010_init();
+
/*pid初始化*/
PID_init (&u->pid.VESC_5065_M1 ,PID_POSITION ,&(u->param ->VESC_5065_M1_param ));
PID_init (&u->pid.VESC_5065_M2 ,PID_POSITION ,&(u->param ->VESC_5065_M2_param ));
@@ -33,12 +32,8 @@ int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
PID_init (&u->pid .GM6020_speed,PID_POSITION ,&(u->param ->UP_GM6020_speed_param ));
PID_init (&u->pid .GM6020_angle,PID_POSITION ,&(u->param ->UP_GM6020_angle_param ));
-
- PID_init (&u->pid .GO1_position ,PID_POSITION,&(u->param->go1_position_param ));
-// for(int i=0;i<2;i++){ //go初始位置设置为0
-// GO_M8010_send_data(BSP_UART_GetHandle(BSP_UART_RS485), i,u->param->go_param[i] .rev ,u->param->go_param[i] .T ,u->param->go_param[i] .W ,0,u->param->go_param [i].K_P ,u->param->go_param[i] .K_W );
-// }
-
+
+ u->go_cmd =u->param ->go_cmd ;
// 初始化上层状态机
if (!u->DribbleContext .is_initialized) { //检查是否为第一次运行状态机，运球
@@ -54,14 +49,14 @@ int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
u->PitchContext .is_initialized = 1;
}
+ BSP_UART_RegisterCallback(BSP_UART_RS485, BSP_UART_RX_CPLT_CB, USART6_RxCompleteCallback);//注册串口回调函数，bsp层
}
/*can，上层状态更新*/
int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) {
- for(int i=0;i<2;i++){ //go初始位置设置为0
- u->motorfeedback .GO_motor_info [i] = getGoPoint(i);
- }
+
+ u->motorfeedback .go_data = get_GO_measure_point() ;
u->motorfeedback .M2006 .motor =M2006;
u->motorfeedback .M3508 .motor =M3508;
@@ -194,19 +189,40 @@ int8_t GM6020_control(UP_t *u,fp32 angle)
}
-/*go电机控制*/
-int8_t GO_SendData(UP_t *u,int id,float pos)
-{
- fp32 deal_pos;
-
-// deal_pos = PID_calc (&u->pid .GO1_position ,u->motorfeedback .GO_motor_info [0]->Pos ,pos);
- GO_M8010_send_data(BSP_UART_GetHandle(BSP_UART_RS485), id,u->param->go_param[id] .rev ,u->param->go_param[id].T ,u->param->go_param[id] .W ,pos,u->param->go_param [id].K_P ,u->param->go_param[id] .K_W );
+/*go电机控制*/
+
+int8_t GO_SendData(UP_t *u,float pos,float limit)
+{
+ u->go_cmd .Pos =pos;
+ // 读取参数
+ float tff = u->go_cmd.T ; // 前馈力矩
+ float kp = u->go_cmd .K_P; // 刚度
+ float kd = u->go_cmd.K_W; // 阻尼（原代码中kdd/kid混淆，需统一为K_W）
+ float q_desired = u->go_cmd.Pos; // 期望位置（rad）
+ float q_current = u->motorfeedback.go_data ->Pos ; // 当前位置（rad）
+ float dq_desired = u->go_cmd.W; // 期望速度（rad/s）
+ float dq_current = u->motorfeedback.go_data ->W ; // 当前速度（rad/s）
+
+ float tau = tff + kp * (q_desired - q_current) + kd * (dq_desired - dq_current);
+
+
+
+ if (tau > limit) {
+
+ tff = limit - kp * (q_desired - q_current) - kd * (dq_desired - dq_current) - (tau - limit) / 2.0;
+ tau = limit;
+ } else if (tau < -limit) {
+
+ tff = limit - kp * (q_desired - q_current) - kd * (dq_desired - dq_current) + (tau - limit) / 2.0;
+ tau = -limit;
+ }
+ u->go_cmd .T =tff;
+ GO_M8010_send_data(&u->go_cmd );
+ u->go_cmd .T =0;
+
return 0;
-
}
-
-
int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
{
//电机控制 ，传进can
@@ -216,7 +232,7 @@ int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
- GO_SendData(u,0 ,u->motor_target .go_shoot );
+ GO_SendData(u,u->motor_target .go_shoot,0.08 );
for(int i=0;i<4;i++){
@@ -338,11 +354,11 @@ int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c)
case PITCH_START:
u->motor_target .go_shoot = u->PitchContext .PitchConfig .go1_release_threshold;
- if(u->motorfeedback .GO_motor_info[0]->Pos < -32.40){ //检测go位置到达最上面，这里的检测条件可以更改
- u->motor_target .M2006_angle = u->PitchContext .PitchConfig .m2006_trigger_angle ;//设置2006角度，0
+// if(u->motorfeedback .GO_motor_info[0]->Pos < -32.40){ //检测go位置到达最上面，这里的检测条件可以更改
+// u->motor_target .M2006_angle = u->PitchContext .PitchConfig .m2006_trigger_angle ;//设置2006角度，0
u->PitchContext .PitchState=PITCH_PULL_TRIGGER;
-}//更改标志位
+//}//更改标志位
break ;
case PITCH_PULL_TRIGGER:
@@ -413,10 +429,10 @@ int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
RELAY2_TOGGLE (1);
- if(((u->motorfeedback.GO_motor_info[1]->Pos ) <-1.1)&&(u->motorfeedback.M3508 .total_angle >1190)){
-
- u->DribbleContext .DribbleState = STATE_RELEASE_BALL; //当go2到标准位置，标志位改变
- }
+// if(((u->motorfeedback.GO_motor_info[1]->Pos ) <-1.1)&&(u->motorfeedback.M3508 .total_angle >1190)){
+//
+// u->DribbleContext .DribbleState = STATE_RELEASE_BALL; //当go2到标准位置，标志位改变
+// }
}
break;
@@ -428,20 +444,20 @@ int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
- if((u->motorfeedback.GO_motor_info[1]->Pos ) <-1.3){
- u->motor_target.go_spin =-40; //恢复go2位置
- u->DribbleContext .DribbleState = STATE_CATCH_PREP; //当go2到标准位置，标志位改变
+// if((u->motorfeedback.GO_motor_info[1]->Pos ) <-1.3){
+// u->motor_target.go_spin =-40; //恢复go2位置
+// u->DribbleContext .DribbleState = STATE_CATCH_PREP; //当go2到标准位置，标志位改变
- }
+// }
break;
case STATE_CATCH_PREP:
- if((u->motorfeedback.GO_motor_info[1]->Pos )> -0.4){
- u->motor_target.M3508_angle =0 ; //当go2到初始位置，3508降
-
- RELAY2_TOGGLE (0);//接球
- }
+// if((u->motorfeedback.GO_motor_info[1]->Pos )> -0.4){
+// u->motor_target.M3508_angle =0 ; //当go2到初始位置，3508降
+//
+// RELAY2_TOGGLE (0);//接球
+// }
if(u->motorfeedback .M3508 .total_angle <51.0f){
RELAY1_TOGGLE (0);//夹球，0夹1开
@@ -477,15 +493,15 @@ void Dribbl_transfer_a(UP_t *u, CAN_Output_t *out)
u->motor_target .go_spin =u->DribbleContext .DribbleConfig .go2_release_threshold ;
}
- if(u->motorfeedback .GO_motor_info [1]->Pos < -4.9)
- {
- RELAY1_TOGGLE (1);//夹球，0夹1开
-
- if(u->motorfeedback .GO_motor_info [1]->Pos > -4.8)
- {
- u->DribbleContext .DribbleState = STATE_CATCH_DONE; //当go2到标准位置，标志位改变
- }
- }
+// if(u->motorfeedback .GO_motor_info [1]->Pos < -4.9)
+// {
+// RELAY1_TOGGLE (1);//夹球，0夹1开
+//
+// if(u->motorfeedback .GO_motor_info [1]->Pos > -4.8)
+// {
+// u->DribbleContext .DribbleState = STATE_CATCH_DONE; //当go2到标准位置，标志位改变
+// }
+// }
break ;
diff --git a/User/Module/up.h b/User/Module/up.h
index 65bed8e..70be9d9 100644
--- a/User/Module/up.h
+++ b/User/Module/up.h
@@ -154,11 +154,10 @@ typedef struct
pid_param_t M3508_speed_param;
pid_param_t M3508_angle_param;
- GO_param_t go_param[2];
- pid_param_t go1_position_param;
-
DribbleConfig_t DribbleConfig_Config;
PitchConfig_t PitchConfig_Config;
+
+ GO_MotorCmd_t go_cmd;
}UP_Param_t;
typedef struct
@@ -205,7 +204,7 @@ typedef struct{
motor_angle_data M2006;
motor_angle_data M3508;
- GO_Motorfield *GO_motor_info[GO_NUM];//存放go电机数据
+ const GO_MotorData_t *go_data;//存放go电机数据
fp32 M3508_angle[4];
fp32 M3508_rpm [4];
@@ -245,11 +244,11 @@ typedef struct{
pid_type_def M3508_angle;
pid_type_def M3508_speed;
- pid_type_def GO1_position;
-
}pid;
-
-
+ GO_MotorCmd_t go_cmd;
+
+
+
/*经PID计算后的实际发送给电机的实时输出值*/
struct
@@ -276,7 +275,7 @@ int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq);
int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) ;
int8_t GM6020_control(UP_t *u,fp32 angle);
int8_t VESC_M5065_Control(UP_t *u,fp32 speed);
-int8_t GO_SendData(UP_t *u,int id,float pos);
+int8_t GO_SendData(UP_t *u,float pos,float limit);
int8_t UP_angle_control(UP_t *u, fp32 target_angle,MotorType_t motor);
int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c);
int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out);
diff --git a/User/bsp/crc16.c b/User/bsp/crc16.c
index 5cbd9ab..87a9ada 100644
--- a/User/bsp/crc16.c
+++ b/User/bsp/crc16.c
@@ -39,6 +39,12 @@ uint16_t CRC16_Calc(const uint8_t *buf, size_t len, uint16_t crc) {
while (len--) crc = CRC16_Byte(crc, *buf++);
return crc;
}
+//uint16_t crc_ccitt(uint16_t crc, uint8_t const *buffer, size_t len)
+//{
+// while (len--)
+// crc = crc_ccitt_byte(crc, *buffer++);
+// return crc;
+//}
bool CRC16_Verify(const uint8_t *buf, size_t len) {
if (len < 2) return false;
diff --git a/User/device/GO_M8010_6_Driver.c b/User/device/GO_M8010_6_Driver.c
index d95ea21..4bff220 100644
--- a/User/device/GO_M8010_6_Driver.c
+++ b/User/device/GO_M8010_6_Driver.c
@@ -5,200 +5,109 @@
#include "usart.h"
#include "bsp_usart.h"
#include
+#include "crc16.h"
+#include
- GO_Motorfield GO_motor_info[GO_NUM];//存放电机数据
-static const float gravit_const =9.81;//计算前馈力矩有关
-
-//数据处理函数
-static void GO_M8010_recv_data(uint8_t* Temp_buffer);
-/**
- *@brief 电机初始化
- */
-void GO_M8010_init (void){
- for (uint8_t id= 0; id = (_MAX)) \
+ (_IN) = (_MAX); \
+ }
+GO_MotorCmd_t cmd_gogogo ;
+RIS_ControlData_t motor_send_data;
+
+
+GO_MotorData_t data = {0}; // 确保 data 定义在此处
+uint8_t rx_buffer[sizeof(data.motor_recv_data)] = {0}; // 确保 rx_buffer 定义在此处
-//暂存接收数据
-uint8_t Temp_buffer[16];
+
+
+
+
+/// @brief 将发送给电机的浮点参数转换为定点类型参数
+/// @param motor_s 要转换的电机指令结构体
+void modify_data(GO_MotorCmd_t *motor_s)
+{
+ motor_send_data.head[0] = 0xFE;
+ motor_send_data.head[1] = 0xEE;
+
+ SATURATE(motor_s->id, 0, 15);
+ SATURATE(motor_s->mode, 0, 7);
+ SATURATE(motor_s->K_P, 0.0f, 25.599f);
+ SATURATE(motor_s->K_W, 0.0f, 25.599f);
+ SATURATE(motor_s->T, -127.99f, 127.99f);
+ SATURATE(motor_s->W, -804.00f, 804.00f);
+ SATURATE(motor_s->Pos, -411774.0f, 411774.0f);
+
+ motor_send_data.mode.id = motor_s->id;
+ motor_send_data.mode.status = motor_s->mode;
+ motor_send_data.comd.k_pos = motor_s->K_P / 25.6f * 32768.0f;
+ motor_send_data.comd.k_spd = motor_s->K_W / 25.6f * 32768.0f;
+ motor_send_data.comd.pos_des = motor_s->Pos / 6.28318f * 32768.0f;
+ motor_send_data.comd.spd_des = motor_s->W / 6.28318f * 256.0f;
+ motor_send_data.comd.tor_des = motor_s->T * 256.0f;
+ motor_send_data.CRC16 = CRC16_Calc( (uint8_t *)&motor_send_data, sizeof(RIS_ControlData_t) - sizeof(motor_send_data.CRC16),0);
+}
+
+/// @brief 将接收到的定点类型原始数据转换为浮点参数类型
+/// @param motor_r 要转换的电机反馈结构体
+void extract_data(GO_MotorData_t *motor_r)
+{
+ if (motor_r->motor_recv_data.head[0] != 0xFD || motor_r->motor_recv_data.head[1] != 0xEE)
+ {
+ motor_r->correct = 0;
+ return;
+ }
+ motor_r->calc_crc = CRC16_Calc((uint8_t *)&motor_r->motor_recv_data, sizeof(RIS_MotorData_t) - sizeof(motor_r->motor_recv_data.CRC16),0);
+ if (motor_r->motor_recv_data.CRC16 != motor_r->calc_crc)
+ {
+ memset(&motor_r->motor_recv_data, 0, sizeof(RIS_MotorData_t));
+ motor_r->correct = 0;
+ motor_r->bad_msg++;
+ return;
+ }
+ else
+ {
+ motor_r->motor_id = motor_r->motor_recv_data.mode.id;
+ motor_r->mode = motor_r->motor_recv_data.mode.status;
+ motor_r->Temp = motor_r->motor_recv_data.fbk.temp;
+ motor_r->MError = motor_r->motor_recv_data.fbk.MError;
+ motor_r->W = ((float)motor_r->motor_recv_data.fbk.speed / 256.0f) * 6.28318f;
+ motor_r->T = ((float)motor_r->motor_recv_data.fbk.torque) / 256.0f;
+ motor_r->Pos = 6.28318f * ((float)motor_r->motor_recv_data.fbk.pos) / 32768.0f;
+ motor_r->footForce = motor_r->motor_recv_data.fbk.force;
+ motor_r->correct = 1;
+ return;
+ }
+}
+void GO_M8010_send_data(GO_MotorCmd_t *cmd)
+{
+ modify_data(cmd);
+
+ HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_RS485), (uint8_t *)(&motor_send_data), sizeof(motor_send_data));
+ osDelay (1);
+ HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_RS485), rx_buffer, sizeof(rx_buffer));
+
+}
void USART6_RxCompleteCallback(void )
+{
+
+ UART_HandleTypeDef *huart6 = BSP_UART_GetHandle(BSP_UART_RS485);
+ uint16_t crc = CRC16_Calc(rx_buffer,sizeof(rx_buffer)-2,0x0000);
+
+ memcpy(&data.motor_recv_data, rx_buffer, sizeof(data.motor_recv_data)); // Copy received data
+ extract_data(&data);
+}
+const GO_MotorData_t *get_GO_measure_point(void)
{
- UART_HandleTypeDef *huart6 = BSP_UART_GetHandle(BSP_UART_RS485);
+ return &data;
+}
+void gogogo(void )
+{
+
- uint16_t crc = CRC16_Calc(Temp_buffer,sizeof(Temp_buffer)-2,0x0000);
- if ((Temp_buffer[14] != (crc&0xFF)) || (Temp_buffer[15] != ((crc>>8) & 0xFF)))
- {
-// HAL_GPIO_WritePin(LED_B_GPIO_Port, LED_B_Pin, GPIO_PIN_SET); //蓝色灯亮
-// HAL_GPIO_WritePin(LED_R_GPIO_Port, LED_R_Pin, GPIO_PIN_RESET); //红色灯灭
- return;
- }
-// HAL_GPIO_WritePin(GPIOH, GPIO_PIN_11, GPIO_PIN_SET); // indicate CRC correct
-// HAL_GPIO_WritePin(LED_R_GPIO_Port, LED_R_Pin, GPIO_PIN_RESET); //红色灯灭
- GO_M8010_recv_data(Temp_buffer);
-
-}
-
-
-/**
- *@brief 用户自定义串口发送完成中断回调函数
- */
-void uartTxCB(UART_HandleTypeDef *huart)
-{
- //发送完成后开启接受dma，并将传输完成中断回调函数指向串口接收完成中断回调函数
- HAL_UART_Receive_DMA(huart,Temp_buffer,sizeof(Temp_buffer));
-}
-
-/**
- *@brief go发送控制命令
- *@input[in] huart 所用串口指针
- *@input[in] id 被控电机id
- *@input[in] rev 暂时不知道干啥用的，github为回答issue
- *@input[in] T 力矩，单位N·m
- *@input[in] Pos 目标位置，单位rad
- *@input[in] W 目标速度，单位rad/s
- *@input[in] K_P 位置环环kp
- *@input[in] K_W 速度环kp
- *@note 控制公式 ： t = T + （设定位置 - 实际位置）*K_P + （设定速度 - 实际速度）*K_W
- */
-
-void GO_M8010_send_data(UART_HandleTypeDef *huart, int id,int rev,float T,float Pos,
- float W,float K_P,float K_W)
-{
- GO_Motorfield* motor;
- // a pointer to target motor
- motor = GO_motor_info+id;
-
- //这个rev是干啥的
- // send preious cmd
- if (rev==1){
- HAL_UART_Transmit_IT(huart,motor->buffer,sizeof(motor->buffer));
- return;
- }
-
- // assign motor target goal to the buffer
- motor->motor_send_data.head[0]=0xFE;
- motor->motor_send_data.head[1]=0xEE;
-
- motor->motor_send_data.mode.id = motor->id & 0xF;
- motor->motor_send_data.mode.status = motor->mode & 0x7;
- motor->motor_send_data.mode.none = 0x0;
- motor->motor_send_data.comd.tor_des = T*256;
-
- motor->motor_send_data.comd.spd_des = (W*256*6.33f)/(6.28319f);
- motor->motor_send_data.comd.pos_des = (Pos*32768*6.33f)/(6.28319f);
- motor->motor_send_data.comd.k_pos = K_P*1280;
- motor->motor_send_data.comd.k_spd = K_W*1280;
-
- uint8_t* mode = (uint8_t *)&(motor->motor_send_data.mode);
- motor->buffer[2] = *mode;
- motor->buffer[3] = (motor->motor_send_data.comd.tor_des) & 0xFF;
- motor->buffer[4] = (motor->motor_send_data.comd.tor_des>>8) & 0xFF;
- motor->buffer[5] = (motor->motor_send_data.comd.spd_des) & 0xFF;
- motor->buffer[6] = (motor->motor_send_data.comd.spd_des>>8) & 0xFF;
- motor->buffer[7] = (motor->motor_send_data.comd.pos_des) & 0xFF;
- motor->buffer[8] = (motor->motor_send_data.comd.pos_des>>8) & 0xFF;
- motor->buffer[9] = (motor->motor_send_data.comd.pos_des>>16) & 0xFF;
- motor->buffer[10] = (motor->motor_send_data.comd.pos_des>>24) & 0xFF;
- motor->buffer[11] = (motor->motor_send_data.comd.k_pos) & 0xFF;
- motor->buffer[12] = (motor->motor_send_data.comd.k_pos>>8) & 0xFF;
- motor->buffer[13] = (motor->motor_send_data.comd.k_spd) & 0xFF;
- motor->buffer[14] = (motor->motor_send_data.comd.k_spd>>8) & 0xFF;
-
- //crc calulation
- uint16_t crc = CRC16_Calc(motor->buffer, sizeof(motor->buffer)-2,0x0000);
- motor->buffer[15] = (crc) & 0xFF;
- motor->buffer[16] = (crc>>8) & 0xFF;
-
- // interrupt buffer sending
- HAL_UART_Transmit_IT(huart,motor->buffer,sizeof(motor->buffer));
- osDelay(1);
- HAL_UART_Receive_DMA(huart, Temp_buffer, 16);//有无都可以
-
-
-}
-
-//数据处理函数
-void GO_M8010_recv_data(uint8_t* Temp_buffer)
-{
- // check for ID
- int8_t ID=-1;
- GO_Motorfield* motor;
- ID = Temp_buffer[2] & 0xF;
-
- // a pointer to target pointer
- motor = GO_motor_info + ID;
- memcpy(motor->Rec_buffer,Temp_buffer,16);
-
- // assign buffer to the target motor
- motor->mode = Temp_buffer[2]>>4 & 0xF;
- motor->correct = 1;
- motor->MError = 0;
- int16_t T = Temp_buffer[4]<<8 | Temp_buffer[3];
- motor->T = T;
- motor->T /= 256;
- int16_t W = Temp_buffer[6]<<8 | Temp_buffer[5];
- motor->W = W;
- motor->W = (motor->W * 6.28319f )/(256*6.33f);
- int32_t Pos = (Temp_buffer[7] << 0) |
- (Temp_buffer[8] << 8) |
- (Temp_buffer[9] << 16) |
- (Temp_buffer[10] << 24);
- if(Pos > 0x7FFFFFFF) Pos -= 0xFFFFFFFF; // 溢出修正
- motor->Pos = ((float)Pos / 32768.0f) * (6.28319f / 6.33f);
- int8_t Temp = Temp_buffer[11] & 0xFF;
- motor->Temp = Temp;
- motor->MError = Temp_buffer[12] & 0x7;
-
-}
-
-//力位混合控制
-// for differnt robot should have different define. Now just for testing first
-/**
- *@brief 力位混合控制
- *@input[in] huart 所用串口指针
- *@input[in] id 被控电机id
- *@input[in] bias 前馈力矩
- *@input[in] length 机械臂长度
- *@input[in] mass 机械臂质量
- *@input[in] tar_pos 目标位置
- *@input[in] tar_w 目标速度
- *@input[in] K_P 位置环kp
- *@input[in] K_W 速度环kp
- */
-void basic_ForceControl (UART_HandleTypeDef *huart, int id, float bias, float length, float mass,
- float tar_pos, float tar_w, float K_P,float K_W)
-{
- float forw_T = mass* gravit_const* length* (cos(GO_motor_info[id].Pos));//计算前馈力矩
- forw_T += bias;
- GO_M8010_send_data(huart,id,0,forw_T,tar_pos,tar_w,K_P,K_W);
-}
-
-/**
- *@brief go发送控制命令
- *@input[in] id 被控电机id
- *@revtal 返回电机数据指针
- */
-GO_Motorfield* getGoPoint(uint8_t id)
-{
- return &GO_motor_info[id];
-}
-
+}
\ No newline at end of file
diff --git a/User/device/GO_M8010_6_Driver.h b/User/device/GO_M8010_6_Driver.h
index 70caadf..32de5f3 100644
--- a/User/device/GO_M8010_6_Driver.h
+++ b/User/device/GO_M8010_6_Driver.h
@@ -1,94 +1,116 @@
#ifndef __GO_M8010_6_H
#define __GO_M8010_6_H
-#ifdef __cplusplus
-extern "C"{
-#endif
-#include "main.h"
-#include "crc16.h"
-#include "usart.h"
-#include "string.h"
-#include
-#include "user_math.h"
-#define GO_NUM 2
-/**
- * @brief
-
- *
- */
-typedef struct
-{
- uint8_t id :4;
- uint8_t status :3;
- uint8_t none :1;
-} RIS_Mode_t ;
-
-
-
-
-typedef struct
-{
- int16_t tor_des;
- int16_t spd_des;
- int32_t pos_des;
- uint16_t k_pos;
- uint16_t k_spd;
-
-} RIS_Comd_t;
-
-
-
+
+#include "struct_typedef.h"
+#pragma pack(1)
/**
+ * @brief ģʽϢ
*
- *
*/
typedef struct
{
- uint8_t head[2];
- RIS_Mode_t mode;
- RIS_Comd_t comd;
- uint16_t CRC16;
+ uint8_t id : 4; // ID: 0,1...,13,14 15ʾе㲥(ʱ)
+ uint8_t status : 3; // ģʽ: 0. 1.FOCջ 2.У 3.
+ uint8_t reserve : 1; // λ
+} RIS_Mode_t; // ģʽ 1Byte
-} ControlData_t;
+/**
+ * @brief ״̬Ϣ
+ *
+ */
+typedef struct
+{
+ int16_t tor_des; // ؽŤ unit: N.m (q8)
+ int16_t spd_des; // ؽٶ unit: rad/s (q8)
+ int32_t pos_des; // ؽλ unit: rad (q15)
+ int16_t k_pos; // ؽڸնϵ unit: -1.0-1.0 (q15)
+ int16_t k_spd; // ؽϵ unit: -1.0-1.0 (q15)
+
+} RIS_Comd_t; // Ʋ 12Byte
+
+/**
+ * @brief ״̬Ϣ
+ *
+ */
+typedef struct
+{
+ int16_t torque; // ʵʹؽŤ unit: N.m (q8)
+ int16_t speed; // ʵʹؽٶ unit: rad/s (q8)
+ int32_t pos; // ʵʹؽλ unit: rad (q15)
+ int8_t temp; // ¶: -128~127C
+ uint8_t MError : 3; // ʶ: 0. 1. 2. 3.ѹ 4. 5-7.
+ uint16_t force : 12; // ѹ 12bit (0-4095)
+ uint8_t none : 1; // λ
+} RIS_Fbk_t; // ״̬ 11Byte
+
+/**
+ * @brief ݰʽ
+ *
+ */
+typedef struct
+{
+ uint8_t head[2]; // ͷ 2Byte
+ RIS_Mode_t mode; // ģʽ 1Byte
+ RIS_Comd_t comd; // 12Byte
+ uint16_t CRC16; // CRC 2Byte
+
+} RIS_ControlData_t; // 17Byte
+
+/**
+ * @brief ݰʽ
+ *
+ */
+typedef struct
+{
+ uint8_t head[2]; // ͷ 2Byte
+ RIS_Mode_t mode; // ģʽ 1Byte
+ RIS_Fbk_t fbk; // 11Byte
+ uint16_t CRC16; // CRC 2Byte
+
+} RIS_MotorData_t; // 16Byte
+
+#pragma pack()
+
+/// @brief ָṹ
+typedef struct
+{
+ unsigned short id; // ID15㲥ݰ
+ unsigned short mode; // 0: 1:FOC 2:궨
+ float T; // ؽڵ()(Nm)
+ float W; // ؽٶ(ٶ)(rad/s)
+ float Pos; // ؽλ(rad)
+ float K_P; // ؽڸնϵ(0-25.599)
+ float K_W; // ؽٶϵ(0-25.599)
+} GO_MotorCmd_t;
+/// @brief ṹ
+typedef struct
+{
+ unsigned char motor_id; // ID
+ unsigned char mode; // 0: 1:FOC 2:궨
+ int Temp; // ¶
+ int MError; //
+ float T; // ǰʵʵ()(Nm)
+ float W; // ǰʵʵٶ(ٶ)(rad/s)
+ float Pos; // ǰλ(rad)
+ int correct; // Ƿ(10)
+ int footForce; // ԭʼֵ
-typedef struct {
- unsigned short id;
- unsigned short mode;
- uint16_t correct;
- int MError;
- int Temp;
- float tar_pos;
- float tar_w;
- float T;
- float W;
- float Pos;
- float angle;
-
- int footForce;
- uint8_t buffer[17];
- uint8_t Rec_buffer[16];
- ControlData_t motor_send_data;
-
-}GO_Motorfield;
+ uint16_t calc_crc;
+ uint32_t timeout; // ͨѶʱ
+ uint32_t bad_msg; // CRCУ
+ RIS_MotorData_t motor_recv_data; // ݽṹ
+} GO_MotorData_t;
-void GO_M8010_init(void);
-
-
-void GO_M8010_send_data(UART_HandleTypeDef *huart,int id, int rev,float T,float W,float Pos,float K_P,float K_W);
+void modify_data(GO_MotorCmd_t *motor_s);
+void extract_data(GO_MotorData_t *motor_r);
+const GO_MotorData_t *get_GO_measure_point(void);
+void GO_M8010_send_data( GO_MotorCmd_t *cmd);
void USART6_RxCompleteCallback(void );
-void uartTxCB(UART_HandleTypeDef *huart);
+void gogogo(void );
-void basic_ForceControl (UART_HandleTypeDef *huart, int id, float bias, float length, float mass,
- float tar_pos, float tar_w, float K_P,float K_W);
-
-
-GO_Motorfield* getGoPoint(uint8_t id);
-
-#ifdef __cplusplus
-}
-#endif
#endif /*__GO_M8010_6_H */
diff --git a/User/task/cmd_task.c b/User/task/cmd_task.c
index 94730ad..5864fae 100644
--- a/User/task/cmd_task.c
+++ b/User/task/cmd_task.c
@@ -45,9 +45,9 @@ void Task_cmd(void *argument){
osKernelUnlock(); /* 同上 解锁RTOS内核 */
- osMessageQueueReset(task_runtime.msgq.cmd.up_ctrl);
+ osMessageQueueReset(task_runtime.msgq.cmd.cmd_ctrl_t);
- osMessageQueuePut(task_runtime.msgq.cmd.up_ctrl,&cmd,0,0);
+ osMessageQueuePut(task_runtime.msgq.cmd.cmd_ctrl_t,&cmd,0,0);
tick += delay_tick; /*计算下一个唤醒时刻*/
osDelayUntil(tick); /*绝对延时 等待下一个唤醒时刻 */
diff --git a/User/task/init.c b/User/task/init.c
index d2cbadb..d74c274 100644
--- a/User/task/init.c
+++ b/User/task/init.c
@@ -85,7 +85,7 @@ void Task_Init(void *argument) {
osMessageQueueNew(3u, sizeof(CMD_RC_t), NULL);
task_runtime.msgq.cmd.raw.nuc =
osMessageQueueNew(3u,sizeof(CMD_NUC_t), NULL);
- task_runtime.msgq.cmd.up_ctrl =
+ task_runtime.msgq.cmd.cmd_ctrl_t =
osMessageQueueNew(3u,sizeof(CMD_t), NULL);
osKernelUnlock();
diff --git a/User/task/up_task.c b/User/task/up_task.c
index 0947102..74e1a88 100644
--- a/User/task/up_task.c
+++ b/User/task/up_task.c
@@ -68,7 +68,7 @@ void Task_up(void *argument)
//
-// UP_control(&UP,&UP_CAN_out,&up_cmd);
+ UP_control(&UP,&UP_CAN_out,&up_cmd);
UP.motor_target .go_shoot =aaa;
// UP.motor_target .M2006_angle =bbb ;
@@ -78,7 +78,6 @@ void Task_up(void *argument)
// UP.motor_target .go_spin =ddd;
//
ALL_Motor_Control(&UP,&UP_CAN_out);
-
osDelay(1);
@@ -91,7 +90,7 @@ void Task_up(void *argument)
/*can上设备数据获取*/
osMessageQueueGet(task_runtime.msgq.can.feedback.CAN_feedback, &can, NULL, 0);
- osMessageQueueGet(task_runtime.msgq.cmd .up_ctrl , &up_cmd, NULL, 0);
+ osMessageQueueGet(task_runtime.msgq.cmd .cmd_ctrl_t , &up_cmd, NULL, 0);
/*锁定RTOS（实时操作系统）内核，防止任务切换，直到 osKernelUnlock() 被调用*/
osKernelLock();
diff --git a/User/task/user_task.h b/User/task/user_task.h
index 0741e55..0eec8e5 100644
--- a/User/task/user_task.h
+++ b/User/task/user_task.h
@@ -67,7 +67,7 @@ typedef struct {
osMessageQueueId_t eulr;/*姿态结算得到的欧拉角*/
}raw;
- osMessageQueueId_t up_ctrl;
+ osMessageQueueId_t cmd_ctrl_t;
osMessageQueueId_t status;
} cmd;
/* can任务放入、读取，电机或电容的输入输出 */