LYL/R2_UP
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

优化上层大疆电机那一块的逻辑

Browse Source
This commit is contained in:
ZHAISHUI04 2025-04-21 22:15:22 +08:00
parent b12bcd25d6
commit a632125bbd
8 changed files with 313 additions and 439 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
MDK-ARM/AUTO_CHASSIS.uvoptx
View File

@ -103,7 +103,7 @@
<bEvRecOn>1</bEvRecOn> <bEvRecOn>1</bEvRecOn>
<bSchkAxf>0</bSchkAxf> <bSchkAxf>0</bSchkAxf>
<bTchkAxf>0</bTchkAxf> <bTchkAxf>0</bTchkAxf>
<nTsel>3</nTsel> <nTsel>6</nTsel>
<sDll></sDll> <sDll></sDll>
<sDllPa></sDllPa> <sDllPa></sDllPa>
<sDlgDll></sDlgDll> <sDlgDll></sDlgDll>
@ -114,7 +114,7 @@
<tDlgDll></tDlgDll> <tDlgDll></tDlgDll>
<tDlgPa></tDlgPa> <tDlgPa></tDlgPa>
<tIfile></tIfile> <tIfile></tIfile>
<pMon>BIN\CMSIS_AGDI.dll</pMon> <pMon>STLink\ST-LINKIII-KEIL_SWO.dll</pMon>
</DebugOpt> </DebugOpt>
<TargetDriverDllRegistry> <TargetDriverDllRegistry>
<SetRegEntry> <SetRegEntry>
@ -140,12 +140,12 @@
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
<Key>CMSIS_AGDI</Key> <Key>CMSIS_AGDI</Key>
<Name>-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)</Name> <Name>-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)</Name>
</SetRegEntry> </SetRegEntry>
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
<Key>DLGUARM</Key> <Key>DLGUARM</Key>
<Name></Name> <Name>(105=-1,-1,-1,-1,0)</Name>
</SetRegEntry> </SetRegEntry>
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
@ -180,6 +180,11 @@
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>motor_send_data,0x0A</ItemText> <ItemText>motor_send_data,0x0A</ItemText>
</Ww> </Ww>
<Ww>
<count>5</count>
<WinNumber>1</WinNumber>
<ItemText>GO_calibration,0x0A</ItemText>
</Ww>
</WatchWindow1> </WatchWindow1>
<WatchWindow2> <WatchWindow2>
<Ww> <Ww>
@ -194,7 +199,7 @@
<DebugFlag> <DebugFlag>
<trace>0</trace> <trace>0</trace>
<periodic>1</periodic> <periodic>1</periodic>
<aLwin>1</aLwin> <aLwin>0</aLwin>
<aCover>0</aCover> <aCover>0</aCover>
<aSer1>0</aSer1> <aSer1>0</aSer1>
<aSer2>0</aSer2> <aSer2>0</aSer2>

BIN
MDK-ARM/ELE_CHASSIS/AUTO_CHASSIS.axf Normal file
View File

Binary file not shown.

39
User/Module/config.c
View File

@ -22,48 +22,33 @@ static const ConfigParam_t param_chassis ={
/*上层pid参数*/ /*上层pid参数*/
.M2006_angle_param = { .Shoot_M2006_angle_param = {
.p = 25.0f, .p = 25.0f,
.i = 0.0f, .i = 0.0f,
.d = 1.5f, .d = 1.5f,
.i_limit = 1000.0f, .i_limit = 1000.0f,
.out_limit = 3000.0f, .out_limit = 3000.0f,
}, },
.M2006_speed_param = { .Shoot_M2006_speed_param = {
.p = 5.0f, .p = 5.0f,
.i = 0.3f, .i = 0.3f,
.d = 0.0f, .d = 0.0f,
.i_limit = 2000.0f, .i_limit = 2000.0f,
.out_limit = 3000.0f, .out_limit = 3000.0f,
}, },
.M3508_angle_param = { .Pitch_M2006_angle_param = {
.p = 10.0f, .p = 25.0f,
.i = 0.5f, .i = 0.0f,
.d = 0.0f, .d = 1.5f,
.i_limit = 1000.0f, .i_limit = 1000.0f,
.out_limit = 3000.0f, .out_limit = 3000.0f,
}, },
.M3508_speed_param={ .Pitch_M2006_speed_param={
.p = 6.0f, .p = 5.0f,
.i = 0.0f, .i = 0.3f,
.d = 3.2f, .d = 0.0f,
.i_limit = 200.0f, .i_limit = 2000.0f,
.out_limit =6000.0f, .out_limit = 3000.0f,
},
.UP_GM6020_angle_param={
.p = 30.0f,
.i = 20.0f,
.d =0.0f,
.i_limit = 200.0f,
.out_limit = 1500.0f,
} ,
.UP_GM6020_speed_param={
.p =3.0f,
.i =0.5f,
.d =0.0f,
.i_limit = 200.0f,
.out_limit = 3000.0f
}, },

570
User/Module/up.c
View File

@ -24,14 +24,12 @@ int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
PID_init (&u->pid.VESC_5065_M1 ,PID_POSITION ,&(u->param ->VESC_5065_M1_param )); PID_init (&u->pid.VESC_5065_M1 ,PID_POSITION ,&(u->param ->VESC_5065_M1_param ));
PID_init (&u->pid.VESC_5065_M2 ,PID_POSITION ,&(u->param ->VESC_5065_M2_param )); PID_init (&u->pid.VESC_5065_M2 ,PID_POSITION ,&(u->param ->VESC_5065_M2_param ));
PID_init (&u->pid .M2006_angle ,PID_POSITION ,&(u->param->M2006_angle_param )); PID_init (&u->pid .Shoot_M2006angle ,PID_POSITION ,&(u->param->Shoot_M2006_angle_param ));
PID_init (&u->pid .M2006_speed ,PID_POSITION ,&(u->param->M2006_speed_param )); PID_init (&u->pid .Shoot_M2006speed ,PID_POSITION ,&(u->param->Shoot_M2006_speed_param ));
PID_init (&u->pid .M3508_angle ,PID_POSITION ,&(u->param->M3508_angle_param )); PID_init (&u->pid .Pitch_M2006_angle ,PID_POSITION ,&(u->param->Pitch_M2006_angle_param ));
PID_init (&u->pid .M3508_speed ,PID_POSITION ,&(u->param->M3508_speed_param )); PID_init (&u->pid .Pitch_M2006_speed ,PID_POSITION ,&(u->param->Pitch_M2006_speed_param ));
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 ));
u->go_cmd =u->param ->go_cmd ; u->go_cmd =u->param ->go_cmd ;
@ -56,20 +54,16 @@ int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
/*can，上层状态更新*/ /*can，上层状态更新*/
int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) { int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) {
u->motorfeedback .go_data = get_GO_measure_point() ;
u->motorfeedback .M2006 .motor =M2006;
u->motorfeedback .M3508 .motor =M3508;
u->motorfeedback .VESC_5065_M1_rpm =can ->motor .chassis5065 .as_array [0].rotor_speed ; u->motorfeedback .VESC_5065_M1_rpm =can ->motor .chassis5065 .as_array [0].rotor_speed ;
u->motorfeedback .VESC_5065_M2_rpm =can ->motor .chassis5065 .as_array [1].rotor_speed ; u->motorfeedback .VESC_5065_M2_rpm =can ->motor .chassis5065 .as_array [1].rotor_speed ;
u->motorfeedback .rotor_pit6020ecd =can ->motor .chassis6020.as_array [2].rotor_ecd ;
u->motorfeedback .rotor_pit6020rpm =can ->motor .chassis6020.as_array [2].rotor_speed ;
for(int i=0;i<4;i++){ for(int i=0;i<4;i++){
u->motorfeedback .M3508_rpm[i] =can ->motor .motor3508 .as_array [i].rotor_speed ; u->motorfeedback .DJmotor_feedback[i].rpm =can ->motor .motor3508 .as_array [i].rotor_speed ;
u->motorfeedback .M3508_angle [i]=can ->motor .motor3508 .as_array [i].rotor_ecd ; u->motorfeedback .DJmotor_feedback[i].ecd =can ->motor .motor3508 .as_array [i].rotor_ecd ;
DJ_processdata(&u->motorfeedback .DJmotor_feedback [i], MOTOR2006_ECD_TO_ANGLE);
} }
u->cmd =c; u->cmd =c;
@ -77,90 +71,42 @@ int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) {
return 0; return 0;
} }
int8_t DJ_processdata(DJmotor_feedback_t *f,fp32 ecd_to_angle)
{
/*上层大疆电机角度控制,使用can1的id1和2*/ int8_t cnt=0;
int8_t UP_angle_control(UP_t *u, fp32 target_angle,MotorType_t motor) { fp32 angle ,delta;
// 获取当前编码器角度
int8_t cnt=0; angle = f->ecd;
fp32 angle ,delta; if (f->init_cnt < 50) {
f->orig_angle= angle;
switch(motor) f->last_angle = angle;
{ f->init_cnt++;
case M2006 : return 0;
angle = u->motorfeedback.M3508_angle[0];
if (u->motorfeedback .M2006 .init_cnt < 50) {
u->motorfeedback .M2006.orig_angle = angle; // 记录初始编码器值
u->motorfeedback .M2006.last_angle = angle;
u->motorfeedback .M2006.init_cnt++; // 初始化计数器递增
return 0;
} }
delta = angle - u->motorfeedback .M2006.last_angle;
delta = angle - u->motorfeedback .M2006.last_angle; delta = angle - f->last_angle;
if (delta > 4096) { if (delta > 4096) {
u->motorfeedback .M2006.round_cnt--; // 逆时针跨圈 f->round_cnt--;
} else if (delta < -4096) { } else if (delta < -4096) {
u->motorfeedback .M2006.round_cnt++; // 顺时针跨圈 f->round_cnt++;
} }
u->motorfeedback.M2006.last_angle = angle; f->last_angle = angle;
// 计算总角度 f->total_angle=(f->round_cnt*8191+(angle -f->orig_angle ))*ecd_to_angle;
float total_angle = (u->motorfeedback.M2006 .round_cnt * 8191 + (angle - u->motorfeedback.M2006.orig_angle)) * MOTOR2006_ECD_TO_ANGLE;
u->motorfeedback.M2006.total_angle =total_angle; }
/*
float delta_angle = PID_calc(&u->pid.M2006_angle, total_angle, target_angle); 这里id范围为1-4，
float delta_speed = PID_calc(&u->pid.M2006_speed, u->motorfeedback.M3508_rpm [0], delta_angle); */
u->motor_target.M2006_angle = target_angle; int8_t DJ_Angle_Control(UP_t *u,int id,DJmotor_feedback_t *f,pid_type_def *Angle_pid,pid_type_def *Speed_pid,fp32 target_angle)
u->final_out .final_3508out [0] =delta_speed; {
fp32 delta_angle,delta_speed;
break ; delta_angle = PID_calc(Angle_pid , f->total_angle , target_angle);
delta_speed = PID_calc(Speed_pid , f->rpm , delta_angle);
case M3508 :
u->final_out .DJfinal_out [id-1]=delta_speed;
angle = u->motorfeedback.M3508_angle[1]; return 0;
if (u->motorfeedback .M3508 .init_cnt < 50) {
u->motorfeedback .M3508.orig_angle = angle; // 记录初始编码器值
u->motorfeedback .M3508.last_angle = angle;
u->motorfeedback .M3508.init_cnt++; // 初始化计数器递增
return 0;
}
delta = angle - u->motorfeedback .M3508.last_angle;
delta = angle - u->motorfeedback .M3508.last_angle;
if (delta > 4096) {
u->motorfeedback .M3508.round_cnt--; // 逆时针跨圈
} else if (delta < -4096) {
u->motorfeedback .M3508.round_cnt++; // 顺时针跨圈
}
u->motorfeedback.M3508.last_angle = angle;
// 计算总角度
total_angle = (u->motorfeedback.M3508 .round_cnt * 8191 + (angle - u->motorfeedback.M3508.orig_angle)) * MOTOR2006_ECD_TO_ANGLE;
u->motorfeedback.M3508.total_angle =total_angle;
delta_angle = PID_calc(&u->pid.M3508_angle , total_angle, target_angle);
delta_speed = PID_calc(&u->pid.M3508_speed , u->motorfeedback.M3508_rpm [1], delta_angle);
u->motor_target.M3508_angle = target_angle;
u->final_out .final_3508out[1] =delta_speed;
break ;
}
return 0;
} }
//int8_t UP_M3508_speed(UP_t *u,fp32 speed)
//{
// u->motor_target .M3508_speed [] =speed;
// for(int i=0;i<3;i++){
// u->final_out .final_3508out [i] =
// PID_calc (&(u->pid .M3508_speed[i] ),u->motorfeedback .M3508_speed [i],speed );
// }
//
//}
int8_t VESC_M5065_Control(UP_t *u,fp32 speed) int8_t VESC_M5065_Control(UP_t *u,fp32 speed)
{ {
@ -175,73 +121,75 @@ int8_t VESC_M5065_Control(UP_t *u,fp32 speed)
} }
int8_t GM6020_control(UP_t *u,fp32 angle)
{
fp32 delat_speed;
// Clip(&angle,90,270);
delat_speed =
PID_calc (&(u->pid .GM6020_angle ),u->motorfeedback .rotor_pit6020ecd ,(angle /360*8191));
u->final_out .final_pitchout =
PID_calc (&(u->pid .GM6020_speed ),u->motorfeedback.rotor_pit6020rpm ,delat_speed);
u->motor_target .rotor_pit6020angle =angle ;
return 0;
}
/*go电机控制*/ /*go电机控制*/
int8_t GO_SendData(UP_t *u,float pos,float limit) int8_t GO_SendData(UP_t *u, float pos, float limit)
{ {
u->go_cmd .Pos =pos; static int is_calibration=0;
// 读取参数 static fp32 error=0; //误差量
float tff = u->go_cmd.T ; // 前馈力矩
float kp = u->go_cmd .K_P; // 刚度
float kd = u->go_cmd.K_W; // 阻尼（原代码中kdd/kid混淆，需统一为K_W）
float q_desired = u->go_cmd.Pos; // 期望位置（rad）
float q_current = u->motorfeedback.go_data ->Pos ; // 当前位置（rad）
float dq_desired = u->go_cmd.W; // 期望速度（rad/s）
float dq_current = u->motorfeedback.go_data ->W ; // 当前速度（rad/s）
GO_MotorData_t *GO_calibration;//校准前，原始数据
GO_calibration = get_GO_measure_point() ;
if(is_calibration==0)
{
is_calibration=HAL_GPIO_ReadPin (GPIOE ,GPIO_PIN_9 );
u->go_cmd.Pos = -50; //上电之后跑
error= GO_calibration->Pos ;
}
u->motorfeedback .go_data = GO_calibration;
u->motorfeedback .go_data ->Pos= error ;
u->go_cmd.Pos = pos;
// 读取参数
float tff = u->go_cmd.T; // 前馈力矩
float kp = u->go_cmd.K_P; // 位置刚度
float kd = u->go_cmd.K_W; // 速度阻尼
float q_desired = u->go_cmd.Pos; // 期望位置（rad）
float q_current = u->motorfeedback.go_data->Pos; // 当前角度位置（rad）
float dq_desired = u->go_cmd.W; // 期望角速度（rad/s）
float dq_current = u->motorfeedback.go_data->W; // 当前角速度（rad/s）
// 计算输出力矩 tau
float tau = tff + kp * (q_desired - q_current) + kd * (dq_desired - dq_current); float tau = tff + kp * (q_desired - q_current) + kd * (dq_desired - dq_current);
/*限制最大输入来限制最大输出*/
if (tau > limit) { if (pos - q_current > limit) {
u->go_cmd.Pos = q_current + limit; // 限制位置
tff = limit - kp * (q_desired - q_current) - kd * (dq_desired - dq_current) - (tau - limit) / 2.0; }else if (pos - q_current < -limit) {
tau = limit; u->go_cmd.Pos = q_current - limit; // 限制位置
} else if (tau < -limit) { }else {
u->go_cmd.Pos = pos; // 允许位置
}
tff = limit - kp * (q_desired - q_current) - kd * (dq_desired - dq_current) + (tau - limit) / 2.0; // 发送数据
tau = -limit; GO_M8010_send_data(&u->go_cmd);
}
u->go_cmd .T =tff;
GO_M8010_send_data(&u->go_cmd );
u->go_cmd .T =0;
return 0;
return 0;
} }
int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out) int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
{ {
//电机控制 ，传进can //电机控制 ，传进can
DJ_Angle_Control(u,1,&u->motorfeedback .DJmotor_feedback[0] ,
UP_angle_control(u,u->motor_target .M2006_angle ,M2006); &u->pid .Shoot_M2006angle ,
UP_angle_control(u,u->motor_target .M3508_angle ,M3508 ); &u->pid .Shoot_M2006speed ,
u->motor_target .Shoot_M2006_angle );
DJ_Angle_Control(u,2,&u->motorfeedback .DJmotor_feedback[1] ,
&u->pid .Pitch_M2006_angle ,
GO_SendData(u,u->motor_target .go_shoot,0.08 ); &u->pid .Pitch_M2006_speed ,
u->motor_target .Pitch_M2006_angle );
GO_SendData(u,u->motor_target .go_shoot,1 );
for(int i=0;i<4;i++){ for(int i=0;i<4;i++){
out ->motor3508 .as_array[i]=u->final_out.final_3508out [i] ; out ->motor3508 .as_array[i]=u->final_out.DJfinal_out [i] ;
} }
out ->chassis5065 .erpm [0]= u->final_out .final_VESC_5065_M1out ; out ->chassis5065 .erpm [0]= u->final_out .final_VESC_5065_M1out ;
out ->chassis5065 .erpm [1]= -u->final_out .final_VESC_5065_M2out ; out ->chassis5065 .erpm [1]= -u->final_out .final_VESC_5065_M2out ;
out ->chassis6020 .as_array [2]=u->final_out .final_pitchout ;
return 0; return 0;
@ -270,8 +218,8 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
u->motor_target .go_shoot =u->PitchContext .PitchConfig .go1_init_position ; u->motor_target .go_shoot =u->PitchContext .PitchConfig .go1_init_position ;
is_pitch=0; is_pitch=0;
} }
u->motor_target .M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ; // u->motor_target .M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ;
u->PitchContext .PitchState = PITCH_PREPARE; //状态更新,开始夹球 // u->PitchContext .PitchState = PITCH_PREPARE; //状态更新,开始夹球
// /*运球*/ // /*运球*/
// u->motor_target .go_spin = u->DribbleContext.DribbleConfig .go2_init_angle ; // u->motor_target .go_spin = u->DribbleContext.DribbleConfig .go2_init_angle ;
@ -291,7 +239,7 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮 u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮
} }
Pitch_Process(u,out,c); // Pitch_Process(u,out,c);
break ; break ;
@ -303,13 +251,13 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
u->DribbleContext .DribbleState =STATE_GRAB_BALL; u->DribbleContext .DribbleState =STATE_GRAB_BALL;
} }
Dribble_Process(u,out); // Dribble_Process(u,out);
}break ; }break ;
case Dribbl_transfer: case Dribbl_transfer:
Dribbl_transfer_a(u,out); // Dribbl_transfer_a(u,out);
} }
break; break;
@ -338,184 +286,184 @@ return 0;
int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c) //int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c)
{ //{
static fp32 go1_position,M2006_Screw_position ; // static fp32 go1_position,M2006_Screw_position ;
static int is_initialize=1; // static int is_initialize=1;
if(is_initialize) // if(is_initialize)
{ // {
go1_position=u->PitchContext .PitchConfig .go1_init_position ; // go1_position=u->PitchContext .PitchConfig .go1_init_position ;
M2006_Screw_position=u->PitchContext .PitchConfig .m2006_Screw_init; // M2006_Screw_position=u->PitchContext .PitchConfig .m2006_Screw_init;
is_initialize=0; // is_initialize=0;
} // }
switch(u->PitchContext .PitchState){ // switch(u->PitchContext .PitchState){
//
case PITCH_START: // case PITCH_START:
u->motor_target .go_shoot = u->PitchContext .PitchConfig .go1_release_threshold; // u->motor_target .go_shoot = u->PitchContext .PitchConfig .go1_release_threshold;
// 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;
//}//更改标志位
break ;
case PITCH_PULL_TRIGGER:
if(u->motorfeedback .M2006 .total_angle >-1) //当2006的总角度小于1，可以认为已经勾上,误差为1
{
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 ;
}
return 0;
}
int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
{
switch (u->DribbleContext.DribbleState) {
case STATE_GRAB_BALL://开始
RELAY1_TOGGLE (0);//夹球
u->motor_target.M3508_angle =u->DribbleContext .DribbleConfig .m3508_high_angle;//3508升起
u->motor_target.go_spin =u->DribbleContext .DribbleConfig .go2_flip_angle;
if((u->motorfeedback .M3508 .total_angle >400)) {
RELAY2_TOGGLE (1);
// if(((u->motorfeedback.GO_motor_info[1]->Pos ) <-1.1)&&(u->motorfeedback.M3508 .total_angle >1190)){
//
// u->DribbleContext .DribbleState = STATE_RELEASE_BALL; //当go2到标准位置，标志位改变
// }
}
break;
case STATE_RELEASE_BALL:
RELAY1_TOGGLE (1);//松球
// if((u->motorfeedback.GO_motor_info[1]->Pos ) <-1.3){
// u->motor_target.go_spin =-40; //恢复go2位置
// u->DribbleContext .DribbleState = STATE_CATCH_PREP; //当go2到标准位置，标志位改变
// }
break;
case STATE_CATCH_PREP:
// if((u->motorfeedback.GO_motor_info[1]->Pos )> -0.4){
// u->motor_target.M3508_angle =0 ; //当go2到初始位置，3508降
// //
// RELAY2_TOGGLE (0);//接球 //// if(u->motorfeedback .GO_motor_info[0]->Pos < -32.40){ //检测go位置到达最上面，这里的检测条件可以更改
// } //// u->motor_target .M2006_angle = u->PitchContext .PitchConfig .m2006_trigger_angle ;//设置2006角度，0
if(u->motorfeedback .M3508 .total_angle <51.0f){ //
RELAY1_TOGGLE (0);//夹球，0夹1开 // u->PitchContext .PitchState=PITCH_PULL_TRIGGER;
////}//更改标志位
u->DribbleContext .DribbleState = STATE_TRANSFER; // break ;
} // case PITCH_PULL_TRIGGER:
//
break; //
// if(u->motorfeedback .M2006 .total_angle >-1) //当2006的总角度小于1，可以认为已经勾上,误差为1
break ;
default:
break;
}
return 0;
}
void Dribbl_transfer_a(UP_t *u, CAN_Output_t *out)
{
switch (u->DribbleContext.DribbleState) {
case STATE_TRANSFER:
if((u->motorfeedback.M3508 .total_angle <52.0f )) //满足这个状态时认为go和3508到达初始位置，再夹上球
{
u->motor_target .go_spin =u->DribbleContext .DribbleConfig .go2_release_threshold ;
}
// if(u->motorfeedback .GO_motor_info [1]->Pos < -4.9)
// { // {
// RELAY1_TOGGLE (1);//夹球，0夹1开 // 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;
//
// //
// if(u->motorfeedback .GO_motor_info [1]->Pos > -4.8)
// {
// u->DribbleContext .DribbleState = STATE_CATCH_DONE; //当go2到标准位置，标志位改变
// }
// } // }
//
// break ;
break ;
case STATE_CATCH_DONE: //
//
RELAY1_TOGGLE (1);//夹球，0夹1开 //
RELAY2_TOGGLE (0);//夹球，0接1收 // }
//
//return 0;
//
//}
u->motor_target.go_spin=u->DribbleContext .DribbleConfig.go2_init_angle ;
break;
break;
}
}
//int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
//{
//
// switch (u->DribbleContext.DribbleState) {
// case STATE_GRAB_BALL://开始
//
// RELAY1_TOGGLE (0);//夹球
//
// u->motor_target.M3508_angle =u->DribbleContext .DribbleConfig .m3508_high_angle;//3508升起
// u->motor_target.go_spin =u->DribbleContext .DribbleConfig .go2_flip_angle;
//
// if((u->motorfeedback .M3508 .total_angle >400)) {
// RELAY2_TOGGLE (1);
//
//// if(((u->motorfeedback.GO_motor_info[1]->Pos ) <-1.1)&&(u->motorfeedback.M3508 .total_angle >1190)){
////
//// u->DribbleContext .DribbleState = STATE_RELEASE_BALL; //当go2到标准位置，标志位改变
//// }
//
// }
// break;
// case STATE_RELEASE_BALL:
// RELAY1_TOGGLE (1);//松球
//
//
//
//// if((u->motorfeedback.GO_motor_info[1]->Pos ) <-1.3){
//// u->motor_target.go_spin =-40; //恢复go2位置
//// u->DribbleContext .DribbleState = STATE_CATCH_PREP; //当go2到标准位置，标志位改变
//// }
// break;
// case STATE_CATCH_PREP:
//// if((u->motorfeedback.GO_motor_info[1]->Pos )> -0.4){
//// u->motor_target.M3508_angle =0 ; //当go2到初始位置，3508降
////
//// RELAY2_TOGGLE (0);//接球
//// }
// if(u->motorfeedback .M3508 .total_angle <51.0f){
// RELAY1_TOGGLE (0);//夹球，0夹1开
// u->DribbleContext .DribbleState = STATE_TRANSFER;
// }
//
// break;
//
//
//
// break ;
// default:
// break;
// }
// return 0;
//}
//void Dribbl_transfer_a(UP_t *u, CAN_Output_t *out)
//{
// switch (u->DribbleContext.DribbleState) {
//
//
//
// case STATE_TRANSFER:
// if((u->motorfeedback.M3508 .total_angle <52.0f )) //满足这个状态时认为go和3508到达初始位置，再夹上球
// {
//
// u->motor_target .go_spin =u->DribbleContext .DribbleConfig .go2_release_threshold ;
// }
//
//// if(u->motorfeedback .GO_motor_info [1]->Pos < -4.9)
//// {
//// RELAY1_TOGGLE (1);//夹球，0夹1开
////
//// if(u->motorfeedback .GO_motor_info [1]->Pos > -4.8)
//// {
//// u->DribbleContext .DribbleState = STATE_CATCH_DONE; //当go2到标准位置，标志位改变
//// }
//// }
//
//
// break ;
// case STATE_CATCH_DONE:
//
// RELAY1_TOGGLE (1);//夹球，0夹1开
// RELAY2_TOGGLE (0);//夹球，0接1收
// u->motor_target.go_spin=u->DribbleContext .DribbleConfig.go2_init_angle ;
// break;
// break;
// }
//}

109
User/Module/up.h
View File

@ -91,7 +91,6 @@ typedef struct {
fp32 go2_init_angle; // GO2初始角度 fp32 go2_init_angle; // GO2初始角度
fp32 go2_flip_angle; // GO2翻转角度 fp32 go2_flip_angle; // GO2翻转角度
fp32 flip_timing; // 翻转触发时机 fp32 flip_timing; // 翻转触发时机
// fp32
fp32 go2_release_threshold; // 释放球 fp32 go2_release_threshold; // 释放球
} DribbleConfig_t; } DribbleConfig_t;
@ -112,47 +111,18 @@ typedef struct {
AHRS_Eulr_t imu_eulr;//解算后存放欧拉角（弧度制） AHRS_Eulr_t imu_eulr;//解算后存放欧拉角（弧度制）
}UP_Imu_t; }UP_Imu_t;
/**
*@input[in] rev 暂时不知道干啥用的，github为回答issue
*@input[in] T 力矩，单位N·m
*@input[in] Pos 目标位置，单位rad
*@input[in] W 目标速度，单位rad/s
*@input[in] K_P 位置环环kp
*@input[in] K_W 速度环kp
*@note 控制公式 ： t = T + （设定位置 - 实际位置）*K_P + （设定速度 - 实际速度）*K_W
*/
typedef struct {
int rev; //暂时不知何用
float T;
float W;
float K_P;
float K_W;
}GO_param_t;
/*角度环控制电机类型*/
typedef enum {
M2006 = 1,
M3508
} MotorType_t;
typedef struct typedef struct
{ {
pid_param_t VESC_5065_M1_param; pid_param_t VESC_5065_M1_param;
pid_param_t VESC_5065_M2_param; pid_param_t VESC_5065_M2_param;
pid_param_t UP_GM6020_speed_param; pid_param_t Shoot_M2006_speed_param;
pid_param_t UP_GM6020_angle_param; pid_param_t Shoot_M2006_angle_param;
pid_param_t M2006_speed_param; pid_param_t Pitch_M2006_speed_param;
pid_param_t M2006_angle_param; pid_param_t Pitch_M2006_angle_param;
pid_param_t M3508_speed_param;
pid_param_t M3508_angle_param;
DribbleConfig_t DribbleConfig_Config; DribbleConfig_t DribbleConfig_Config;
PitchConfig_t PitchConfig_Config; PitchConfig_t PitchConfig_Config;
@ -161,88 +131,69 @@ typedef struct
}UP_Param_t; }UP_Param_t;
typedef struct typedef struct
{ {
MotorType_t motor; fp32 ecd;
fp32 rpm;
float orig_angle;
float last_angle; fp32 orig_angle;
int32_t round_cnt; fp32 last_angle;
uint16_t init_cnt; int32_t round_cnt;
float total_angle; uint16_t init_cnt;
fp32 total_angle;
}motor_angle_data; }DJmotor_feedback_t;
typedef struct{ typedef struct{
uint8_t up_task_run; uint8_t up_task_run;
const UP_Param_t *param; const UP_Param_t *param;
int state;
/*运球过程*/ /*运球过程*/
DribbleContext_t DribbleContext; DribbleContext_t DribbleContext;
/*投篮过程*/ /*投篮过程*/
PitchContext_t PitchContext; PitchContext_t PitchContext;
UP_Imu_t pos088; UP_Imu_t pos088;
CMD_t *cmd; CMD_t *cmd;
struct{ struct{
fp32 rotor_pit6020ecd;
fp32 rotor_pit6020rpm;
fp32 VESC_5065_M1_rpm; fp32 VESC_5065_M1_rpm;
fp32 VESC_5065_M2_rpm; fp32 VESC_5065_M2_rpm;
motor_angle_data M2006; GO_MotorData_t *go_data;//存放go电机数据
motor_angle_data M3508;
const GO_MotorData_t *go_data;//存放go电机数据
fp32 M3508_angle[4]; DJmotor_feedback_t DJmotor_feedback[4];
fp32 M3508_rpm [4];
int flag;
}motorfeedback; }motorfeedback;
struct{ struct{
fp32 rotor_pit6020angle;
fp32 VESC_5065_M1_rpm; fp32 VESC_5065_M1_rpm;
fp32 VESC_5065_M2_rpm; fp32 VESC_5065_M2_rpm;
fp32 M2006_angle; fp32 Shoot_M2006_angle;
fp32 M3508_angle; fp32 Pitch_M2006_angle;
fp32 go_shoot; fp32 go_shoot;
fp32 go_spin;
}motor_target; }motor_target;
struct{ struct{
pid_type_def GM6020_speed;
pid_type_def GM6020_angle;
pid_type_def VESC_5065_M1; pid_type_def VESC_5065_M1;
pid_type_def VESC_5065_M2; pid_type_def VESC_5065_M2;
pid_type_def M2006_angle; pid_type_def Shoot_M2006angle;
pid_type_def M2006_speed; pid_type_def Shoot_M2006speed;
pid_type_def M3508_angle; pid_type_def Pitch_M2006_angle;
pid_type_def M3508_speed; pid_type_def Pitch_M2006_speed;
}pid; }pid;
GO_MotorCmd_t go_cmd; GO_MotorCmd_t go_cmd;
@ -253,8 +204,7 @@ typedef struct{
/*经PID计算后的实际发送给电机的实时输出值*/ /*经PID计算后的实际发送给电机的实时输出值*/
struct struct
{ {
fp32 final_3508out[4]; fp32 DJfinal_out[4];
fp32 final_pitchout;
fp32 final_VESC_5065_M1out; fp32 final_VESC_5065_M1out;
fp32 final_VESC_5065_M2out; fp32 final_VESC_5065_M2out;
@ -271,20 +221,13 @@ typedef struct{
int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq); int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq);
int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) ; int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) ;
int8_t GM6020_control(UP_t *u,fp32 angle);
int8_t VESC_M5065_Control(UP_t *u,fp32 speed); int8_t VESC_M5065_Control(UP_t *u,fp32 speed);
int8_t GO_SendData(UP_t *u,float pos,float limit); int8_t GO_SendData(UP_t *u,float pos,float limit);
int8_t UP_angle_control(UP_t *u, fp32 target_angle,MotorType_t motor);
int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c); 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 ALL_Motor_Control(UP_t *u,CAN_Output_t *out);
int8_t UP_M2006_angle(UP_t *u,fp32 target_angle); int8_t DJ_processdata(DJmotor_feedback_t *f,fp32 ecd_to_angle);
int8_t UP_M3508_speed(UP_t *u,fp32 speed); int8_t DJ_Angle_Control(UP_t *u,int id,DJmotor_feedback_t *f,pid_type_def *Angle_pid,pid_type_def *Speed_pid,fp32 target_angle);
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);
void Dribbl_transfer_a(UP_t *u, CAN_Output_t *out);
#endif #endif

8
User/device/GO_M8010_6_Driver.c
View File

@ -15,7 +15,6 @@
else if ((_IN) >= (_MAX)) \ else if ((_IN) >= (_MAX)) \
(_IN) = (_MAX); \ (_IN) = (_MAX); \
} }
GO_MotorCmd_t cmd_gogogo ;
RIS_ControlData_t motor_send_data; RIS_ControlData_t motor_send_data;
@ -102,12 +101,7 @@ void USART6_RxCompleteCallback(void )
memcpy(&data.motor_recv_data, rx_buffer, sizeof(data.motor_recv_data)); // Copy received data memcpy(&data.motor_recv_data, rx_buffer, sizeof(data.motor_recv_data)); // Copy received data
extract_data(&data); extract_data(&data);
} }
const GO_MotorData_t *get_GO_measure_point(void) GO_MotorData_t *get_GO_measure_point(void)
{ {
return &data; return &data;
} }
void gogogo(void )
{
}

3
User/device/GO_M8010_6_Driver.h
View File

@ -107,10 +107,9 @@ typedef struct
void modify_data(GO_MotorCmd_t *motor_s); void modify_data(GO_MotorCmd_t *motor_s);
void extract_data(GO_MotorData_t *motor_r); void extract_data(GO_MotorData_t *motor_r);
const GO_MotorData_t *get_GO_measure_point(void); GO_MotorData_t *get_GO_measure_point(void);
void GO_M8010_send_data( GO_MotorCmd_t *cmd); void GO_M8010_send_data( GO_MotorCmd_t *cmd);
void USART6_RxCompleteCallback(void ); void USART6_RxCompleteCallback(void );
void gogogo(void );
#endif /*__GO_M8010_6_H */ #endif /*__GO_M8010_6_H */

8
User/task/up_task.c
View File

@ -31,7 +31,7 @@ static UP_t UP;
static CMD_t up_cmd; static CMD_t up_cmd;
#endif #endif
float aaa=0; float aaa=1;
float bbb=0; float bbb=0;
float CCC=0; float CCC=0;
float ddd=0; float ddd=0;
@ -65,11 +65,11 @@ void Task_up(void *argument)
// //
// VESC_M5065_Control(&UP, 20000); // VESC_M5065_Control(&UP, 20000);
// //
UP_control(&UP,&UP_CAN_out,&up_cmd); // UP_control(&UP,&UP_CAN_out,&up_cmd);
UP.motor_target .go_shoot =aaa; UP.motor_target .Pitch_M2006_angle =aaa;
// UP.motor_target .M2006_angle =bbb ; // UP.motor_target .M2006_angle =bbb ;
// UP.motor_target .M3508_angle =CCC; // UP.motor_target .M3508_angle =CCC;