优化上层大疆电机那一块的逻辑
This commit is contained in:
parent
b12bcd25d6
commit
a632125bbd
@ -103,7 +103,7 @@
|
||||
<bEvRecOn>1</bEvRecOn>
|
||||
<bSchkAxf>0</bSchkAxf>
|
||||
<bTchkAxf>0</bTchkAxf>
|
||||
<nTsel>3</nTsel>
|
||||
<nTsel>6</nTsel>
|
||||
<sDll></sDll>
|
||||
<sDllPa></sDllPa>
|
||||
<sDlgDll></sDlgDll>
|
||||
@ -114,7 +114,7 @@
|
||||
<tDlgDll></tDlgDll>
|
||||
<tDlgPa></tDlgPa>
|
||||
<tIfile></tIfile>
|
||||
<pMon>BIN\CMSIS_AGDI.dll</pMon>
|
||||
<pMon>STLink\ST-LINKIII-KEIL_SWO.dll</pMon>
|
||||
</DebugOpt>
|
||||
<TargetDriverDllRegistry>
|
||||
<SetRegEntry>
|
||||
@ -140,12 +140,12 @@
|
||||
<SetRegEntry>
|
||||
<Number>0</Number>
|
||||
<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>
|
||||
<Number>0</Number>
|
||||
<Key>DLGUARM</Key>
|
||||
<Name></Name>
|
||||
<Name>(105=-1,-1,-1,-1,0)</Name>
|
||||
</SetRegEntry>
|
||||
<SetRegEntry>
|
||||
<Number>0</Number>
|
||||
@ -180,6 +180,11 @@
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>motor_send_data,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>5</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>GO_calibration,0x0A</ItemText>
|
||||
</Ww>
|
||||
</WatchWindow1>
|
||||
<WatchWindow2>
|
||||
<Ww>
|
||||
@ -194,7 +199,7 @@
|
||||
<DebugFlag>
|
||||
<trace>0</trace>
|
||||
<periodic>1</periodic>
|
||||
<aLwin>1</aLwin>
|
||||
<aLwin>0</aLwin>
|
||||
<aCover>0</aCover>
|
||||
<aSer1>0</aSer1>
|
||||
<aSer2>0</aSer2>
|
||||
|
||||
BIN
MDK-ARM/ELE_CHASSIS/AUTO_CHASSIS.axf
Normal file
BIN
MDK-ARM/ELE_CHASSIS/AUTO_CHASSIS.axf
Normal file
Binary file not shown.
@ -22,48 +22,33 @@ static const ConfigParam_t param_chassis ={
|
||||
|
||||
|
||||
/*上层pid参数*/
|
||||
.M2006_angle_param = {
|
||||
.Shoot_M2006_angle_param = {
|
||||
.p = 25.0f,
|
||||
.i = 0.0f,
|
||||
.d = 1.5f,
|
||||
.i_limit = 1000.0f,
|
||||
.out_limit = 3000.0f,
|
||||
},
|
||||
.M2006_speed_param = {
|
||||
.Shoot_M2006_speed_param = {
|
||||
.p = 5.0f,
|
||||
.i = 0.3f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 2000.0f,
|
||||
.out_limit = 3000.0f,
|
||||
},
|
||||
.M3508_angle_param = {
|
||||
.p = 10.0f,
|
||||
.i = 0.5f,
|
||||
.d = 0.0f,
|
||||
.Pitch_M2006_angle_param = {
|
||||
.p = 25.0f,
|
||||
.i = 0.0f,
|
||||
.d = 1.5f,
|
||||
.i_limit = 1000.0f,
|
||||
.out_limit = 3000.0f,
|
||||
},
|
||||
.M3508_speed_param={
|
||||
.p = 6.0f,
|
||||
.i = 0.0f,
|
||||
.d = 3.2f,
|
||||
.i_limit = 200.0f,
|
||||
.out_limit =6000.0f,
|
||||
},
|
||||
|
||||
.UP_GM6020_angle_param={
|
||||
.p = 30.0f,
|
||||
.i = 20.0f,
|
||||
.Pitch_M2006_speed_param={
|
||||
.p = 5.0f,
|
||||
.i = 0.3f,
|
||||
.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
|
||||
.i_limit = 2000.0f,
|
||||
.out_limit = 3000.0f,
|
||||
},
|
||||
|
||||
|
||||
|
||||
568
User/Module/up.c
568
User/Module/up.c
@ -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_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 .M2006_speed ,PID_POSITION ,&(u->param->M2006_speed_param ));
|
||||
PID_init (&u->pid .Shoot_M2006angle ,PID_POSITION ,&(u->param->Shoot_M2006_angle_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 .M3508_speed ,PID_POSITION ,&(u->param->M3508_speed_param ));
|
||||
PID_init (&u->pid .Pitch_M2006_angle ,PID_POSITION ,&(u->param->Pitch_M2006_angle_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 ;
|
||||
|
||||
@ -56,20 +54,16 @@ int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
|
||||
/*can，上层状态更新*/
|
||||
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_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++){
|
||||
u->motorfeedback .M3508_rpm[i] =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].rpm =can ->motor .motor3508 .as_array [i].rotor_speed ;
|
||||
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;
|
||||
@ -77,91 +71,43 @@ int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*上层大疆电机角度控制,使用can1的id1和2*/
|
||||
int8_t UP_angle_control(UP_t *u, fp32 target_angle,MotorType_t motor) {
|
||||
// 获取当前编码器角度
|
||||
int8_t DJ_processdata(DJmotor_feedback_t *f,fp32 ecd_to_angle)
|
||||
{
|
||||
int8_t cnt=0;
|
||||
fp32 angle ,delta;
|
||||
|
||||
switch(motor)
|
||||
angle = f->ecd;
|
||||
if (f->init_cnt < 50) {
|
||||
f->orig_angle= angle;
|
||||
f->last_angle = angle;
|
||||
f->init_cnt++;
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
delta = angle - f->last_angle;
|
||||
if (delta > 4096) {
|
||||
f->round_cnt--;
|
||||
} else if (delta < -4096) {
|
||||
f->round_cnt++;
|
||||
}
|
||||
f->last_angle = angle;
|
||||
f->total_angle=(f->round_cnt*8191+(angle -f->orig_angle ))*ecd_to_angle;
|
||||
|
||||
}
|
||||
/*
|
||||
这里id范围为1-4，
|
||||
*/
|
||||
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)
|
||||
{
|
||||
case M2006 :
|
||||
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;
|
||||
fp32 delta_angle,delta_speed;
|
||||
delta_angle = PID_calc(Angle_pid , f->total_angle , target_angle);
|
||||
delta_speed = PID_calc(Speed_pid , f->rpm , delta_angle);
|
||||
|
||||
u->motorfeedback .M2006.init_cnt++; // 初始化计数器递增
|
||||
u->final_out .DJfinal_out [id-1]=delta_speed;
|
||||
return 0;
|
||||
}
|
||||
|
||||
delta = angle - u->motorfeedback .M2006.last_angle;
|
||||
delta = angle - u->motorfeedback .M2006.last_angle;
|
||||
if (delta > 4096) {
|
||||
u->motorfeedback .M2006.round_cnt--; // 逆时针跨圈
|
||||
} else if (delta < -4096) {
|
||||
u->motorfeedback .M2006.round_cnt++; // 顺时针跨圈
|
||||
}
|
||||
u->motorfeedback.M2006.last_angle = 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);
|
||||
float delta_speed = PID_calc(&u->pid.M2006_speed, u->motorfeedback.M3508_rpm [0], delta_angle);
|
||||
u->motor_target.M2006_angle = target_angle;
|
||||
u->final_out .final_3508out [0] =delta_speed;
|
||||
|
||||
break ;
|
||||
|
||||
case M3508 :
|
||||
|
||||
angle = u->motorfeedback.M3508_angle[1];
|
||||
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)
|
||||
{
|
||||
u->motor_target .VESC_5065_M1_rpm =speed;
|
||||
@ -175,50 +121,51 @@ 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电机控制*/
|
||||
|
||||
int8_t GO_SendData(UP_t *u, float pos, float limit)
|
||||
{
|
||||
static int is_calibration=0;
|
||||
static fp32 error=0; //误差量
|
||||
|
||||
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; // 阻尼（原代码中kdd/kid混淆，需统一为K_W）
|
||||
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）
|
||||
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);
|
||||
|
||||
|
||||
|
||||
if (tau > limit) {
|
||||
|
||||
tff = limit - kp * (q_desired - q_current) - kd * (dq_desired - dq_current) - (tau - limit) / 2.0;
|
||||
tau = limit;
|
||||
} else if (tau < -limit) {
|
||||
|
||||
tff = limit - kp * (q_desired - q_current) - kd * (dq_desired - dq_current) + (tau - limit) / 2.0;
|
||||
tau = -limit;
|
||||
/*限制最大输入来限制最大输出*/
|
||||
if (pos - q_current > limit) {
|
||||
u->go_cmd.Pos = q_current + limit; // 限制位置
|
||||
}else if (pos - q_current < -limit) {
|
||||
u->go_cmd.Pos = q_current - limit; // 限制位置
|
||||
}else {
|
||||
u->go_cmd.Pos = pos; // 允许位置
|
||||
}
|
||||
u->go_cmd .T =tff;
|
||||
|
||||
// 发送数据
|
||||
GO_M8010_send_data(&u->go_cmd);
|
||||
u->go_cmd .T =0;
|
||||
|
||||
|
||||
return 0;
|
||||
}
|
||||
@ -226,22 +173,23 @@ int8_t GO_SendData(UP_t *u,float pos,float limit)
|
||||
int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
|
||||
{
|
||||
//电机控制 ，传进can
|
||||
|
||||
UP_angle_control(u,u->motor_target .M2006_angle ,M2006);
|
||||
UP_angle_control(u,u->motor_target .M3508_angle ,M3508 );
|
||||
|
||||
|
||||
|
||||
GO_SendData(u,u->motor_target .go_shoot,0.08 );
|
||||
DJ_Angle_Control(u,1,&u->motorfeedback .DJmotor_feedback[0] ,
|
||||
&u->pid .Shoot_M2006angle ,
|
||||
&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 ,
|
||||
&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++){
|
||||
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 [1]= -u->final_out .final_VESC_5065_M2out ;
|
||||
out ->chassis6020 .as_array [2]=u->final_out .final_pitchout ;
|
||||
|
||||
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 ;
|
||||
is_pitch=0;
|
||||
}
|
||||
u->motor_target .M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ;
|
||||
u->PitchContext .PitchState = PITCH_PREPARE; //状态更新,开始夹球
|
||||
// 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 ;
|
||||
@ -291,7 +239,7 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
|
||||
u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮
|
||||
}
|
||||
|
||||
Pitch_Process(u,out,c);
|
||||
// Pitch_Process(u,out,c);
|
||||
|
||||
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;
|
||||
}
|
||||
Dribble_Process(u,out);
|
||||
// Dribble_Process(u,out);
|
||||
|
||||
|
||||
}break ;
|
||||
case Dribbl_transfer:
|
||||
|
||||
Dribbl_transfer_a(u,out);
|
||||
// Dribbl_transfer_a(u,out);
|
||||
}
|
||||
break;
|
||||
|
||||
@ -338,184 +286,184 @@ return 0;
|
||||
|
||||
|
||||
|
||||
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:
|
||||
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 .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)
|
||||
//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:
|
||||
// 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 .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开
|
||||
//
|
||||
// if(u->motorfeedback .GO_motor_info [1]->Pos > -4.8)
|
||||
// {
|
||||
// u->DribbleContext .DribbleState = STATE_CATCH_DONE; //当go2到标准位置，标志位改变
|
||||
// RELAY2_TOGGLE (0);//夹球，0接1收
|
||||
|
||||
// u->motor_target.go_spin=u->DribbleContext .DribbleConfig.go2_init_angle ;
|
||||
|
||||
// break;
|
||||
|
||||
// break;
|
||||
|
||||
// }
|
||||
//}
|
||||
|
||||
|
||||
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;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@ -91,7 +91,6 @@ typedef struct {
|
||||
fp32 go2_init_angle; // GO2初始角度
|
||||
fp32 go2_flip_angle; // GO2翻转角度
|
||||
fp32 flip_timing; // 翻转触发时机
|
||||
// fp32
|
||||
fp32 go2_release_threshold; // 释放球
|
||||
} DribbleConfig_t;
|
||||
|
||||
@ -113,46 +112,17 @@ typedef struct {
|
||||
|
||||
}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
|
||||
{
|
||||
|
||||
pid_param_t VESC_5065_M1_param;
|
||||
pid_param_t VESC_5065_M2_param;
|
||||
|
||||
pid_param_t UP_GM6020_speed_param;
|
||||
pid_param_t UP_GM6020_angle_param;
|
||||
pid_param_t Shoot_M2006_speed_param;
|
||||
pid_param_t Shoot_M2006_angle_param;
|
||||
|
||||
pid_param_t M2006_speed_param;
|
||||
pid_param_t M2006_angle_param;
|
||||
|
||||
pid_param_t M3508_speed_param;
|
||||
pid_param_t M3508_angle_param;
|
||||
pid_param_t Pitch_M2006_speed_param;
|
||||
pid_param_t Pitch_M2006_angle_param;
|
||||
|
||||
DribbleConfig_t DribbleConfig_Config;
|
||||
PitchConfig_t PitchConfig_Config;
|
||||
@ -162,54 +132,40 @@ typedef struct
|
||||
|
||||
typedef struct
|
||||
{
|
||||
MotorType_t motor;
|
||||
fp32 ecd;
|
||||
fp32 rpm;
|
||||
|
||||
float orig_angle;
|
||||
float last_angle;
|
||||
fp32 orig_angle;
|
||||
fp32 last_angle;
|
||||
int32_t round_cnt;
|
||||
uint16_t init_cnt;
|
||||
float total_angle;
|
||||
|
||||
}motor_angle_data;
|
||||
fp32 total_angle;
|
||||
}DJmotor_feedback_t;
|
||||
|
||||
|
||||
typedef struct{
|
||||
|
||||
uint8_t up_task_run;
|
||||
const UP_Param_t *param;
|
||||
|
||||
int state;
|
||||
|
||||
|
||||
|
||||
/*运球过程*/
|
||||
DribbleContext_t DribbleContext;
|
||||
/*投篮过程*/
|
||||
PitchContext_t PitchContext;
|
||||
|
||||
|
||||
|
||||
|
||||
UP_Imu_t pos088;
|
||||
|
||||
CMD_t *cmd;
|
||||
|
||||
struct{
|
||||
fp32 rotor_pit6020ecd;
|
||||
fp32 rotor_pit6020rpm;
|
||||
|
||||
fp32 VESC_5065_M1_rpm;
|
||||
fp32 VESC_5065_M2_rpm;
|
||||
|
||||
motor_angle_data M2006;
|
||||
motor_angle_data M3508;
|
||||
GO_MotorData_t *go_data;//存放go电机数据
|
||||
|
||||
const GO_MotorData_t *go_data;//存放go电机数据
|
||||
DJmotor_feedback_t DJmotor_feedback[4];
|
||||
|
||||
fp32 M3508_angle[4];
|
||||
fp32 M3508_rpm [4];
|
||||
|
||||
int flag;
|
||||
|
||||
}motorfeedback;
|
||||
|
||||
@ -217,32 +173,27 @@ typedef struct{
|
||||
|
||||
struct{
|
||||
|
||||
fp32 rotor_pit6020angle;
|
||||
|
||||
fp32 VESC_5065_M1_rpm;
|
||||
fp32 VESC_5065_M2_rpm;
|
||||
|
||||
fp32 M2006_angle;
|
||||
fp32 M3508_angle;
|
||||
fp32 Shoot_M2006_angle;
|
||||
fp32 Pitch_M2006_angle;
|
||||
|
||||
fp32 go_shoot;
|
||||
fp32 go_spin;
|
||||
|
||||
}motor_target;
|
||||
|
||||
struct{
|
||||
|
||||
pid_type_def GM6020_speed;
|
||||
pid_type_def GM6020_angle;
|
||||
|
||||
pid_type_def VESC_5065_M1;
|
||||
pid_type_def VESC_5065_M2;
|
||||
|
||||
pid_type_def M2006_angle;
|
||||
pid_type_def M2006_speed;
|
||||
pid_type_def Shoot_M2006angle;
|
||||
pid_type_def Shoot_M2006speed;
|
||||
|
||||
pid_type_def M3508_angle;
|
||||
pid_type_def M3508_speed;
|
||||
pid_type_def Pitch_M2006_angle;
|
||||
pid_type_def Pitch_M2006_speed;
|
||||
|
||||
}pid;
|
||||
GO_MotorCmd_t go_cmd;
|
||||
@ -253,8 +204,7 @@ typedef struct{
|
||||
/*经PID计算后的实际发送给电机的实时输出值*/
|
||||
struct
|
||||
{
|
||||
fp32 final_3508out[4];
|
||||
fp32 final_pitchout;
|
||||
fp32 DJfinal_out[4];
|
||||
fp32 final_VESC_5065_M1out;
|
||||
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_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 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 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,CMD_t *c);
|
||||
int8_t DJ_processdata(DJmotor_feedback_t *f,fp32 ecd_to_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);
|
||||
|
||||
int8_t Dribble_Process(UP_t *u, CAN_Output_t *out);
|
||||
|
||||
void Dribbl_transfer_a(UP_t *u, CAN_Output_t *out);
|
||||
|
||||
#endif
|
||||
|
||||
@ -15,7 +15,6 @@
|
||||
else if ((_IN) >= (_MAX)) \
|
||||
(_IN) = (_MAX); \
|
||||
}
|
||||
GO_MotorCmd_t cmd_gogogo ;
|
||||
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
|
||||
extract_data(&data);
|
||||
}
|
||||
const GO_MotorData_t *get_GO_measure_point(void)
|
||||
GO_MotorData_t *get_GO_measure_point(void)
|
||||
{
|
||||
return &data;
|
||||
}
|
||||
void gogogo(void )
|
||||
{
|
||||
|
||||
|
||||
}
|
||||
@ -107,10 +107,9 @@ typedef struct
|
||||
|
||||
void modify_data(GO_MotorCmd_t *motor_s);
|
||||
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 USART6_RxCompleteCallback(void );
|
||||
void gogogo(void );
|
||||
|
||||
|
||||
#endif /*__GO_M8010_6_H */
|
||||
|
||||
@ -31,7 +31,7 @@ static UP_t UP;
|
||||
static CMD_t up_cmd;
|
||||
|
||||
#endif
|
||||
float aaa=0;
|
||||
float aaa=1;
|
||||
float bbb=0;
|
||||
float CCC=0;
|
||||
float ddd=0;
|
||||
@ -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 .Pitch_M2006_angle =aaa;
|
||||
// UP.motor_target .M2006_angle =bbb ;
|
||||
|
||||
// UP.motor_target .M3508_angle =CCC;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user