From cf66cc255f1b834bb950dd0134145d21ff5c16ff Mon Sep 17 00:00:00 2001 From: ws <1621320660@qq.com> Date: Thu, 3 Apr 2025 15:11:57 +0800 Subject: [PATCH] =?UTF-8?q?=E8=A7=86=E8=A7=89=E6=B5=8B=E8=AF=95=E5=8F=AF?= =?UTF-8?q?=E4=BB=A5?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/bsp/can_init.c | 6 +- User/device/GO_M8010_6_Driver.c | 3 + User/device/djiMotor.c | 185 ++++++++++++++++++++++++++++++-- User/device/djiMotor.h | 58 ++++++++++ User/device/nuc.c | 111 +++++++++++-------- User/device/nuc.h | 8 +- User/module/ball.c | 43 +++++--- User/module/ball.h | 2 +- User/module/dji.c | 46 +++++++- User/module/dji.h | 1 + User/module/go.c | 26 +++-- User/module/go.h | 9 +- User/task/ball_task.c | 13 ++- User/task/dji_task.c | 56 +++++----- User/task/go_task.c | 4 +- User/task/shoot_task.c | 39 ++++++- 16 files changed, 484 insertions(+), 126 deletions(-) diff --git a/User/bsp/can_init.c b/User/bsp/can_init.c index feb34ad..aee2407 100644 --- a/User/bsp/can_init.c +++ b/User/bsp/can_init.c @@ -18,18 +18,18 @@ void can_filter_init(void) can_filter_st.FilterMaskIdHigh = 0x0000; can_filter_st.FilterMaskIdLow = 0x0000; can_filter_st.FilterBank = 0; + can_filter_st.SlaveStartFilterBank = 14; can_filter_st.FilterFIFOAssignment = CAN_RX_FIFO0; HAL_CAN_ConfigFilter(&hcan1, &can_filter_st); HAL_CAN_Start(&hcan1); HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING); - can_filter_st.SlaveStartFilterBank = 14; + can_filter_st.FilterBank = 14; + can_filter_st.FilterFIFOAssignment = CAN_RX_FIFO1; HAL_CAN_ConfigFilter(&hcan2, &can_filter_st); HAL_CAN_Start(&hcan2); HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO1_MSG_PENDING); - - } diff --git a/User/device/GO_M8010_6_Driver.c b/User/device/GO_M8010_6_Driver.c index 2e9a4d4..e5acc7d 100644 --- a/User/device/GO_M8010_6_Driver.c +++ b/User/device/GO_M8010_6_Driver.c @@ -93,6 +93,8 @@ void GO_M8010_send_data(UART_HandleTypeDef *huart, int id,int rev,float T,float return; } + Pos = Pos * (3.14159f / 180.0f); // 角度转弧度 + // assign motor target goal to the buffer motor->motor_send_data.head[0]=0xFE; motor->motor_send_data.head[1]=0xEE; @@ -160,6 +162,7 @@ void GO_M8010_recv_data(uint8_t* Temp_buffer) int32_t Pos = Temp_buffer[10]<<24 | Temp_buffer[9]<<16 | Temp_buffer[8]<<8 | Temp_buffer[7]; motor->Pos = Pos; motor->Pos = (motor->Pos * 6.28319f )/(32768*6.33f); + motor->Pos = motor->Pos * (180.0f / 3.14159f); // 弧度转角度 int8_t Temp = Temp_buffer[11] & 0xFF; motor->Temp = Temp; motor->MError = Temp_buffer[12] & 0x7; diff --git a/User/device/djiMotor.c b/User/device/djiMotor.c index cbb5947..33156d2 100644 --- a/User/device/djiMotor.c +++ b/User/device/djiMotor.c @@ -22,6 +22,7 @@ #include "TopDefine.h" #include #include "FreeRTOS.h" +#include #include "odrive_can.h" @@ -78,7 +79,12 @@ #if DEBUG == 1 + +//电机回传数据结构体 motor_measure_t motor_chassis[5]; +motor_measure_t GM6020_motor[2]; +motor_measure_t motor_2006[2]; +CAN_MotorFeedback_t motor_5065[2]; #else static motor_measure_t motor_chassis[5]; #endif @@ -92,6 +98,41 @@ static uint8_t can_send_data_200[8]; CAN_RxHeaderTypeDef dji_rx_header; uint8_t dji_rx_data[8]; +static CAN_TxHeaderTypeDef vesc_tx_message; +static uint8_t vesc_send_data[4]; + + +/** + * @brief 解析vesc电机反馈数据 + * @param[in,out] feedback : Vesc电机反馈数据结构体指针 + * @param[in] raw : Vesc电机反馈数据的原始数据 + * @retval none + */ +static void CAN_VescMotor_Decode_1(CAN_MotorFeedback_t *feedback, + const uint8_t *raw) +{ + if (feedback == NULL || raw == NULL) return; + union + { + int x; + uint8_t data[4]; + }speed; + + speed.data[0]= raw[3]; + speed.data[1]= raw[2]; + speed.data[2]= raw[1]; + speed.data[3]= raw[0]; + feedback->rotor_speed = speed.x; + union + { + int16_t y; + uint8_t dat[2]; + }current; + current.dat[0]= raw[5]; + current.dat[1]= raw[4]; + feedback->torque_current =(fp32)current.y/10; +} + /** * @brief 数据处理函数 * @param[in] none @@ -101,13 +142,10 @@ void djiMotorEncode() { switch (dji_rx_header.StdId) { - case 0x201: - case 0x202: - case 0x203: - case 0x204: - case 0x205: - case 0x206: - case 0x207: + case M3508_1: + case M3508_2: + case M3508_3: + case M3508_4: { static uint8_t i = 0; //get motor id @@ -121,6 +159,36 @@ void djiMotorEncode() } break; } + case M2006_1: + case M2006_2: + { + static uint8_t i = 0; + //get motor id + i = dji_rx_header.StdId - 0x205; + if(motor_chassis[i].msg_cnt<=50) + { + motor_chassis[i].msg_cnt++; + get_motor_offset(&motor_chassis[i], dji_rx_data); + }else{ + get_2006_motor_measure(&motor_chassis[i], dji_rx_data); + } + break; + } + case GM6020_1: + case GM6020_2: + { + static uint8_t i = 0; + //get motor id + i = dji_rx_header.StdId - 0x207; + if(GM6020_motor[i].msg_cnt<=50) + { + GM6020_motor[i].msg_cnt++; + get_motor_offset(&GM6020_motor[i], dji_rx_data); + }else{ + get_6020_motor_measure(&GM6020_motor[i], dji_rx_data); + } + break; + } default: @@ -129,6 +197,26 @@ void djiMotorEncode() } } } + +void vescMotorEncode() +{ + switch (dji_rx_header.ExtId) { + + case CAN_VESC5065_M1_MSG1: + // 存储消息到对应的电机结构体中 + CAN_VescMotor_Decode_1(&motor_5065[0], dji_rx_data); + break; + + case CAN_VESC5065_M2_MSG1: + // 存储消息到对应的电机结构体中 + CAN_VescMotor_Decode_1(&motor_5065[1], dji_rx_data); + break; + + default: + break; + } + +} #if FREERTOS_DJI == 0 /** * @brief hal库CAN回调函数,接收电机数据 @@ -232,7 +320,7 @@ void CAN_cmd_200(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4, } /** - * @brief 发送电机控制电流(0x205,0x206,0x207,0x208) + * @brief 发送电机控制电流(0x209,0x20A,0x20B) * @param[in] motor1: (0x209) 电机控制电流 * @param[in] motor2: (0x20A) 电机控制电流 * @param[in] motor3: (0x20B) 电机控制电流 @@ -241,7 +329,7 @@ void CAN_cmd_200(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4, void CAN_cmd_2FF(int16_t motor1, int16_t motor2, int16_t motor3, CAN_HandleTypeDef*hcan) { uint32_t send_mail_box; - tx_meessage_2ff.StdId = 0x1FF; + tx_meessage_2ff.StdId = 0x2FF; tx_meessage_2ff.IDE = CAN_ID_STD; tx_meessage_2ff.RTR = CAN_RTR_DATA; tx_meessage_2ff.DLC = 0x08; @@ -265,4 +353,83 @@ motor_measure_t *get_motor_point(uint8_t i) { return &motor_chassis[i]; } +/** + * @brief 返回GM6020电机数据指针 + * @param[in] i: 电机编号 + * @retval 电机数据指针 + */ +motor_measure_t *get_6020_motor_point(uint8_t i) +{ + return &GM6020_motor[i]; +} +/** + * @brief 返回M2006电机数据指针 + * @param[in] i: 电机编号 + * @retval 电机数据指针 + */ +motor_measure_t *get_2006_motor_point(uint8_t i) +{ + return &motor_2006[i]; +} +/** + * @brief 限制vesc电机转速 + * @param[in/out] rpm: vesce电机转速 + * @retval none + * + * 如果rpm的绝对值大于wtrcfg_VESC_COMMAND_ERPM_MAX, + * 则将rpm的值设置为wtrcfg_VESC_COMMAND_ERPM_MAX或-wtrcfg_VESC_COMMAND_ERPM_MAX + */ + void assert_param_rpm(float *rpm){ + if( fabsf(*rpm) > wtrcfg_VESC_COMMAND_ERPM_MAX ) + *rpm = *rpm > 0 ? wtrcfg_VESC_COMMAND_ERPM_MAX : - wtrcfg_VESC_COMMAND_ERPM_MAX ; + } + + /** + * @brief 使vesc电机进入转速模式 + * @param[in] controller_id: vesc电机控制器id + * @param[in] RPM: 电机转速 + * @retval RPM（1000-50000之间的数） + */ + void CAN_VESC_RPM(uint8_t controller_id, float RPM) + { + uint32_t id; + int32_t data; + uint32_t send_mail_box; + + id = controller_id | ((uint32_t)CAN_PACKET_SET_RPM << 8); + assert_param_rpm(&RPM); + data = (int32_t)(RPM); + + vesc_tx_message.ExtId = id; + vesc_tx_message.IDE = CAN_ID_EXT; + vesc_tx_message.RTR = CAN_RTR_DATA; + vesc_tx_message.DLC = 0x04; + vesc_send_data[0] = data >> 24 ; + vesc_send_data[1] = data >> 16 ; + vesc_send_data[2] = data >> 8 ; + vesc_send_data[3] = data ; + + HAL_CAN_AddTxMessage(&hcan1, &vesc_tx_message, vesc_send_data, &send_mail_box); + } + + + /** + * @brief 使vesc电机进入制动模式 + * @param[in] controller_id: vesc电机控制器id + * @retval none + */ + void CAN_VESC_HEAD(uint8_t controller_id) + { + uint32_t id; + uint32_t send_mail_box; + + id = controller_id | ((uint32_t)CAN_PACKET_SET_CURRENT_BRAKE << 8); + + vesc_tx_message.ExtId = id; + vesc_tx_message.IDE = CAN_ID_EXT; + vesc_tx_message.RTR = CAN_RTR_DATA; + vesc_tx_message.DLC = 0x04; + + HAL_CAN_AddTxMessage(&hcan1, &vesc_tx_message, vesc_send_data, &send_mail_box); +} diff --git a/User/device/djiMotor.h b/User/device/djiMotor.h index c07c14d..ab76ad7 100644 --- a/User/device/djiMotor.h +++ b/User/device/djiMotor.h @@ -16,6 +16,18 @@ typedef enum{ M2006 = 2 }motor_type_e; + enum{ + M3508_1 = 0x201, + M3508_2 = 0x202, + M3508_3 = 0x203, + M3508_4 = 0x204, + M2006_1 = 0x205, + M2006_2 = 0x206, + GM6020_1 = 0x207, + GM6020_2 = 0x208, + CAN_VESC5065_M1_MSG1 =0x90a, //vesc的数据回传使用了扩展id，[0:7]为驱动器id，[8:15]为帧类型 + CAN_VESC5065_M2_MSG1 =0x90b, +}; //rm motor data typedef struct @@ -45,13 +57,40 @@ typedef struct } odrive_measure_t; +typedef enum { + CAN_PACKET_SET_DUTY = 0, + CAN_PACKET_SET_CURRENT = 1, + CAN_PACKET_SET_CURRENT_BRAKE =2, + CAN_PACKET_SET_RPM = 3, + CAN_PACKET_SET_POS = 4, + CAN_PACKET_FILL_RX_BUFFER = 5, + CAN_PACKET_FILL_RX_BUFFER_LONG = 6, + CAN_PACKET_PROCESS_RX_BUFFER = 7, + CAN_PACKET_PROCESS_SHORT_BUFFER = 8, + CAN_PACKET_STATUS = 9, + CAN_PACKET_SET_CURRENT_REL = 10, + CAN_PACKET_SET_CURRENT_BRAKE_REL = 11, + CAN_PACKET_SET_CURRENT_HANDBRAKE = 12, + CAN_PACKET_SET_CURRENT_HANDBRAKE_REL = 13 +} CAN_PACKET_ID; //motor calc ecd to angle +/* 电机反馈信息 */ +typedef struct { + float rotor_ecd; + float rotor_speed; + float torque_current; + float temp; +} CAN_MotorFeedback_t; + #define MOTOR_ECD_TO_ANGLE_3508 (360.0 / 8191.0 / (3591.0/187.0)) #define MOTOR_ECD_TO_ANGLE_2006 (360.0 / 8191.0 / 36) #define MOTOR_ECD_TO_RAD 0.000766990394f #define MOTOR_ECD_TO_ANGLE_6020 (360.0 / 8191.0 ) +#define wtrcfg_VESC_COMMAND_ERPM_MAX 35000 +#define CAN_VESC_CTRL_ID_BASE (0x300) + #if FREERTOS_DJI == 1 /** @@ -102,6 +141,19 @@ extern void CAN_cmd_2FF(int16_t motor1, int16_t motor2, int16_t motor3, CAN_Hand */ extern motor_measure_t *get_motor_point(uint8_t i); +/** + * @brief 返回GM6020电机数据指针 + * @param[in] i: 电机编号 + * @retval 电机数据指针 + */ + motor_measure_t *get_6020_motor_point(uint8_t i); + + /** + * @brief 返回M2006电机数据指针 + * @param[in] i: 电机编号 + * @retval 电机数据指针 + */ +motor_measure_t *get_2006_motor_point(uint8_t i); /** * @brief 数据处理函数 @@ -110,6 +162,12 @@ extern motor_measure_t *get_motor_point(uint8_t i); */ void djiMotorEncode(void); +void vescMotorEncode(void); + +void CAN_VESC_RPM(uint8_t controller_id, float RPM); + +void CAN_VESC_HEAD(uint8_t controller_id); + #ifdef __cplusplus } #endif diff --git a/User/device/nuc.c b/User/device/nuc.c index 665b753..b2c725c 100644 --- a/User/device/nuc.c +++ b/User/device/nuc.c @@ -1,6 +1,6 @@ #include "nuc.h" #include -//#include "crc16.h" +#include "crc_ccitt.h" uint8_t nucbuf[32]; @@ -32,60 +32,77 @@ int8_t NUC_Restart(void) { return DEVICE_OK; } + bool_t NUC_WaitDmaCplt(void) { // return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll,500) == // SIGNAL_NUC_RAW_REDY); + return 1; } -int8_t NUC_RawParse(NUC_t *n){ - if(n ==NULL) return DEVICE_ERR_NULL; - union - { - float x[3]; - char data[12]; - }instance;//方便在浮点数和字符数组之间进行数据转换 +int8_t NUC_RawParse(NUC_t *n) { + if (n == NULL) return DEVICE_ERR_NULL; + union { + float x[3]; + char data[12]; + } instance; // 方便在浮点数和字符数组之间进行数据转换 - if(nucbuf[0]!=HEAD) goto error; //发送ID不是底盘 - else{ - n->status_fromnuc =nucbuf[1]; - n->ctrl_status =nucbuf[2]; - switch (n->status_fromnuc) - { - case VISION: - /* 协议格式 - 0xFF HEAD - 0x0X 控制帧 - 0x01 相机帧 - x fp32 - y fp32 - z fp32 - 0xFE TAIL - */ - if(nucbuf[15]!=TAIL)goto error; - instance.data[3] = nucbuf[6]; - instance.data[2] = nucbuf[5]; - instance.data[1] = nucbuf[4]; - instance.data[0] = nucbuf[3]; - n->vision.x= instance.x[0]; // - instance.data[7] = nucbuf[10]; - instance.data[6] = nucbuf[9]; - instance.data[5] = nucbuf[8]; - instance.data[4] = nucbuf[7]; - n->vision.y = instance.x[1];// - instance.data[11] = nucbuf[14]; - instance.data[10] = nucbuf[13]; - instance.data[9] = nucbuf[12]; - instance.data[8] = nucbuf[11]; - n->vision.z = instance.x[2];// - break; - case PICK: - break; - } - return DEVICE_OK; + // 校验数据包头 + if (nucbuf[0] != HEAD) { + return DEVICE_ERR; } - error: - + // 提取数据包中的 CRC 值（假设 CRC 值在数据包的最后两个字节） + uint16_t received_crc = (nucbuf[16] << 8) | nucbuf[17]; // 假设 CRC 在第 16 和 17 字节 + uint16_t calculated_crc = do_crc_table(nucbuf, 16); // 计算前 16 字节的 CRC + // 校验 CRC + if (received_crc == calculated_crc) { + + n->status_fromnuc = nucbuf[1]; + n->ctrl_status = nucbuf[2]; + + switch (n->status_fromnuc) { + case VISION: + /* 协议格式 + 0xFF HEAD + 0x02 控制帧 + 0x01 相机帧 + x fp32 + y fp32 + z fp32 + 0xFE TAIL + */ + if (nucbuf[15] != TAIL) goto error; + + instance.data[3] = nucbuf[3]; + instance.data[2] = nucbuf[4]; + instance.data[1] = nucbuf[5]; + instance.data[0] = nucbuf[6]; + n->vision.x = instance.x[0]; + + instance.data[7] = nucbuf[7]; + instance.data[6] = nucbuf[8]; + instance.data[5] = nucbuf[9]; + instance.data[4] = nucbuf[10]; + n->vision.y = instance.x[1]; + + instance.data[11] = nucbuf[11]; + instance.data[10] = nucbuf[12]; + instance.data[9] = nucbuf[13]; + instance.data[8] = nucbuf[14]; + n->vision.z = instance.x[2]; + break; + + + } + return DEVICE_OK; + } + else + { return DEVICE_ERR; + + } + +error: + return DEVICE_ERR; } diff --git a/User/device/nuc.h b/User/device/nuc.h index 9d0c2d4..e049328 100644 --- a/User/device/nuc.h +++ b/User/device/nuc.h @@ -13,7 +13,7 @@ #define ODOM (0x04) #define PICK (0x06) -#define VISION (0x01) +#define VISION (0x09) //写结构体存入视觉信息 @@ -27,9 +27,9 @@ typedef struct{ struct { - float x; - float y; - float z; + fp32 x; + fp32 y; + fp32 z; }vision; }NUC_t; diff --git a/User/module/ball.c b/User/module/ball.c index 11f6cf1..504a4c3 100644 --- a/User/module/ball.c +++ b/User/module/ball.c @@ -3,10 +3,18 @@ #include "cmsis_os.h" #include "FreeRTOS.h" #include "bsp_delay.h" +#include "go.h" +// PC6 ball_up_Pin读运球 +// PE13 down_Pin 下推 +// PE14 paw_Pin 爪子 +// PI6 ball_in1_Pin 读接球 +// PI7 ball_in2_Pin 读接球 +// PE11 key_Pin 收网 #define ballcome 1 #define balldown 0 +extern GO go; //光电识别 int ball_in(void) { @@ -39,34 +47,34 @@ int pass_ball(void) //运球 -void handling_ball(int hand, int paw) +void handling_ball(int hand, int paw,int angle) { - paw =ball_in(); + GO_TURN_ball(TURN,go,angle);//停止运球 + paw =ball_in();//获取爪子有无球状态 if (hand == 1 && paw == 1) { //爪子 delay_us(100); - HAL_GPIO_WritePin(paw_GPIO_Port, paw_Pin, GPIO_PIN_RESET); - osDelay(2); - delay_us(100); - //上推 - HAL_GPIO_WritePin(down_GPIO_Port, down_Pin, GPIO_PIN_RESET); - hand=0; - } - else - { - - //爪子 HAL_GPIO_WritePin(paw_GPIO_Port, paw_Pin, GPIO_PIN_SET); osDelay(2); delay_us(100); - //下推 + //下推 推下立马收回 HAL_GPIO_WritePin(down_GPIO_Port, down_Pin, GPIO_PIN_SET); + osDelay(2); + HAL_GPIO_WritePin(down_GPIO_Port, down_Pin, GPIO_PIN_RESET); + + hand=0;//清除手指状态 + } + else if(hand == 0 && paw == 1) + { + + //闭合爪子 + HAL_GPIO_WritePin(paw_GPIO_Port, paw_Pin, GPIO_PIN_RESET); + osDelay(2); + } - - } @@ -77,10 +85,13 @@ int catch_ball(int inball) inball =pass_ball(); if(inball==1) { + //接球 + HAL_GPIO_WritePin(key_GPIO_Port, key_Pin, GPIO_PIN_SET); return ballcome; } else { + HAL_GPIO_WritePin(key_GPIO_Port, key_Pin, GPIO_PIN_RESET); return balldown; } diff --git a/User/module/ball.h b/User/module/ball.h index cc2eacf..38c4c55 100644 --- a/User/module/ball.h +++ b/User/module/ball.h @@ -12,7 +12,7 @@ typedef struct ball int ball_in(void); int pass_ball(void); -void handling_ball(int hand, int paw); +void handling_ball(int hand, int paw,int angle); int catch_ball(int inball); diff --git a/User/module/dji.c b/User/module/dji.c index d1048c7..5847a57 100644 --- a/User/module/dji.c +++ b/User/module/dji.c @@ -15,8 +15,6 @@ pid_type_def speed_pid;//3508速度环pid结构体 pid_type_def angle_pid;//3508位置环pid结构体 //pid -//fp32 M3508_speed_PID[3]={10.0,0.3,0.0}; //P,I,D(速度环) -//fp32 M3508_angle_PID[3]={10.0,0.0,1.5}; //P,I,D(角度环) fp32 M3508_speed_PID[3]={5.0,0.3,0.0}; //P,I,D(速度环) fp32 M3508_angle_PID[3]={25.0,0.0,1.5}; //P,I,D(角度环) @@ -30,6 +28,7 @@ float angleSet[MOTOR_NUM]; //位置环 //编码器数据 const motor_measure_t *trigger_motor_data;//3508电机数据 + pid_type_def t_speed_pid;//2006速度环pid结构体 pid_type_def t_angle_pid;//2006位置环pid结构体 @@ -41,14 +40,24 @@ fp32 M2006_PID[3]={4.9,0.01,0.0}; int16_t t_result; //速度环 //can最后发送的数据 float t_angleSet[1]; //位置环 +//GM6020电机数据 +const motor_measure_t *GM6020_motor_data;//3508电机数据 + +//6020pid +pid_type_def speed_6020_pid; +pid_type_def angle_6020_pid; + +const fp32 PID_6020_angle[3] = {50, 0.1, 0}; +const fp32 PID_6020_speed[3]={5,0.01,0}; + void motor_init(void) { // motor_3508_data=get_motor_point(0); trigger_motor_data=get_motor_point(0); -// PID_init(&speed_pid,PID_POSITION,M3508_speed_PID,3000,1000); -// PID_init(&angle_pid,PID_POSITION,M3508_angle_PID,7000,2000); + GM6020_motor_data=get_6020_motor_point(0); + // PID_init(&speed_pid,PID_POSITION,M3508_speed_PID,500000,500000); // PID_init(&angle_pid,PID_POSITION,M3508_angle_PID,500000,100000); @@ -56,6 +65,11 @@ void motor_init(void) PID_init(&t_speed_pid,PID_POSITION,M2006_speed_PID,500000,500000); PID_init(&t_angle_pid,PID_POSITION,M2006_angle_PID,500000,100000); + + PID_init(&speed_6020_pid,PID_POSITION,PID_6020_speed,15000,15000);//6020 pid初始化 + PID_init(&angle_6020_pid,PID_POSITION,PID_6020_angle,600,500); + + } float trigger_angle = 0; @@ -67,6 +81,9 @@ void trigger_pos(fp32 angle) delta[0]=PID_calc(&t_angle_pid,trigger_motor_data->total_angle,angle); t_result=PID_calc(&t_speed_pid,trigger_motor_data->speed_rpm,delta[0]); + CAN_cmd_200(t_result,0,0,0,&hcan1); + osDelay(2); + } @@ -86,3 +103,24 @@ void motor_pos(fp32 angle) } +//6020控制 +void my_GM6020_control(fp32 angle) +{ + + fp32 angle_get; + fp32 my_angle; + int32_t M6020_speed; + + //6020串级 + angle_get= angle; + + //外环角度环 + my_angle=PID_calc(&angle_6020_pid,GM6020_motor_data->total_angle,angle_get);//角度环 + //内环速度环 + M6020_speed=PID_calc(&speed_6020_pid,GM6020_motor_data->speed_rpm, my_angle);//速度环 + + CAN_cmd_1FF(M6020_speed, M6020_speed, M6020_speed, M6020_speed,&hcan1); + osDelay(2); + +} + diff --git a/User/module/dji.h b/User/module/dji.h index 49d4b5a..80598fb 100644 --- a/User/module/dji.h +++ b/User/module/dji.h @@ -20,5 +20,6 @@ typedef enum void motor_pos(fp32 angle); void trigger_pos(fp32 angle); + void my_GM6020_control(fp32 angle); #endif diff --git a/User/module/go.c b/User/module/go.c index aa971ac..63f01c6 100644 --- a/User/module/go.c +++ b/User/module/go.c @@ -4,7 +4,7 @@ #include "calc_lib.h" #include "FreeRTOS.h" #include -#define KP 0.12 +#define KP 0.9 #define KD 0.008 //可活动角度 #define ANGLE_ALLOW 0.3f @@ -13,7 +13,7 @@ extern RC_mess_t RC_mess; GO go; //初始化 -void gimbalInit(void) +void GO_Init(void) { int i; GO_M8010_init(); @@ -38,22 +38,30 @@ void gimbalInit(void) void gimbalFlow(void) { - //用485模块发送数据有问题 - GO_M8010_send_data(&huart1, 0,0,0,go.angleSetgo[0],1,KP,KD); + go.angleSetgo[0]=1; //串口6发送数据没有问题 - GO_M8010_send_data(&huart6, 0,0,0,go.angleSetgo[0],1,KP,KD); + // GO_M8010_send_data(&huart6, 0,0,0,go.angleSetgo[0],1,KP,KD); GO_M8010_send_data(&huart6, 1,0,0,go.angleSetgo[0],1,KP,KD); - GO_M8010_send_data(&huart6, 2,0,0,go.angleSetgo[0],1,KP,KD); +// GO_M8010_send_data(&huart6, 2,0,0,go.angleSetgo[0],1,KP,KD); osDelay(1); } - void gimbalZero(void) { - GO_M8010_send_data(&huart1, 0,0,0,0,0,0,0); + GO_M8010_send_data(&huart6, 0,0,0,0,0,0,0); osDelay(1); - GO_M8010_send_data(&huart1, 1,0,0,0,0,0,0); + GO_M8010_send_data(&huart6, 1,0,0,0,0,0,0); +} + +void GO_TURN_ball(GO_ID_t go_id,GO go_set,int angle) +{ + + go_set.angleSetgo[go_id] = angle; + go_set.Kp = KP; + go_set.Kd = KD; + GO_M8010_send_data(&huart6, go_id,0,0,go_set.angleSetgo[go_id],1,go_set.Kp,go_set.Kd); + } diff --git a/User/module/go.h b/User/module/go.h index 21da674..c946f3f 100644 --- a/User/module/go.h +++ b/User/module/go.h @@ -3,6 +3,12 @@ #include "GO_M8010_6_Driver.h" +typedef enum{ + TURN = 0, + GIMBAL = 1, + OTHER = 2 + }GO_ID_t; + typedef struct { /* data */ @@ -16,9 +22,10 @@ typedef struct float allowRange; }GO; -void gimbalInit(void); +void GO_Init(void); void gimbalFlow(void); void gimbalZero(void); +void GO_TURN_ball(GO_ID_t go_id,GO go_set,int angle); #endif diff --git a/User/task/ball_task.c b/User/task/ball_task.c index e02ee5e..730ae1c 100644 --- a/User/task/ball_task.c +++ b/User/task/ball_task.c @@ -9,6 +9,8 @@ extern RC_mess_t RC_mess; BASKETBALL basketball; +int bb=0; +int aa=8; void Task_Ball(void *argument) { (void)argument; /* 未使用argument，消除警告 */ @@ -20,7 +22,16 @@ void Task_Ball(void *argument) while(1) { - handling_ball(basketball.hand, basketball.paw); + // handling_ball(basketball.hand, basketball.paw,aa); + if(bb==1) + { + HAL_GPIO_WritePin(key_GPIO_Port, key_Pin, GPIO_PIN_SET); + } + else + { + HAL_GPIO_WritePin(key_GPIO_Port, key_Pin, GPIO_PIN_RESET); + } + tick += delay_tick; /* 计算下一个唤醒时刻 */ osDelayUntil(tick); /* 运行结束，等待下一个周期唤醒 */ diff --git a/User/task/dji_task.c b/User/task/dji_task.c index f55296f..367eced 100644 --- a/User/task/dji_task.c +++ b/User/task/dji_task.c @@ -20,6 +20,7 @@ int speed=0; float m=0; float trigger_angle_o=0; int mode=0; +int angle_6020=0; /** * \brief 电机任务 * @@ -38,39 +39,40 @@ void Task_Motor(void *argument) while(1) { - //收到消息队列新数据 + // //收到消息队列新数据 - // 更新编码器数据 - Update_Encoder(&encoder); - m=trigger_angle_o*(8191/360); - trigger_angle_o=-2.5;// - if( mode == THREE ) - { + // // 更新编码器数据 + // Update_Encoder(&encoder); + // m=trigger_angle_o*(8191/360); + // trigger_angle_o=-2.5;// + // if( mode == THREE ) + // { - //当最高点时进入离合 - if(encoder.round_cnt>=6) - { - trigger_angle_o=-2.5; - trigger_pos(m); - } - else - { - trigger_angle_o=0; - trigger_pos(m); - } + // //当最高点时进入离合 + // if(encoder.round_cnt>=6) + // { + // trigger_angle_o=-2.5; + // trigger_pos(m); + // } + // else + // { + // trigger_angle_o=0; + // trigger_pos(m); + // } - } - else if( mode == DZ ) - { - //退出离合 - trigger_angle_o=0; - trigger_pos(m); + // } + // else if( mode == DZ ) + // { + // //退出离合 + // trigger_angle_o=0; + // trigger_pos(m); - } + // } + // my_GM6020_control(angle_6020); - CAN_cmd_200(t_result,0,0,0,&hcan1); - osDelay(2); + // CAN_cmd_200(t_result,0,0,0,&hcan1);//在module写好 + // osDelay(2); // vofa[0]=motor_3508_data->speed_rpm; // vofa[1]=speed; diff --git a/User/task/go_task.c b/User/task/go_task.c index 2a8cea0..5a7eef1 100644 --- a/User/task/go_task.c +++ b/User/task/go_task.c @@ -16,7 +16,7 @@ void Task_Go(void *argument) #if GOUSE==1 HAL_Delay(2000); - gimbalInit(); + GO_Init(); HAL_GPIO_WritePin(LED_G_GPIO_Port, LED_G_Pin,GPIO_PIN_RESET); while(1) @@ -24,7 +24,7 @@ void Task_Go(void *argument) //LED_green(); //LED_bule(); - gimbalFlow(); + // gimbalFlow(); osDelay(1); } #else diff --git a/User/task/shoot_task.c b/User/task/shoot_task.c index bafd11e..c664a1b 100644 --- a/User/task/shoot_task.c +++ b/User/task/shoot_task.c @@ -7,15 +7,22 @@ #include "shoot.h" #include "odrive_shoot.h" #include "read_spi.h" -#include "dji_task.h" +#include "dji.h" +#include "go.h" extern RC_mess_t RC_mess; extern Encoder_t encoder; extern motor_measure_t *trigger_motor_data;//3508电机数据 extern int mode; +extern GO go; +int shoot_flag = 0; +int trigger_flag = 0; +int pos=0; +int gopos=0; #define GO1_SHOOT 0 -#define ODRIVE_SHOOT 1 +#define ODRIVE_SHOOT 0 +#define VESC_SHOOT 1 //odrive发射 #define SHOOT3 12 @@ -34,6 +41,9 @@ void Task_Shoot(void *argument) uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计算 */ + go.angleSetgo[0] = 0; //id暂时未知 + GO_TURN_ball(TURN,go,gopos); //发射舵机位置 + osDelay(1000); while(1) { #if GO1_SHOOT == 1 @@ -54,8 +64,33 @@ void Task_Shoot(void *argument) } } + #endif + #if VESC_SHOOT == 1 + gopos=35; + GO_TURN_ball(TURN,go,gopos); //发射舵机位置 + if(shoot_flag==1) + { + CAN_VESC_RPM(0, 10000); //发射电机转速 + CAN_VESC_RPM(1, 10000); //发射电机转速 + if(trigger_flag == 1) + { + trigger_pos(pos); //扳机电机转速 + trigger_flag = 0; + } + + + } + else if(shoot_flag == 0) + { + CAN_VESC_RPM(0, 0); //发射电机转速 + CAN_VESC_RPM(1, 0); //发射电机转速 + trigger_pos(0); //扳机电机转速 + } + + + #endif tick += delay_tick; /* 计算下一个唤醒时刻 */ osDelayUntil(tick); /* 运行结束，等待下一个周期唤醒 */ }