diff --git a/.vscode/settings.json b/.vscode/settings.json index 0cc108a..7c0a420 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -29,6 +29,8 @@ "stdlib.h": "c", "lk9025.h": "c", "dji_motor.h": "c", - "ht04.h": "c" + "ht04.h": "c", + "controller.h": "c", + "memory.h": "c" } } \ No newline at end of file diff --git a/bsp/bsp_can.c b/bsp/bsp_can.c index 149060a..b63dda1 100644 --- a/bsp/bsp_can.c +++ b/bsp/bsp_can.c @@ -52,17 +52,22 @@ static void CANServiceInit() HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO1_MSG_PENDING); } -void CANRegister(can_instance* _instance) +can_instance* CANRegister(uint8_t tx_id,uint8_t rx_id,CAN_HandleTypeDef* can_handle,can_callback module_callback) { - static uint8_t instance_idx; - if(!instance_idx) + static uint8_t idx; + if(!idx) { CANServiceInit(); } - _instance->rx_buff=(uint8_t*)malloc(8*sizeof(uint8_t)); - _instance->tx_buff=(uint8_t*)malloc(8*sizeof(uint8_t)); - CANAddFilter(_instance); - instance[instance_idx++]=_instance; + instance[idx]=(can_instance*)malloc(sizeof(can_instance)); + instance[idx]->can_handle=can_handle; + instance[idx]->tx_id=tx_id; + instance[idx]->rx_id=rx_id; + instance[idx]->can_module_callback=module_callback; + instance[idx]->tx_buff=(uint8_t*)malloc(8*sizeof(uint8_t)); + instance[idx]->rx_buff=(uint8_t*)malloc(8*sizeof(uint8_t)); + CANAddFilter(instance[idx]); + return instance[idx++]; } void CANTransmit(can_instance* _instance) diff --git a/bsp/bsp_can.h b/bsp/bsp_can.h index c1032ac..c00c445 100644 --- a/bsp/bsp_can.h +++ b/bsp/bsp_can.h @@ -9,9 +9,6 @@ #define MX_CAN_FILTER_CNT (4 * 14) // temporarily useless #define DEVICE_CAN_CNT 2 // CAN1,CAN2 -/* module callback,which resolve protocol when new mesg arrives*/ - - /* can instance typedef, every module registered to CAN should have this variable */ typedef struct tmp { @@ -24,6 +21,9 @@ typedef struct tmp void (*can_module_callback)(struct tmp*); // callback needs an instance to tell among registered ones } can_instance; +/* module callback,which resolve protocol when new mesg arrives*/ +typedef void (*can_callback)(can_instance*); + /** * @brief transmit mesg through CAN device * @@ -34,13 +34,10 @@ void CANTransmit(can_instance* _instance); /** * @brief Register a module to CAN service,remember to call this before using a CAN device * - * @attention tx_id, rx_id, can_handle and module_callback should be set before calling this func - * for the rest configs, this func will do for you - * * @param _instance can instance owned by a specific device, remember to initialize it! * */ -void CANRegister(can_instance* _instance); +can_instance* CANRegister(uint8_t tx_id,uint8_t rx_id,CAN_HandleTypeDef* can_handle,can_callback module_callback); #endif diff --git a/modules/motor/HT04.c b/modules/motor/HT04.c index af80fcb..35399f8 100644 --- a/modules/motor/HT04.c +++ b/modules/motor/HT04.c @@ -3,18 +3,6 @@ joint_instance* joint_motor_info[HT_MOTOR_CNT]; - -void HTMotorInit(joint_instance* _instance,CAN_HandleTypeDef* _hcan,uint8_t tx_id,uint8_t rx_id) -{ - static uint8_t idx; - _instance->motor_can_instace.can_handle=_hcan; - _instance->motor_can_instace.tx_id=tx_id; - _instance->motor_can_instace.rx_id=rx_id; - _instance->motor_can_instace.can_module_callback=DecodeJoint; - CANRegister(&_instance->motor_can_instace); - joint_motor_info[idx++]=_instance; -} - static uint16_t float_to_uint(float x, float x_min, float x_max, uint8_t bits) { float span = x_max - x_min; @@ -29,25 +17,7 @@ static float uint_to_float(int x_int, float x_min, float x_max, int bits) return ((float)x_int)*span/((float)((1<motor_can_instace.rx_buff[6] = tmp>>8; - _instance->motor_can_instace.rx_buff[7] = tmp&0xff; - CANTransmit(&_instance->motor_can_instace); -} - -void SetJointMode(joint_mode cmd,joint_instance* _instance) -{ - static uint8_t buf[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00}; - buf[7]=(uint8_t)cmd; - memcpy(_instance->motor_can_instace.rx_buff,buf,8*sizeof(uint8_t)); - CANTransmit(&_instance->motor_can_instace); -} - -void DecodeJoint(can_instance* motor_instance) +static void DecodeJoint(can_instance* motor_instance) { uint16_t tmp; for (size_t i = 0; i < HT_MOTOR_CNT; i++) @@ -64,4 +34,29 @@ void DecodeJoint(can_instance* motor_instance) break; } } -} \ No newline at end of file +} + +joint_instance* HTMotorInit(CAN_HandleTypeDef* _hcan,uint8_t tx_id,uint8_t rx_id) +{ + static uint8_t idx; + joint_motor_info[idx]=(joint_instance*)malloc(sizeof(joint_instance)); + joint_motor_info[idx++]->motor_can_instace=CANRegister(tx_id,rx_id,_hcan,DecodeJoint); +} + +void JointControl(joint_instance* _instance,float current) +{ + uint16_t tmp; + LIMIT_MIN_MAX(current, T_MIN, T_MAX); + tmp = float_to_uint(current, T_MIN, T_MAX, 12); + _instance->motor_can_instace->rx_buff[6] = tmp>>8; + _instance->motor_can_instace->rx_buff[7] = tmp&0xff; + CANTransmit(&_instance->motor_can_instace); +} + +void SetJointMode(joint_mode cmd,joint_instance* _instance) +{ + static uint8_t buf[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00}; + buf[7]=(uint8_t)cmd; + memcpy(_instance->motor_can_instace->rx_buff,buf,8*sizeof(uint8_t)); + CANTransmit(&_instance->motor_can_instace); +} diff --git a/modules/motor/HT04.h b/modules/motor/HT04.h index 6419a00..cb1ddcd 100644 --- a/modules/motor/HT04.h +++ b/modules/motor/HT04.h @@ -22,7 +22,7 @@ typedef struct //HT04 float given_current; PID_t pid; - can_instance motor_can_instace; + can_instance* motor_can_instace; } joint_instance; typedef enum @@ -32,11 +32,10 @@ typedef enum CMD_ZERO_POSITION = 0xfe } joint_mode; -void HTMotorInit(joint_instance* _instance,CAN_HandleTypeDef* _hcan,uint8_t tx_id,uint8_t rx_id); +joint_instance* HTMotorInit(CAN_HandleTypeDef* _hcan,uint8_t tx_id,uint8_t rx_id); void JointControl(joint_instance* _instance,float current); void SetJointMode(joint_mode cmd,joint_instance* _instance); -void DecodeJoint(can_instance* motor_instance); #endif // !HT04_H#define HT04_H \ No newline at end of file diff --git a/modules/motor/LK9025.c b/modules/motor/LK9025.c index 0a81304..272b618 100644 --- a/modules/motor/LK9025.c +++ b/modules/motor/LK9025.c @@ -18,25 +18,21 @@ static void DecodeDriven(can_instance* _instance) } } -void LKMotroInit(driven_instance* instance,CAN_HandleTypeDef* _hcan,uint8_t tx_id,uint8_t rx_id) +driven_instance* LKMotroInit(CAN_HandleTypeDef* _hcan,uint8_t tx_id,uint8_t rx_id) { static uint8_t idx; - instance->motor_can_instance.can_module_callback=DecodeDriven; - instance->motor_can_instance.can_handle=_hcan; - instance->motor_can_instance.tx_id=tx_id; - instance->motor_can_instance.rx_id=rx_id; - CANRegister(&instance->motor_can_instance); - driven_motor_info[idx++]=instance; + driven_motor_info[idx]=(driven_instance*)malloc(sizeof(driven_instance)); + driven_motor_info[idx++]->motor_can_instance=CANRegister(tx_id,rx_id,_hcan,DecodeDriven); } void DrivenControl(int16_t motor1_current,int16_t motor2_current) { LIMIT_MIN_MAX(motor1_current, I_MIN, I_MAX); LIMIT_MIN_MAX(motor2_current, I_MIN, I_MAX); - driven_motor_info[0]->motor_can_instance.tx_buff[0] = motor1_current; - driven_motor_info[0]->motor_can_instance.tx_buff[1] = motor1_current>>8; - driven_motor_info[0]->motor_can_instance.tx_buff[2] = motor2_current; - driven_motor_info[0]->motor_can_instance.tx_buff[3] = motor2_current>>8; + driven_motor_info[0]->motor_can_instance->tx_buff[0] = motor1_current; + driven_motor_info[0]->motor_can_instance->tx_buff[1] = motor1_current>>8; + driven_motor_info[0]->motor_can_instance->tx_buff[2] = motor2_current; + driven_motor_info[0]->motor_can_instance->tx_buff[3] = motor2_current>>8; CANTransmit(&driven_motor_info[0]->motor_can_instance); } diff --git a/modules/motor/LK9025.h b/modules/motor/LK9025.h index 442a925..3ad9529 100644 --- a/modules/motor/LK9025.h +++ b/modules/motor/LK9025.h @@ -19,8 +19,8 @@ typedef struct //9025 int16_t given_current; uint8_t temperate; - PID_t pid; - can_instance motor_can_instance; + PID_t* pid; + can_instance* motor_can_instance; } driven_instance; @@ -29,7 +29,7 @@ typedef enum unused = 0, } driven_mode; -void LKMotroInit(driven_instance* instance,CAN_HandleTypeDef* _hcan,uint8_t tx_id,uint8_t rx_id); +driven_instance* LKMotroInit(CAN_HandleTypeDef* _hcan,uint8_t tx_id,uint8_t rx_id); void DrivenControl(int16_t motor1_current,int16_t motor2_current); diff --git a/modules/motor/dji_motor.c b/modules/motor/dji_motor.c index c6b6704..f5fd786 100644 --- a/modules/motor/dji_motor.c +++ b/modules/motor/dji_motor.c @@ -7,7 +7,7 @@ static void DecodeDJIMotor(can_instance* _instance) { for (size_t i = 0; i < DJI_MOTOR_CNT; i++) { - if(&dji_motor_info[i]->motor_can_instance==_instance) + if(dji_motor_info[i]->motor_can_instance==_instance) { dji_motor_info[i]->last_ecd = dji_motor_info[i]->ecd; dji_motor_info[i]->ecd = (uint16_t)(_instance->rx_buff[0] << 8 | _instance->rx_buff[1]); @@ -19,15 +19,11 @@ static void DecodeDJIMotor(can_instance* _instance) } } -void DJIMotorInit(dji_motor_instance* motor_instance,CAN_HandleTypeDef* _hcan,uint16_t tx_id,uint16_t rx_id) +dji_motor_instance* DJIMotorInit(CAN_HandleTypeDef* _hcan,uint16_t tx_id,uint16_t rx_id) { static uint8_t idx; - motor_instance->motor_can_instance.can_handle=_hcan; - motor_instance->motor_can_instance.tx_id=tx_id; - motor_instance->motor_can_instance.rx_id=rx_id; - motor_instance->motor_can_instance.can_module_callback=DecodeDJIMotor; - CANRegister(&motor_instance->motor_can_instance); - dji_motor_info[idx++]=motor_instance; + dji_motor_info[idx]=(dji_motor_instance*)malloc(sizeof(dji_motor_instance)); + dji_motor_info[idx++]->motor_can_instance=CANRegister(tx_id,rx_id,_hcan,DecodeDJIMotor); } void DJIMotorControl() diff --git a/modules/motor/dji_motor.h b/modules/motor/dji_motor.h index 2501ce2..1416fd9 100644 --- a/modules/motor/dji_motor.h +++ b/modules/motor/dji_motor.h @@ -15,11 +15,11 @@ typedef struct int16_t last_ecd; PID_t motor_pid; - can_instance motor_can_instance; + can_instance *motor_can_instance; } dji_motor_instance; -void DJIMotorInit(dji_motor_instance* motor_instace,CAN_HandleTypeDef* _hcan,uint16_t tx_id,uint16_t rx_id); +dji_motor_instance* DJIMotorInit(CAN_HandleTypeDef* _hcan,uint16_t tx_id,uint16_t rx_id); void DJIMotorControl();