diff --git a/User/device/GO_M8010_6_Driver.c b/User/device/GO_M8010_6_Driver.c index e5acc7d..ce75d6a 100644 --- a/User/device/GO_M8010_6_Driver.c +++ b/User/device/GO_M8010_6_Driver.c @@ -46,12 +46,11 @@ void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) 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); //蓝色灯亮 // indicate CRC incorrect - HAL_GPIO_WritePin(LED_R_GPIO_Port, LED_G_Pin, GPIO_PIN_SET); //红色灯灭 return; } // CRC pass and start converting data to the motor - HAL_GPIO_WritePin(GPIOH, GPIO_PIN_11, GPIO_PIN_SET); // indicate CRC correct - HAL_GPIO_WritePin(LED_R_GPIO_Port, LED_G_Pin, GPIO_PIN_RESET); //红色灯灭 + HAL_GPIO_WritePin(LED_B_GPIO_Port, LED_B_Pin, GPIO_PIN_RESET); //蓝色灯亮 // indicate CRC incorrect + GO_M8010_recv_data(Temp_buffer); } diff --git a/User/device/djiMotor.c b/User/device/djiMotor.c index 33156d2..a0b42a7 100644 --- a/User/device/djiMotor.c +++ b/User/device/djiMotor.c @@ -118,10 +118,10 @@ static void CAN_VescMotor_Decode_1(CAN_MotorFeedback_t *feedback, 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]; + speed.data[0]= raw[0]; + speed.data[1]= raw[1]; + speed.data[2]= raw[2]; + speed.data[3]= raw[3]; feedback->rotor_speed = speed.x; union { @@ -165,12 +165,12 @@ void djiMotorEncode() static uint8_t i = 0; //get motor id i = dji_rx_header.StdId - 0x205; - if(motor_chassis[i].msg_cnt<=50) + if(motor_2006[i].msg_cnt<=50) { - motor_chassis[i].msg_cnt++; - get_motor_offset(&motor_chassis[i], dji_rx_data); + motor_2006[i].msg_cnt++; + get_motor_offset(&motor_2006[i], dji_rx_data); }else{ - get_2006_motor_measure(&motor_chassis[i], dji_rx_data); + get_2006_motor_measure(&motor_2006[i], dji_rx_data); } break; } diff --git a/User/device/djiMotor.h b/User/device/djiMotor.h index ab76ad7..1dab77b 100644 --- a/User/device/djiMotor.h +++ b/User/device/djiMotor.h @@ -25,8 +25,8 @@ typedef enum{ 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, + CAN_VESC5065_M1_MSG1 =0x1001, //vesc的数据回传使用了扩展id,[0:7]为驱动器id,[8:15]为帧类型 + CAN_VESC5065_M2_MSG1 =0x1002, }; //rm motor data diff --git a/User/device/remote_control.c b/User/device/remote_control.c index 40dbe10..0d3fd7c 100644 --- a/User/device/remote_control.c +++ b/User/device/remote_control.c @@ -202,6 +202,8 @@ void RC_init(uint8_t *rx1_buf, uint8_t *rx2_buf, uint16_t dma_buf_num) #else //DMA双缓冲区+串口空闲中断 +// 02 53 44 1x1 3x2 + static osEventFlagsId_t eventReceive; RC_mess_t RC_mess; diff --git a/User/module/ball.c b/User/module/ball.c index 504a4c3..765186f 100644 --- a/User/module/ball.c +++ b/User/module/ball.c @@ -5,7 +5,7 @@ #include "bsp_delay.h" #include "go.h" -// PC6 ball_up_Pin读运球 +// PC6 ball_up_Pin读运球 检测到输出0,未检测到输出1 // PE13 down_Pin 下推 // PE14 paw_Pin 爪子 // PI6 ball_in1_Pin 读接球 @@ -18,67 +18,46 @@ extern GO go; //光电识别 int ball_in(void) { - if (HAL_GPIO_ReadPin(ball_up_GPIO_Port, ball_up_Pin) == GPIO_PIN_SET) - { - return ballcome; - } - else - { - return balldown; - } - - + return (HAL_GPIO_ReadPin(ball_up_GPIO_Port, ball_up_Pin)); } + int pass_ball(void) { - if (HAL_GPIO_ReadPin(ball_in1_GPIO_Port, ball_in1_Pin) == GPIO_PIN_SET) - { - return ballcome; - } - else - { - return balldown; - } + return (HAL_GPIO_ReadPin(ball_in1_GPIO_Port, ball_in1_Pin)); } - - - //运球 -void handling_ball(int hand, int paw,int angle) +void handling_ball(int hand, int angle) { GO_TURN_ball(TURN,go,angle);//停止运球 - paw =ball_in();//获取爪子有无球状态 - if (hand == 1 && paw == 1) + int ball_state =0;//有球 + int paw_state =0;//获取爪子开合状态 + if( hand==1) { - //爪子 - delay_us(100); 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;//清除手指状态 + paw_state =1;//爪子开 + // osDelay(5); + HAL_GPIO_WritePin(down_GPIO_Port, down_Pin, GPIO_PIN_RESET);//打出 + ball_state =1;//无球 + osDelay(500); + HAL_GPIO_WritePin(down_GPIO_Port, down_Pin, GPIO_PIN_SET);//收回 + ball_state =ball_in();//获取爪子有无球状态 } - else if(hand == 0 && paw == 1) + if (paw_state==1&&ball_state==0) { - - //闭合爪子 - HAL_GPIO_WritePin(paw_GPIO_Port, paw_Pin, GPIO_PIN_RESET); - osDelay(2); + HAL_GPIO_WritePin(paw_GPIO_Port, paw_Pin, GPIO_PIN_RESET);//爪子合 + osDelay(500); + ball_state=1;//有球清除标志 + paw_state=0;//爪子合清除标志 + hand=0; } - + } - - //接球 int catch_ball(int inball) { diff --git a/User/module/ball.h b/User/module/ball.h index 38c4c55..263daa7 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,int angle); +void handling_ball(int hand, int angle); int catch_ball(int inball); diff --git a/User/module/dji.c b/User/module/dji.c index 5847a57..830dd5e 100644 --- a/User/module/dji.c +++ b/User/module/dji.c @@ -16,14 +16,11 @@ pid_type_def angle_pid;//3508位置环pid结构体 //pid -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(角度环) +fp32 M3508_speed_PID[3]={50.0,0.3,0.0}; //P,I,D(速度环) +fp32 M3508_angle_PID[3]={50.0,0.0,1.5}; //P,I,D(角度环) //速度环pid参数 fp32 M3508_PID[3]={4.9,0.01,0.0}; -int16_t result; //速度环 -float angleSet[MOTOR_NUM]; //位置环 - //trigger //编码器数据 const motor_measure_t *trigger_motor_data;//3508电机数据 @@ -32,14 +29,12 @@ const motor_measure_t *trigger_motor_data;//3508电机数据 pid_type_def t_speed_pid;//2006速度环pid结构体 pid_type_def t_angle_pid;//2006位置环pid结构体 -fp32 M2006_speed_PID[3]={15.0,0.3,0.0}; //P,I,D(速度环) -fp32 M2006_angle_PID[3]={25.0,0.0,1.5}; //P,I,D(角度环) +fp32 M2006_speed_PID[3]={10.0,0.1,0.0}; //P,I,D(速度环) +fp32 M2006_angle_PID[3]={5.0,0.0,0.05}; //P,I,D(角度环) //速度环pid参数 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电机数据 @@ -55,24 +50,22 @@ 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); + trigger_motor_data=get_2006_motor_point(0); 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); - - 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(&t_speed_pid,PID_POSITION,M2006_speed_PID,1600,600); + PID_init(&t_angle_pid,PID_POSITION,M2006_angle_PID,500,200); - 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); + PID_init(&speed_6020_pid,PID_POSITION,PID_6020_speed,16000, 6000);//6020 pid初始化 + PID_init(&angle_6020_pid,PID_POSITION,PID_6020_angle,5000, 2000); + t_result=0; } -float trigger_angle = 0; + void trigger_pos(fp32 angle) { @@ -81,27 +74,10 @@ 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); } - -void motor_speed(fp32 speed) -{ - - result=PID_calc(&speed_pid,motor_3508_data->speed_rpm,speed); -} - -void motor_pos(fp32 angle) -{ - int16_t delta[1]; - - delta[0]=PID_calc(&angle_pid,motor_3508_data->total_angle,angle); - result=PID_calc(&speed_pid,motor_3508_data->speed_rpm,delta[0]); - -} //6020控制 void my_GM6020_control(fp32 angle) diff --git a/User/module/dji.h b/User/module/dji.h index 80598fb..ed0bda3 100644 --- a/User/module/dji.h +++ b/User/module/dji.h @@ -15,9 +15,9 @@ typedef enum }motorput_e; +//float t_angleSet[1]; //位置环 + void motor_init(void); - void motor_speed(fp32 speed); - void motor_pos(fp32 angle); void trigger_pos(fp32 angle); void my_GM6020_control(fp32 angle); diff --git a/User/module/go.c b/User/module/go.c index 63f01c6..4837237 100644 --- a/User/module/go.c +++ b/User/module/go.c @@ -23,11 +23,9 @@ void GO_Init(void) go.angleSetgo[i] = 0; go.offestAngle[i] = 0; - // GO_M8010_send_data(&huart1, i,0,0,0,0,0,0); GO_M8010_send_data(&huart6, i,0,0,0,0,0,0); go.offestAngle[i] = go.goData[i]->Pos; HAL_Delay(100); - // GO_M8010_send_data(&huart1, i,0,0,0,0,0,0); GO_M8010_send_data(&huart6, i,0,0,0,0,0,0); go.offestAngle[i] = go.goData[i]->Pos; HAL_Delay(100); @@ -43,7 +41,7 @@ void gimbalFlow(void) //串口6发送数据没有问题 // 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); diff --git a/User/module/go.h b/User/module/go.h index c946f3f..28969ac 100644 --- a/User/module/go.h +++ b/User/module/go.h @@ -4,8 +4,8 @@ #include "GO_M8010_6_Driver.h" typedef enum{ - TURN = 0, - GIMBAL = 1, + GIMBAL = 0, + TURN = 1, OTHER = 2 }GO_ID_t; diff --git a/User/module/led.c b/User/module/led.c index c803071..3058287 100644 --- a/User/module/led.c +++ b/User/module/led.c @@ -4,10 +4,9 @@ void LED_red(void) { - //GPIO_PIN_12 - //HAL_GPIO_TogglePin(ledBlue_GPIO_Port, ledBlue_Pin); + //GPIO_PIN_12 HAL_GPIO_WritePin(LED_R_GPIO_Port, LED_R_Pin, GPIO_PIN_SET); - osDelay(1000); + osDelay(500); HAL_GPIO_WritePin(LED_R_GPIO_Port, LED_R_Pin, GPIO_PIN_RESET); } diff --git a/User/module/shoot.c b/User/module/shoot.c index b0af48a..e8d0cc8 100644 --- a/User/module/shoot.c +++ b/User/module/shoot.c @@ -1,90 +1,14 @@ #include "shoot.h" #include "remote_control.h" +#include "go.h" + extern RC_mess_t RC_mess; extern motor_measure_t *trigger_motor_data;//3508电机数据 - - -#define GO1_SHOOT 0 -#define ODRIVE_SHOOT 1 +extern GO go; #define KP 0.12 #define KD 0.008 -#if GO1_SHOOT == 1 - GO_SHOOT goshoot; - -void shooterInit(void) -{ - int i; - GO_M8010_init(); - for(i = 0;i < GO_NUM;i ++) - { - goshoot.goData[i] = getGoPoint(i);//获取电机数据指针 - - goshoot.angleSetgo[i] = 0; - goshoot.offestAngle[i] = 0; - // GO_M8010_send_data(&huart1, i,0,0,0,0,0,0); - GO_M8010_send_data(&huart6, i,0,0,0,0,0,0); - goshoot.offestAngle[i] = goshoot.goData[i]->Pos; - HAL_Delay(100); - // GO_M8010_send_data(&huart1, i,0,0,0,0,0,0); - GO_M8010_send_data(&huart6, i,0,0,0,0,0,0); - goshoot.offestAngle[i] = goshoot.goData[i]->Pos; - HAL_Delay(100); - } - -} - - -void shoot_ball(int key) -{ - - - //蓄力 - if(trigger_motor_data->real_angle ==60)//扳机已闭合 - { - goshoot.angleSetgo[0] = 10; - GO_M8010_send_data(&huart6, 0,0,0,goshoot.angleSetgo[0],1,KP,KD); - } - if (key ==2)//上升准备蓄力 - { - goshoot.angleSetgo[0] = 0; - GO_M8010_send_data(&huart6, 0,0,0,goshoot.angleSetgo[0],1,KP,KD); - } - - -} - - -#elif ODRIVE_SHOOT == 1 -//初始位置在最上面, 有球放过来, 电机转动到最下面等待发射 -//发射指令,当到最高点时,扳机离合 -//返回扳机,远动到最下面 - -extern float angle_encoder; -extern volatile int32_t multi_turn_angle ; -extern volatile uint16_t last_angle ; - -void shoot_odrive(int angle) -{ - - odrive_accel_cmd(AXIS0_NODE,500,500); - osDelay(2); - odrive_pos_cmd(AXIS0_NODE,angle); - -} - -void shoot_back(int angle) -{ - - odrive_accel_cmd(AXIS0_NODE,100,100); - osDelay(2); - odrive_pos_cmd(AXIS0_NODE,angle); - -} - - -#endif diff --git a/User/module/shoot.h b/User/module/shoot.h index e4386f4..0aa1da4 100644 --- a/User/module/shoot.h +++ b/User/module/shoot.h @@ -20,9 +20,6 @@ typedef struct float allowRange; }GO_SHOOT; -void shooterInit(void); -void shoot_ball(int key); -void shoot_odrive(int angle); -void shoot_back(int angle); + #endif diff --git a/User/task/ball_task.c b/User/task/ball_task.c index 730ae1c..6a12ef7 100644 --- a/User/task/ball_task.c +++ b/User/task/ball_task.c @@ -5,12 +5,15 @@ #include #include "remote_control.h" #include "ball.h" +#include "go.h" + extern RC_mess_t RC_mess; +extern GO go; BASKETBALL basketball; int bb=0; -int aa=8; +int Turn_pos=0; void Task_Ball(void *argument) { (void)argument; /* 未使用argument,消除警告 */ @@ -18,18 +21,27 @@ void Task_Ball(void *argument) uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计算 */ basketball.hand=0; - basketball.paw=0; + Turn_pos=20; + GO_TURN_ball(TURN,go,Turn_pos); //发射舵机位置 + HAL_GPIO_WritePin(down_GPIO_Port, down_Pin, GPIO_PIN_SET);//收回 while(1) { - // handling_ball(basketball.hand, basketball.paw,aa); + // Turn_pos=20; + + handling_ball(basketball.hand,Turn_pos); + bb=ball_in(); if(bb==1) { - HAL_GPIO_WritePin(key_GPIO_Port, key_Pin, GPIO_PIN_SET); + // HAL_GPIO_WritePin(key_GPIO_Port, key_Pin, GPIO_PIN_SET); + // HAL_GPIO_WritePin(paw_GPIO_Port, paw_Pin, GPIO_PIN_SET); + // HAL_GPIO_WritePin(down_GPIO_Port, down_Pin, GPIO_PIN_SET); } else { - HAL_GPIO_WritePin(key_GPIO_Port, key_Pin, GPIO_PIN_RESET); + // HAL_GPIO_WritePin(key_GPIO_Port, key_Pin, GPIO_PIN_RESET); + // HAL_GPIO_WritePin(paw_GPIO_Port, paw_Pin, GPIO_PIN_RESET); + // HAL_GPIO_WritePin(down_GPIO_Port, down_Pin, GPIO_PIN_RESET); } diff --git a/User/task/can_task.c b/User/task/can_task.c index 9ea03e6..7cb0252 100644 --- a/User/task/can_task.c +++ b/User/task/can_task.c @@ -25,7 +25,7 @@ void Task_Can(void *argument) { waitNewDji();//等待新数据 djiMotorEncode();//数据解析 - + vescMotorEncode();//数据解析 //将can数据添加到消息队列 diff --git a/User/task/dji_task.c b/User/task/dji_task.c index 367eced..65a7f02 100644 --- a/User/task/dji_task.c +++ b/User/task/dji_task.c @@ -7,20 +7,17 @@ #include "dji.h" #include "read_spi.h" #include "vofa.h" - +#include "buzzer.h" extern RC_mess_t RC_mess; -extern int16_t result; + extern int16_t t_result; extern motor_measure_t *trigger_motor_data; Encoder_t encoder; -float vofa[8]; -int speed=0; -float m=0; -float trigger_angle_o=0; -int mode=0; -int angle_6020=0; +int shoot_f=0; +int pos2006=0; + /** * \brief 电机任务 * @@ -39,44 +36,23 @@ void Task_Motor(void *argument) while(1) { - // //收到消息队列新数据 + + if(RC_mess.RC_data.sw[4]==306||shoot_f==3) + { + pos2006=200; + trigger_pos(pos2006); + + } + else if(RC_mess.RC_data.sw[4]==1694) + { + pos2006=0; + trigger_pos(pos2006); + } + - // // 更新编码器数据 - // 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); - // } - - // } - // 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);//在module写好 - // osDelay(2); - -// vofa[0]=motor_3508_data->speed_rpm; -// vofa[1]=speed; -// vofa_tx_main(vofa); + CAN_cmd_1FF(t_result,0,0,0,&hcan1); + osDelay(1); tick += delay_tick; osDelayUntil(tick); diff --git a/User/task/go_task.c b/User/task/go_task.c index 5a7eef1..49f8d2d 100644 --- a/User/task/go_task.c +++ b/User/task/go_task.c @@ -15,7 +15,7 @@ void Task_Go(void *argument) { #if GOUSE==1 - HAL_Delay(2000); + //HAL_Delay(2000); GO_Init(); HAL_GPIO_WritePin(LED_G_GPIO_Port, LED_G_Pin,GPIO_PIN_RESET); @@ -25,7 +25,7 @@ void Task_Go(void *argument) //LED_green(); //LED_bule(); // gimbalFlow(); - osDelay(1); + } #else while(1) diff --git a/User/task/led_task.c b/User/task/led_task.c index 58f61ad..7d9e595 100644 --- a/User/task/led_task.c +++ b/User/task/led_task.c @@ -4,17 +4,16 @@ #include "main.h" #include "led.h" #include "buzzer.h" - +int a1=0; void Task_Led(void *argument) { (void)argument; /* 未使用argument,消除警告 */ - for (;;) { + while(1) { // LED_red(); - // LED_bule(); - // see_you_again(); - LED_green(); - - osDelay(500); + // LED_bule(); + LED_green(); + osDelay(1); + } } diff --git a/User/task/rc_task.c b/User/task/rc_task.c index 52d525f..407d9c2 100644 --- a/User/task/rc_task.c +++ b/User/task/rc_task.c @@ -7,6 +7,7 @@ #include "remote_control.h" extern RC_mess_t RC_mess; +// sw[0]2 下306上1694 sw[5]3前306后1694 sw[4]4前1694后306 sw[1]xuan1 sw[3]xuan2 /** * \brief 遥控器任务 *