diff --git a/MDK-ARM/AUTO_CHASSIS.uvoptx b/MDK-ARM/AUTO_CHASSIS.uvoptx index 969da2f..564be7f 100644 --- a/MDK-ARM/AUTO_CHASSIS.uvoptx +++ b/MDK-ARM/AUTO_CHASSIS.uvoptx @@ -103,7 +103,7 @@ 1 0 0 - 6 + 3 @@ -114,7 +114,7 @@ - STLink\ST-LINKIII-KEIL_SWO.dll + BIN\CMSIS_AGDI.dll @@ -140,12 +140,12 @@ 0 CMSIS_AGDI - -X"Horco CMSIS-DAP" -U8626380832 -O206 -S0 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO65554 -TC168000000 -TT168000000 -TP20 -TDS8010 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM) + -X"Horco CMSIS-DAP" -U4626385832 -O206 -S0 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO65554 -TC168000000 -TT168000000 -TP20 -TDS8010 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM) 0 DLGUARM - (105=-1,-1,-1,-1,0) + 0 @@ -158,87 +158,32 @@ 0 1 - raw,0x0A + UP,0x0A 1 1 - UP,0x0A + GO_motor_info 2 1 - can,0x0A + dr16,0x0A 3 1 - can_out,0x0A + cmd,0x0A 4 1 - bbb,0x0A + rc_ctrl,0x0A 5 1 - aaa,0x0A - - - 6 - 1 - CCC - - - 7 - 1 - GO_motor_info - - - 8 - 1 - cbuf[36] - - - 9 - 1 - dr16,0x0A - - - 10 - 1 - cmd,0x0A - - - 11 - 1 - rc_ctrl,0x0A - - - 12 - 1 - up_cmd,0x0A - - - 13 - 1 - angle - - - 14 - 1 - can_ZZ - - - 15 - 1 - flaggg,0x0A - - - 16 - 1 - ddd + aaa diff --git a/MDK-ARM/ELE_CHASSIS/AUTO_CHASSIS.axf b/MDK-ARM/ELE_CHASSIS/AUTO_CHASSIS.axf index dc51a86..9794aa3 100644 Binary files a/MDK-ARM/ELE_CHASSIS/AUTO_CHASSIS.axf and b/MDK-ARM/ELE_CHASSIS/AUTO_CHASSIS.axf differ diff --git a/User/Algorithm/user_math.c b/User/Algorithm/user_math.c index 8b7dec0..4d5edbb 100644 --- a/User/Algorithm/user_math.c +++ b/User/Algorithm/user_math.c @@ -214,6 +214,14 @@ void abs_limit_fp(fp32 *num, fp32 Limit) } } +void limit(double *a, double b, double c) { + if (*a < b) { + *a = b; // 如果 a 小于下限 b，将 a 设置为 b + } else if (*a > c) { + *a = c; // 如果 a 大于上限 c，将 a 设置为 c + } + // 如果 a 在范围 [b, c] 内，不做任何修改 +} /* 移动向量 */ MoveVector_t *mv; diff --git a/User/Algorithm/user_math.h b/User/Algorithm/user_math.h index e66dd23..dc87c6c 100644 --- a/User/Algorithm/user_math.h +++ b/User/Algorithm/user_math.h @@ -159,5 +159,6 @@ PolarCoordinate_t addPolarVectors(PolarCoordinate_t v1, PolarCoordinate_t v2); uint8_t average(uint8_t arr[], uint8_t n); /*判断是否在误差内*/ bool is_arrived(float target, float current, float mistake) ; +void limit(double *a, double b, double c) ; #endif diff --git a/User/Module/config.c b/User/Module/config.c index e68cdfe..26fb7e9 100644 --- a/User/Module/config.c +++ b/User/Module/config.c @@ -68,17 +68,26 @@ static const ConfigParam_t param_chassis ={ .go_param[0]={ .rev = 0, - .T=0.1, + .T=0.79, .W=0.1, - .K_P=0.1, + .K_P=0.2, .K_W=0.1, }, .go_param[1]={ .rev = 0, - .T=0.1, - .W=0.1, - .K_P=0.20f, + .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 + + }, /*上层其他参数*/ /*运球*/ @@ -96,6 +105,7 @@ static const ConfigParam_t param_chassis ={ .m2006_trigger_angle =0, .go1_init_position = -500, .go1_release_threshold =-1900, + .m2006_Screw_init=0 }, diff --git a/User/Module/up.c b/User/Module/up.c index 9d37a0d..a50dc90 100644 --- a/User/Module/up.c +++ b/User/Module/up.c @@ -34,10 +34,10 @@ 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 )); - - 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 ); - } + 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 ); +// } // 初始化上层状态机 @@ -197,8 +197,10 @@ int8_t GM6020_control(UP_t *u,fp32 angle) /*go电机控制*/ int8_t GO_SendData(UP_t *u,int id,float 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 ,AngleChange(RADIAN,pos),u->param->go_param [id].K_P ,u->param->go_param[id] .K_W ); + 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 ); return 0; } @@ -215,7 +217,6 @@ int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out) GO_SendData(u,0 ,u->motor_target .go_shoot ); - GO_SendData(u,1 ,u->motor_target .go_spin); for(int i=0;i<4;i++){ @@ -239,7 +240,7 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c) { if(u ==NULL) return 0; - + static int is_pitch=1; switch (c->CMD_CtrlType ) { case UP_RCcontrol: //在手动模式下 @@ -249,19 +250,22 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c) case Normal : /*投篮*/ + if(is_pitch){ u->motor_target .go_shoot =u->PitchContext .PitchConfig .go1_init_position ; + is_pitch=0; + } u->motor_target .M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ; u->PitchContext .PitchState = PITCH_PREPARE; //状态更新,开始夹球 - /*运球*/ - u->motor_target .go_spin = u->DribbleContext.DribbleConfig .go2_init_angle ; - u->motor_target .M3508_angle =u->DribbleContext .DribbleConfig .m3508_init_angle ; - u->DribbleContext .DribbleState = Dribble_PREPARE; //重置最初状态 +// /*运球*/ +// u->motor_target .go_spin = u->DribbleContext.DribbleConfig .go2_init_angle ; +// u->motor_target .M3508_angle =u->DribbleContext .DribbleConfig .m3508_init_angle ; +// u->DribbleContext .DribbleState = Dribble_PREPARE; //重置最初状态 - RELAY1_TOGGLE (1);//夹球，0夹1开 - RELAY2_TOGGLE (0);//球，0接1收 - +// RELAY1_TOGGLE (1);//夹球，0夹1开 +// RELAY2_TOGGLE (0);//球，0接1收 +// break; @@ -271,7 +275,7 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c) u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮 } - Pitch_Process(u,out); + Pitch_Process(u,out,c); break ; @@ -305,10 +309,30 @@ return 0; } -int8_t Pitch_Process(UP_t *u, CAN_Output_t *out) + + + + + + + + + + + + + +int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c) { - - + static fp32 go1_position,M2006_Screw_position ; + static int is_initialize=1; + if(is_initialize) + { + go1_position=u->PitchContext .PitchConfig .go1_init_position ; + M2006_Screw_position=u->PitchContext .PitchConfig .m2006_Screw_init; + is_initialize=0; + } + switch(u->PitchContext .PitchState){ case PITCH_START: @@ -317,32 +341,25 @@ int8_t Pitch_Process(UP_t *u, CAN_Output_t *out) 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;//置标志位用于启动视觉辅助投篮 + u->PitchContext .PitchState=PITCH_PULL_TRIGGER; }//更改标志位 break ; case PITCH_PULL_TRIGGER: + + if(u->motorfeedback .M2006 .total_angle >-1) //当2006的总角度小于1，可以认为已经勾上,误差为1 { - u->motor_target .go_shoot =u->PitchContext.PitchConfig .go1_init_position;//go下拉 - u->PitchContext.PitchState =PITCH_LAUNCHING;//转移标志位 + u->motor_target .go_shoot = go1_position;//go下拉 + u->motor_target .M3508_angle = M2006_Screw_position;//丝杠上的2006 + go1_position = go1_position + c->Vx ; + M2006_Screw_position=M2006_Screw_position+ c->Vy; + + } break ; - case PITCH_LAUNCHING: - if(u->motorfeedback .GO_motor_info [0]->Pos >0) - { -// u->motor_target .M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ;//给-140角度发射 - u->PitchContext .PitchState =PITCH_COMPLETE; - } - - - break ; - case PITCH_COMPLETE://完成运行动作后，恢复2006，-140度，go，pos0位置 - u->motor_target .M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ;//给-140角度 - u->motor_target .go_shoot = u->PitchContext .PitchConfig .go1_init_position; - break ; @@ -354,8 +371,29 @@ return 0; -// RELAY1_TOGGLE (1);//夹球，0夹1开 -// RELAY2_TOGGLE (0);//夹球，0接1收 + + + + + + + + + + + + + + + + + + + + + + + diff --git a/User/Module/up.h b/User/Module/up.h index 3019478..65bed8e 100644 --- a/User/Module/up.h +++ b/User/Module/up.h @@ -55,6 +55,7 @@ typedef struct { fp32 m2006_trigger_angle; // M2006触发角度0,用来合并扳机 fp32 go1_init_position; // GO电机触发位置,0,初始位置 fp32 go1_release_threshold; // go上升去合并扳机，扳机位置 + fp32 m2006_Screw_init; } PitchConfig_t; /* 投球控制上下文 */ @@ -154,6 +155,7 @@ typedef struct 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; @@ -242,6 +244,8 @@ typedef struct{ pid_type_def M3508_angle; pid_type_def M3508_speed; + + pid_type_def GO1_position; }pid; @@ -278,7 +282,7 @@ 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); int8_t UP_M2006_angle(UP_t *u,fp32 target_angle); int8_t UP_M3508_speed(UP_t *u,fp32 speed); -int8_t Pitch_Process(UP_t *u, CAN_Output_t *out); +int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c); int8_t Dribble_Process(UP_t *u, CAN_Output_t *out); diff --git a/User/device/GO_M8010_6_Driver.c b/User/device/GO_M8010_6_Driver.c index a4880ce..d95ea21 100644 --- a/User/device/GO_M8010_6_Driver.c +++ b/User/device/GO_M8010_6_Driver.c @@ -18,7 +18,7 @@ void GO_M8010_init (void){ for (uint8_t id= 0; id W = W; motor->W = (motor->W * 6.28319f )/(256*6.33f); - 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); + 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; - motor ->angle = AngleChange(DEGREE,motor ->Pos); } //力位混合控制 diff --git a/User/device/cmd.c b/User/device/cmd.c index d74300e..f7a45e3 100644 --- a/User/device/cmd.c +++ b/User/device/cmd.c @@ -44,20 +44,29 @@ int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n){ /*遥控器，上下层通用，按键控制，统一到cmd*/ int8_t CMD_ParseRC(CMD_t *cmd,const CMD_RC_t *rc) { - + if(cmd == NULL) return -1; const CMD_CtrlType_t prev_CtrlType = cmd->CMD_CtrlType ; const CMD_UP_mode_t prev_mode = cmd->CMD_UP_mode ;//保存旧状态 + cmd->Vx = rc->ch_r_x; + cmd->Vy = rc->ch_r_y; + cmd->Vw = rc->ch_l_x; + + cmd->poscamear = rc->ch_l_y; + + cmd->key_ctrl_l = rc->sw_l; + cmd->key_ctrl_r = rc->sw_r ; + if ((rc->sw_l == CMD_SW_ERR) || (rc->sw_r == CMD_SW_ERR)) { cmd->CMD_CtrlType =RELAXED; } else if(rc->sw_l==CMD_SW_UP) { cmd ->CMD_CtrlType =UP_RCcontrol; - if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_UP_mode =Pitch; //左上，右上，投篮 + if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_UP_mode =Pitch; //左上，右上，投篮，设置好的 if(rc->sw_r ==CMD_SW_MID) cmd ->CMD_UP_mode =Normal; //左上，右中，无模式 - if(rc->sw_r ==CMD_SW_DOWN) cmd ->CMD_UP_mode =Normal; //左上，右上，运球 + if(rc->sw_r ==CMD_SW_DOWN) cmd ->CMD_UP_mode =Pitch_HAND; //左上，右上，手动调整 } else if(rc->sw_l==CMD_SW_MID) @@ -91,3 +100,5 @@ int8_t CMD_ParseRC(CMD_t *cmd,const CMD_RC_t *rc) { } + + diff --git a/User/device/cmd.h b/User/device/cmd.h index cb73edc..2a65eeb 100644 --- a/User/device/cmd.h +++ b/User/device/cmd.h @@ -25,7 +25,8 @@ typedef enum{ Pitch, //投篮 /*视觉辅助下的投篮*/ - PICK_Pitch_Normal, + Pitch_HAND, + Dribbl_transfer }CMD_UP_mode_t; typedef struct { @@ -80,7 +81,11 @@ typedef struct { fp32 key_ctrl_l; fp32 key_ctrl_r; - + fp32 Vx; + fp32 Vy; + fp32 Vw; + fp32 poscamear; + /*视觉*/ struct { uint8_t cmd_status; @@ -109,5 +114,7 @@ int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n); int8_t CMD_ParseRC(CMD_t *cmd,const CMD_RC_t *rc) ; +static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd) ; + #endif diff --git a/User/task/up_task.c b/User/task/up_task.c index 5f1b52a..0947102 100644 --- a/User/task/up_task.c +++ b/User/task/up_task.c @@ -68,8 +68,8 @@ void Task_up(void *argument) // - UP_control(&UP,&UP_CAN_out,&up_cmd); -// UP.motor_target .go_shoot =aaa; +// UP_control(&UP,&UP_CAN_out,&up_cmd); + UP.motor_target .go_shoot =aaa; // UP.motor_target .M2006_angle =bbb ; // UP.motor_target .M3508_angle =CCC;