diff --git a/CMakeLists.txt b/CMakeLists.txt index b5c42ce..8ca44ab 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -54,7 +54,7 @@ include_directories(Inc Drivers/STM32F4xx_HAL_Driver/Inc Drivers/STM32F4xx_HAL_D modules modules/alarm modules/algorithm modules/BMI088 modules/can_comm modules/daemon modules/encoder modules/imu modules/ist8310 modules/led modules/master_machine modules/message_center modules/motor modules/oled modules/referee modules/remote modules/standard_cmd modules/super_cap modules/TFminiPlus modules/unicomm modules/vofa - modules/motor/DJImotor modules/motor/HTmotor modules/motor/LKmotor modules/motor/servo_motor modules/motor/step_motor + modules/motor/DJImotor modules/motor/HTmotor modules/motor/LKmotor modules/motor/servo_motor modules/motor/step_motor modules/motor/ECmotor application application/chassis application/cmd application/gimbal application/shoot Middlewares/Third_Party/SEGGER/RTT Middlewares/Third_Party/SEGGER/Config) diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index 9b6258f..f1b76fc 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -1,6 +1,7 @@ #include "gimbal.h" #include "robot_def.h" #include "dji_motor.h" +#include "ECmotor/ECA8210.h" #include "ins_task.h" #include "message_center.h" #include "general_def.h" @@ -11,11 +12,15 @@ static attitude_t *gimba_IMU_data; // 云台IMU数据 static DJIMotorInstance *yaw_motor, *pitch_motor; +static ECMotorInstance *big_yaw_motor; + static Publisher_t *gimbal_pub; // 云台应用消息发布者(云台反馈给cmd) static Subscriber_t *gimbal_sub; // cmd控制消息订阅者 static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息 static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv; // 来自cmd的控制信息 + +sin_input_generate_t sinInputGenerate; void GimbalInit() { gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源 @@ -73,40 +78,51 @@ void GimbalInit() // .IntegralLimit = 100, // .MaxOut = 500, // }, -// .speed_PID = { -// .Kp = 50, // 50 -// .Ki = 350, // 350 -// .Kd = 0, // 0 -// .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, -// .IntegralLimit = 2500, -// .MaxOut = 20000, -// }, + .speed_PID = { + .Kp = 5.13, // 50 + .Ki = 88.26, // 350 + .Kd = 0, // 0 + .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, + .IntegralLimit = 2500, + .MaxOut = 30000, + }, // .other_angle_feedback_ptr = &gimba_IMU_data->Pitch, // // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明 // .other_speed_feedback_ptr = (&gimba_IMU_data->Gyro[0]), - .current_PID = { - .Kp = 1, // 10 - .Ki = 0, - .Kd = 0, - .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, - .IntegralLimit = 3000, - .MaxOut = 30000, - } + }, .controller_setting_init_config = { - .outer_loop_type = OPEN_LOOP, - .close_loop_type = OPEN_LOOP, + .outer_loop_type = SPEED_LOOP, + .close_loop_type = SPEED_LOOP|CURRENT_LOOP, .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, }, .motor_type = GM6020, .motor_control_type = CURRENT_CONTROL }; + + Motor_Init_Config_s big_yaw_config = { + .can_init_config = { + .can_handle = &hcan1, + .tx_id = 1, + }, + .controller_setting_init_config = { + .outer_loop_type = OPEN_LOOP, + .close_loop_type = OPEN_LOOP, + .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, + }, + .motor_type = ECA8210 + }; + // 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动 - yaw_motor = DJIMotorInit(&yaw_config); - pitch_motor = DJIMotorInit(&pitch_config); +// yaw_motor = DJIMotorInit(&yaw_config); +// pitch_motor = DJIMotorInit(&pitch_config); + + big_yaw_motor = ECMotorInit(&big_yaw_config); gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s)); gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s)); + + sin_input_frequency_init(&sinInputGenerate); } /* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */ @@ -154,21 +170,30 @@ void GimbalTask() // 在合适的地方添加pitch重力补偿前馈力矩 // 根据IMU姿态/pitch电机角度反馈计算出当前配重下的重力矩 // ... - static uint32_t cnt; - static float time; - DJIMotorEnable(pitch_motor); - float deltaT=DWT_GetDeltaT(&cnt); - time += deltaT; + + //DJIMotorEnable(pitch_motor); + ECMotorEnable(big_yaw_motor); + + float input = step_input_generate(&sinInputGenerate); + + + + //DJIMotorSetRef(pitch_motor,input); + ECMotorSetRef(big_yaw_motor,30); + + //ANODT_SendF1(input*1000,pitch_motor->measure.speed_aps*1000,0,0); + float theta = pitch_motor->measure.angle_single_round - 6200 * ECD_ANGLE_COEF_DJI; - //float input = arm_sin_f32(2*PI*10*time); + float gravity_feed = 3800*arm_cos_f32(theta/180*PI); - DJIMotorSetRef(pitch_motor,gravity_feed); + //DJIMotorSetRef(pitch_motor,gravity_feed); float vofa_send_data[4]; -// vofa_send_data[0]=yaw_motor->motor_controller.speed_PID.Ref; -// vofa_send_data[1]=yaw_motor->motor_controller.speed_PID.Measure; -// vofa_send_data[2]=yaw_motor->motor_controller.angle_PID.Ref; + vofa_send_data[0]=big_yaw_motor->measure.speed_rads; + vofa_send_data[1]=big_yaw_motor->measure.angle_single_round; + vofa_send_data[2]=big_yaw_motor->measure.real_current; + vofa_send_data[3]=big_yaw_motor->measure.temperature; // vofa_send_data[3]=yaw_motor->motor_controller.angle_PID.Measure; -// vofa_justfloat_output(vofa_send_data,16,&huart1); + vofa_justfloat_output(vofa_send_data,16,&huart1); vofa_send_data[0] = pitch_motor->motor_controller.pid_ref; @@ -185,16 +210,71 @@ void GimbalTask() } -void sin_input_generate(float frequency, int count) -{ - static uint32_t cnt; - static float time; - while(time>=count*(1/frequency)) - { - float deltaT=DWT_GetDeltaT(&cnt); - time += deltaT; +//void sin_input_generate(float frequency, int count) +//{ +// static uint32_t cnt; +// static float time; +// while(time>=count*(1/frequency)) +// { +// float deltaT=DWT_GetDeltaT(&cnt); +// time += deltaT; +// +// float input = arm_sin_f32(2*PI*frequency*time); +// DJIMotorSetRef(yaw_motor,input); +// } +//} - float input = arm_sin_f32(2*PI*frequency*time); - DJIMotorSetRef(yaw_motor,input); +void sin_input_frequency_init(sin_input_generate_t* InputGenerate) +{ + for(int i=0;i<43;i++) + { + InputGenerate->frequency[i] = 1.0 + 0.5*i; } + for(int i=0;i<9;i++) + { + InputGenerate->frequency[i+43] = 24.0 + 2.0*i; + } + for(int i=0;i<8;i++) + { + InputGenerate->frequency[i+43+9] = 50 + 10*i; + } + InputGenerate->frequency[60] = 200; + InputGenerate->frequency[61] = 250; + InputGenerate->frequency[62] = 333; + InputGenerate->frequency[63] = 500; +} + +float sin_input_generate(sin_input_generate_t* InputGenerate) +{ + InputGenerate->DeltaT = DWT_GetDeltaT(&InputGenerate->cnt); + InputGenerate->time += InputGenerate->DeltaT; + if(InputGenerate->time >= 20*(1/InputGenerate->frequency[InputGenerate->frequency_index])) + { + InputGenerate->time = 0; + InputGenerate->frequency_index += 1; + } + if(InputGenerate->frequency_index >= 64) + { + InputGenerate->input = 0; + } + else + InputGenerate->input = arm_sin_f32(2*PI*InputGenerate->frequency[InputGenerate->frequency_index]*InputGenerate->time); + //float input = arm_sin_f32(2*PI*frequency*time); + + return InputGenerate->input; +} + +float step_input_generate(sin_input_generate_t* InputGenerate) +{ + static int8_t forward_flag = 1; + InputGenerate->DeltaT = DWT_GetDeltaT(&InputGenerate->cnt); + InputGenerate->time += InputGenerate->DeltaT; + if(InputGenerate->time >= 3) + { + if(forward_flag ==1) forward_flag = -1; + else if (forward_flag == -1) forward_flag = 1; + + InputGenerate->time = 0; + } + return 60*forward_flag; } \ No newline at end of file diff --git a/application/gimbal/gimbal.h b/application/gimbal/gimbal.h index 0bcc37e..263525b 100644 --- a/application/gimbal/gimbal.h +++ b/application/gimbal/gimbal.h @@ -1,6 +1,6 @@ #ifndef GIMBAL_H #define GIMBAL_H - +#include "robot_def.h" /** * @brief 初始化云台,会被RobotInit()调用 * @@ -13,4 +13,18 @@ void GimbalInit(); */ void GimbalTask(); +typedef struct +{ + uint32_t cnt;//计时用 + uint8_t frequency_index;//运行到哪个频率 + float time; + float frequency[64]; + float DeltaT; + float input; +} sin_input_generate_t; + +void sin_input_frequency_init(sin_input_generate_t* InputGenerate); +float sin_input_generate(sin_input_generate_t* InputGenerate); +float step_input_generate(sin_input_generate_t* InputGenerate); + #endif // GIMBAL_H \ No newline at end of file diff --git a/application/robot.c b/application/robot.c index 44bc884..7de82b4 100644 --- a/application/robot.c +++ b/application/robot.c @@ -37,7 +37,7 @@ void RobotInit() #endif #if defined(ONE_BOARD) || defined(CHASSIS_BOARD) - ChassisInit(); + //ChassisInit(); #endif OSTaskInit(); // 创建基础任务 @@ -56,7 +56,7 @@ void RobotTask() #endif #if defined(ONE_BOARD) || defined(CHASSIS_BOARD) - ChassisTask(); + //ChassisTask(); #endif } \ No newline at end of file diff --git a/application/robot_task.h b/application/robot_task.h index 252e77b..78a4f20 100644 --- a/application/robot_task.h +++ b/application/robot_task.h @@ -119,9 +119,9 @@ __attribute__((noreturn)) void StartROBOTTASK(void const *argument) robot_start = DWT_GetTimeline_ms(); RobotTask(); robot_dt = DWT_GetTimeline_ms() - robot_start; - if (robot_dt > 5) + if (robot_dt > 1) LOGERROR("[freeRTOS] ROBOT core Task is being DELAY! dt = [%f]", &robot_dt); - osDelay(5); + osDelay(1); } } diff --git a/modules/motor/DJImotor/dji_motor.c b/modules/motor/DJImotor/dji_motor.c index c0d9935..eb14da8 100644 --- a/modules/motor/DJImotor/dji_motor.c +++ b/modules/motor/DJImotor/dji_motor.c @@ -306,7 +306,9 @@ void DJIMotorControl() pid_ref += *motor_controller->current_feedforward_ptr; if (motor_setting->close_loop_type & CURRENT_LOOP) { - pid_ref = PIDCalculate(&motor_controller->current_PID, measure->real_current, pid_ref); + //现在3508和6020电调都内置电流闭环 无需PID计算 + //pid_ref = PIDCalculate(&motor_controller->current_PID, measure->real_current, pid_ref); + pid_ref = pid_ref; } diff --git a/modules/motor/ECmotor/ECA8210.c b/modules/motor/ECmotor/ECA8210.c new file mode 100644 index 0000000..47ad42d --- /dev/null +++ b/modules/motor/ECmotor/ECA8210.c @@ -0,0 +1,179 @@ +// +// Created by SJQ on 2023/12/11. +// + +#include "ECA8210.h" + +ECMotor_Report_t ECMotor_Report; +ECMotor_SpdCMD_t ECMotor_SpdCMD; +ECMotor_PosCMD_t ECMotor_PosCMD; +static uint8_t idx; +static ECMotorInstance *ecmotor_instance[EC_MOTOR_MX_CNT] = {NULL}; + +static void swap ( uint8_t *a ,uint8_t *b ) +{ + uint8_t temp = *a; + *a = *b; + *b = temp; +} +/** + * @brief 电机反馈报文解析 + * + * @param _instance 发生中断的caninstance + */ +static void ECMotorDecode(CANInstance *_instance) +{ + ECMotorInstance *motor = (ECMotorInstance *)_instance->id; // 通过caninstance保存的father id获取对应的motorinstance + ECMotor_Measure_t *measure = &motor->measure; + uint8_t *rx_buff = _instance->rx_buff; + + swap(rx_buff+1,rx_buff+2);// 输出端位置 高位在前 + + memcpy(&ECMotor_Report,rx_buff,sizeof(ECMotor_Report)); + //高位在前 这两个12位数据需要做特殊处理 + ECMotor_Report.speed_raw = (uint16_t)(rx_buff[3]<<4|(rx_buff[4]>>4)); + ECMotor_Report.current_raw = (uint16_t)(rx_buff[4]<<8|(rx_buff[5])); + + DaemonReload(motor->daemon); // 喂狗 + measure->feed_dt = DWT_GetDeltaT(&measure->feed_dwt_cnt); + + measure->angle_single_round = (float)(ECMotor_Report.angle_raw-32768) * 0.0003814697; + measure->speed_rads = (float)(ECMotor_Report.speed_raw-2048) * 0.008789; + measure->real_current = (float)(ECMotor_Report.current_raw-2048) * 0.014648; + measure->temperature = (ECMotor_Report.temperature_raw - 50) /2; +} + +static void ECMotorLostCallback(void *motor_ptr) +{ + ECMotorInstance *motor = (ECMotorInstance *)motor_ptr; + LOGWARNING("[ECMotor] motor lost, id: %d", motor->motor_can_ins->tx_id); +} + +ECMotorInstance *ECMotorInit(Motor_Init_Config_s *config) +{ + ECMotorInstance *motor = (ECMotorInstance *)malloc(sizeof(ECMotorInstance)); + motor = (ECMotorInstance *)malloc(sizeof(ECMotorInstance)); + memset(motor, 0, sizeof(ECMotorInstance)); + + motor->motor_settings = config->controller_setting_init_config; + PIDInit(&motor->current_PID, &config->controller_param_init_config.current_PID); + PIDInit(&motor->speed_PID, &config->controller_param_init_config.speed_PID); + PIDInit(&motor->angle_PID, &config->controller_param_init_config.angle_PID); + motor->other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr; + motor->other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr; + + config->can_init_config.id = motor; + config->can_init_config.can_module_callback = ECMotorDecode; + config->can_init_config.rx_id = config->can_init_config.tx_id; + config->can_init_config.tx_id = config->can_init_config.tx_id; + motor->motor_can_ins = CANRegister(&config->can_init_config); + + ECMotorEnable(motor); + + DWT_GetDeltaT(&motor->measure.feed_dwt_cnt); + ecmotor_instance[idx++] = motor; + + Daemon_Init_Config_s daemon_config = { + .callback = ECMotorLostCallback, + .owner_id = motor, + .reload_count = 5, // 50ms + }; + motor->daemon = DaemonRegister(&daemon_config); + + return motor; +} + + + +void ECMotorControl() +{ + float pid_measure, pid_ref; + int16_t set; + ECMotorInstance *motor; + ECMotor_Measure_t *measure; + Motor_Control_Setting_s *setting; + + for (size_t i = 0; i < idx; ++i) + { + motor = ecmotor_instance[i]; + measure = &motor->measure; + setting = &motor->motor_settings; + pid_ref = motor->pid_ref; + + if ((setting->close_loop_type & ANGLE_LOOP) && setting->outer_loop_type == ANGLE_LOOP) + { + if (setting->angle_feedback_source == OTHER_FEED) + pid_measure = *motor->other_angle_feedback_ptr; + else + pid_measure = measure->real_current; + pid_ref = PIDCalculate(&motor->angle_PID, pid_measure, pid_ref); + if (setting->feedforward_flag & SPEED_FEEDFORWARD) + pid_ref += *motor->speed_feedforward_ptr; + } + + if ((setting->close_loop_type & SPEED_LOOP) && setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP)) + { + if (setting->angle_feedback_source == OTHER_FEED) + pid_measure = *motor->other_speed_feedback_ptr; + else + pid_measure = measure->speed_rads; + pid_ref = PIDCalculate(&motor->angle_PID, pid_measure, pid_ref); + if (setting->feedforward_flag & CURRENT_FEEDFORWARD) + pid_ref += *motor->current_feedforward_ptr; + } + + if (setting->close_loop_type & CURRENT_LOOP) + { + pid_ref = PIDCalculate(&motor->current_PID, measure->real_current, pid_ref); + } + + set = pid_ref; + if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE) + set *= -1; + + ECMotor_SpdCMD.mode = 0b010; + ECMotor_SpdCMD.reserve = 0b000; + ECMotor_SpdCMD.ack_type = 0b01; + ECMotor_SpdCMD.spd_target = set; + ECMotor_SpdCMD.max_current = 10; + // 这里随便写的,为了兼容多电机命令.后续应该将tx_id以更好的方式表达电机id,单独使用一个CANInstance,而不是用第一个电机的CANInstance + //memcpy(sender_instance->tx_buff + (motor->motor_can_ins->tx_id - 0x280) * 2, &set, sizeof(uint16_t)); + CANSetDLC(motor->motor_can_ins,7); + memcpy(motor->motor_can_ins->tx_buff,&ECMotor_SpdCMD,sizeof(ECMotor_SpdCMD)); + //发送数据高位在前！！ 交换一下顺序 + swap(motor->motor_can_ins->tx_buff+1,motor->motor_can_ins->tx_buff+4); + swap(motor->motor_can_ins->tx_buff+2,motor->motor_can_ins->tx_buff+3); + swap(motor->motor_can_ins->tx_buff+5,motor->motor_can_ins->tx_buff+6); + + //memcpy(motor->motor_can_ins->tx_buff+1,&ECMotor_SpdCMD.spd_target,sizeof(float32_t)); + + if (motor->stop_flag == MOTOR_STOP) + { // 若该电机处于停止状态,直接将发送buff置零 + memcpy(motor->motor_can_ins->tx_buff,0,sizeof(ECMotor_SpdCMD)); + } + + CANTransmit(motor->motor_can_ins,0.2); + } + + +} + +void ECMotorStop(ECMotorInstance *motor) +{ + motor->stop_flag = MOTOR_STOP; +} + +void ECMotorEnable(ECMotorInstance *motor) +{ + motor->stop_flag = MOTOR_ENALBED; +} + +void ECMotorSetRef(ECMotorInstance *motor, float ref) +{ + motor->pid_ref = ref; +} + +uint8_t ECMotorIsOnline(ECMotorInstance *motor) +{ + return DaemonIsOnline(motor->daemon); +} diff --git a/modules/motor/ECmotor/ECA8210.h b/modules/motor/ECmotor/ECA8210.h new file mode 100644 index 0000000..e5e4ee4 --- /dev/null +++ b/modules/motor/ECmotor/ECA8210.h @@ -0,0 +1,130 @@ +// +// Created by SJQ on 2023/12/11. +// + +#ifndef BASIC_FRAMEWORK_ECA8210_H +#define BASIC_FRAMEWORK_ECA8210_H + +#include "stdint.h" +#include "bsp_can.h" +#include "controller.h" +#include "motor_def.h" +#include "daemon.h" + +#define EC_MOTOR_MX_CNT 4 + +#pragma pack(1) +typedef struct +{ + uint8_t bug_info:5; //错误信息 + uint8_t report_type:3; //报文类别 + + uint16_t angle_raw:16; //输出端位置数值范围：0~65536 对应-12.5f~12.5f，单位 rad + uint16_t speed_raw:12; //输出端转速数值范围：0~4095 对应-18.0f~18.0f，单位 rad/s + uint16_t current_raw:12; //实际电流：0~4095 对应-30~30A + uint8_t temperature_raw:8;//电机温度：反馈的数值数据类型为无符号 8 位，数值等于实际温度*2+50 +}ECMotor_Report_t; +#pragma pack() +typedef struct +{ + uint8_t mode:3; //控制模式 + float32_t pos_target; //错误信息 + uint16_t spd_target:15; //输出端位置数值范围：0~65536 对应-12.5f~12.5f，单位 rad + uint16_t max_current:12; //输出端转速数值范围：0~4095 对应-18.0f~18.0f，单位 rad/s + uint8_t ack_type:2; //实际电流：0~4095 对应-30~30A + +}ECMotor_PosCMD_t; +#pragma pack(1) +typedef struct +{ + uint8_t ack_type:2; + uint8_t reserve:3; + uint8_t mode:3; //控制模式 + + + float32_t spd_target; //输出端速度指令 单位RPM + uint16_t max_current; //电流限幅 电机电流阈值数值解释：0~65536 对应 0~6553.6A，比例为10 + +}ECMotor_SpdCMD_t; + +#pragma pack() + +typedef struct // EC-A8120 +{ + uint16_t last_ecd; // 上一次读取的编码器值 + uint16_t ecd; // 当前编码器值 + float angle_single_round; // 单圈角度 + float speed_rads; // speed rad/s + float real_current; // 实际电流 + uint8_t temperature; // 温度,C° + + float total_angle; // 总角度 + int32_t total_round; // 总圈数 + + float feed_dt; + uint32_t feed_dwt_cnt; +} ECMotor_Measure_t; + +typedef struct +{ + ECMotor_Measure_t measure; + + Motor_Control_Setting_s motor_settings; + + float *other_angle_feedback_ptr; // 其他反馈来源的反馈数据指针 + float *other_speed_feedback_ptr; + float *speed_feedforward_ptr; // 速度前馈数据指针,可以通过此指针设置速度前馈值,或LQR等时作为速度状态变量的输入 + float *current_feedforward_ptr; // 电流前馈指针 + PIDInstance current_PID; + PIDInstance speed_PID; + PIDInstance angle_PID; + float pid_ref; + + Motor_Working_Type_e stop_flag; // 启停标志 + + CANInstance *motor_can_ins; + + DaemonInstance *daemon; + +} ECMotorInstance; + +/** + * @brief 初始化LK电机 + * + * @param config 电机配置 + * @return LKMotorInstance* 返回实例指针 + */ +ECMotorInstance *ECMotorInit(Motor_Init_Config_s *config); + +/** + * @brief 设置参考值 + * @attention 注意此函数设定的ref是最外层闭环的输入,若要设定内层闭环的值请通过前馈数据指针设置 + * + * @param motor 要设置的电机 + * @param ref 设定值 + */ +void ECMotorSetRef(ECMotorInstance *motor, float ref); + +/** + * @brief 为所有EC电机计算pid/反转/模式控制,并通过bspcan发送电流值(发送CAN报文) + * + */ +void ECMotorControl(); + +/** + * @brief 停止EC电机,之后电机不会响应任何指令 + * + * @param motor + */ +void ECMotorStop(ECMotorInstance *motor); + +/** + * @brief 启动EC电机 + * + * @param motor + */ +void ECMotorEnable(ECMotorInstance *motor); + +uint8_t ECMotorIsOnline(ECMotorInstance *motor); + +#endif //BASIC_FRAMEWORK_ECA8210_H diff --git a/modules/motor/motor_def.h b/modules/motor/motor_def.h index 72c42fa..e7d4f61 100644 --- a/modules/motor/motor_def.h +++ b/modules/motor/motor_def.h @@ -106,6 +106,7 @@ typedef enum M2006, LK9025, HT04, + ECA8210, } Motor_Type_e; /* 电机控制方式枚举 */ diff --git a/modules/motor/motor_task.c b/modules/motor/motor_task.c index 47ff232..807957b 100644 --- a/modules/motor/motor_task.c +++ b/modules/motor/motor_task.c @@ -1,5 +1,6 @@ #include "motor_task.h" #include "LK9025.h" +#include "ECA8210.h" #include "HT04.h" #include "dji_motor.h" #include "step_motor.h" @@ -14,7 +15,7 @@ void MotorControlTask() /* 如果有对应的电机则取消注释,可以加入条件编译或者register对应的idx判断是否注册了电机 */ LKMotorControl(); - + ECMotorControl(); // legacy support // 由于ht04电机的反馈方式为接收到一帧消息后立刻回传,以此方式连续发送可能导致总线拥塞 // 为了保证高频率控制,HTMotor中提供了以任务方式启动控制的接口,可通过宏定义切换 diff --git a/modules/vofa/vofa.c b/modules/vofa/vofa.c index 4ea4ae8..41526ea 100644 --- a/modules/vofa/vofa.c +++ b/modules/vofa/vofa.c @@ -33,3 +33,54 @@ void vofa_justfloat_output(float *data, uint8_t num , UART_HandleTypeDef *huart //HAL_UART_Transmit(huart, (uint8_t *)send_data, 4 * num + 4, 100); CDC_Transmit_FS((uint8_t *)send_data,4 * num + 4); } + +#define BYTE0(dwTemp) (*(char*)(&dwTemp)) +#define BYTE1(dwTemp) (*((char*)(&dwTemp)+1)) +#define BYTE2(dwTemp) (*((char*)(&dwTemp)+2)) +#define BYTE3(dwTemp) (*((char*)(&dwTemp)+3)) + +uint8_t DataSendBuf[100]; +//匿名上位机 +void ANODT_SendF1(int32_t Angle,int32_t speed_rpm,int32_t Angle_target,int32_t speed_target)//F1灵活格式帧 +{ + uint8_t cnt=0; + DataSendBuf[cnt++]=0xAA; //帧头 + DataSendBuf[cnt++]=0xFF; //目标地址 + DataSendBuf[cnt++]=0xF1; //功能码 + DataSendBuf[cnt++]=16; //数据长度 + + DataSendBuf[cnt++]=BYTE0(Angle); + DataSendBuf[cnt++]=BYTE1(Angle); + DataSendBuf[cnt++]=BYTE2(Angle); + DataSendBuf[cnt++]=BYTE3(Angle); + + DataSendBuf[cnt++]=BYTE0(speed_rpm); + DataSendBuf[cnt++]=BYTE1(speed_rpm); + DataSendBuf[cnt++]=BYTE2(speed_rpm); + DataSendBuf[cnt++]=BYTE3(speed_rpm); + + DataSendBuf[cnt++]=BYTE0(Angle_target); + DataSendBuf[cnt++]=BYTE1(Angle_target); + DataSendBuf[cnt++]=BYTE2(Angle_target); + DataSendBuf[cnt++]=BYTE3(Angle_target); + + DataSendBuf[cnt++]=BYTE0(speed_target); + DataSendBuf[cnt++]=BYTE1(speed_target); + DataSendBuf[cnt++]=BYTE2(speed_target); + DataSendBuf[cnt++]=BYTE3(speed_target); + + uint8_t sc=0; //和校验 + uint8_t ac=0; //附加校验 + for(uint8_t i=0;i