diff --git a/MDK-ARM/R2.uvoptx b/MDK-ARM/R2.uvoptx index 52b5e21..b84add3 100644 --- a/MDK-ARM/R2.uvoptx +++ b/MDK-ARM/R2.uvoptx @@ -103,7 +103,7 @@ 1 0 0 - 6 + 3 @@ -114,13 +114,13 @@ - STLink\ST-LINKIII-KEIL_SWO.dll + BIN\CMSIS_AGDI.dll 0 ST-LINKIII-KEIL_SWO - -U00260035480000034E575152 -O206 -SF1000 -C0 -A0 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP (ARM Core") -D00(2BA01477) -L00(0) -TO131090 -TC10000000 -TT10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM) + -U00160029510000164E574E32 -O206 -SF1000 -C0 -A0 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP (ARM Core") -D00(2BA01477) -L00(0) -TO131090 -TC10000000 -TT10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM) 0 @@ -140,7 +140,7 @@ 0 DLGUARM - (105=-1,-1,-1,-1,0) + 0 @@ -168,27 +168,32 @@ 2 1 - f,0x0A + chassis,0x0A 3 1 - a,0x0A + cmd_fromnuc,0x0A 4 1 - chassis + nucbuf,0x10 5 1 - cmd_fromnuc,0x0A + UP,0x0A 6 1 - nucbuf,0x0A + a,0x0A + + + 7 + 1 + posss @@ -877,6 +882,18 @@ 0 0 + + 6 + 50 + 1 + 0 + 0 + 0 + ..\User\Module\up_utils.c + up_utils.c + 0 + 0 + @@ -887,7 +904,7 @@ 0 7 - 50 + 51 1 0 0 @@ -899,7 +916,7 @@ 7 - 51 + 52 1 0 0 @@ -911,7 +928,7 @@ 7 - 52 + 53 1 0 0 @@ -923,7 +940,7 @@ 7 - 53 + 54 1 0 0 @@ -935,7 +952,7 @@ 7 - 54 + 55 1 0 0 @@ -947,7 +964,7 @@ 7 - 55 + 56 1 0 0 @@ -959,7 +976,7 @@ 7 - 56 + 57 1 0 0 @@ -971,7 +988,7 @@ 7 - 57 + 58 1 0 0 @@ -983,7 +1000,7 @@ 7 - 58 + 59 5 0 0 @@ -995,7 +1012,7 @@ 7 - 59 + 60 1 0 0 @@ -1007,7 +1024,7 @@ 7 - 60 + 61 1 0 0 @@ -1019,7 +1036,7 @@ 7 - 61 + 62 1 0 0 @@ -1039,7 +1056,7 @@ 0 8 - 62 + 63 1 0 0 @@ -1051,7 +1068,7 @@ 8 - 63 + 64 1 0 0 @@ -1063,7 +1080,7 @@ 8 - 64 + 65 1 0 0 @@ -1075,7 +1092,7 @@ 8 - 65 + 66 1 0 0 @@ -1087,7 +1104,7 @@ 8 - 66 + 67 1 0 0 @@ -1099,7 +1116,7 @@ 8 - 67 + 68 1 0 0 @@ -1111,7 +1128,7 @@ 8 - 68 + 69 1 0 0 @@ -1123,7 +1140,7 @@ 8 - 69 + 70 1 0 0 @@ -1135,7 +1152,7 @@ 8 - 70 + 71 1 0 0 @@ -1147,7 +1164,7 @@ 8 - 71 + 72 1 0 0 @@ -1167,7 +1184,7 @@ 0 9 - 72 + 73 1 0 0 @@ -1179,7 +1196,7 @@ 9 - 73 + 74 1 0 0 @@ -1191,7 +1208,7 @@ 9 - 74 + 75 1 0 0 @@ -1203,7 +1220,7 @@ 9 - 75 + 76 1 0 0 @@ -1215,7 +1232,7 @@ 9 - 76 + 77 1 0 0 @@ -1227,7 +1244,7 @@ 9 - 77 + 78 1 0 0 @@ -1247,7 +1264,7 @@ 0 10 - 78 + 79 1 0 0 @@ -1259,7 +1276,7 @@ 10 - 79 + 80 1 0 0 @@ -1271,7 +1288,7 @@ 10 - 80 + 81 1 0 0 @@ -1283,7 +1300,7 @@ 10 - 81 + 82 1 0 0 @@ -1295,7 +1312,7 @@ 10 - 82 + 83 1 0 0 @@ -1307,7 +1324,7 @@ 10 - 83 + 84 1 0 0 @@ -1319,7 +1336,7 @@ 10 - 84 + 85 1 0 0 @@ -1339,7 +1356,7 @@ 0 11 - 85 + 86 1 0 0 @@ -1351,7 +1368,7 @@ 11 - 86 + 87 1 0 0 @@ -1363,7 +1380,7 @@ 11 - 87 + 88 1 0 0 @@ -1383,7 +1400,7 @@ 0 12 - 88 + 89 1 0 0 @@ -1403,7 +1420,7 @@ 0 13 - 89 + 90 1 0 0 @@ -1415,7 +1432,7 @@ 13 - 90 + 91 1 0 0 @@ -1427,7 +1444,7 @@ 13 - 91 + 92 1 0 0 @@ -1439,7 +1456,7 @@ 13 - 92 + 93 1 0 0 diff --git a/MDK-ARM/R2.uvprojx b/MDK-ARM/R2.uvprojx index 42309e5..893edd4 100644 --- a/MDK-ARM/R2.uvprojx +++ b/MDK-ARM/R2.uvprojx @@ -1113,6 +1113,11 @@ 1 ..\User\Module\up.c + + up_utils.c + 1 + ..\User\Module\up_utils.c + diff --git a/MDK-ARM/R2/R2.axf b/MDK-ARM/R2/R2.axf index 011c36e..c030fe1 100644 Binary files a/MDK-ARM/R2/R2.axf and b/MDK-ARM/R2/R2.axf differ diff --git a/User/Module/Chassis.c b/User/Module/Chassis.c index f0593f6..ff17d3f 100644 --- a/User/Module/Chassis.c +++ b/User/Module/Chassis.c @@ -28,11 +28,13 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can) { c->motorfeedback.rotor_rpm3508[i] = can->motor.chassis_motor3508.as_array[i].rotor_speed; c->motorfeedback.rotor_current3508[i] = can->motor.chassis_motor3508.as_array[i].torque_current; } - for (uint8_t i = 0; i < 4; i++) { - - c->sick_cali.sick_dis[i]=can->sickfed.raw_dis[i] / 10; //没使用原值 - } - +// for (uint8_t i = 0; i < 4; i++) { +// +// c->sick_cali.sick_dis[i]=can->sickfed.raw_dis[i] / 10; //没使用原值 +// } + c->sick_cali.sick_dis[0]=can->sickfed.raw_dis[0] ; + c->sick_cali.sick_dis[1]=can->sickfed.raw_dis[1]+50 ; //有点误差，手动补偿 + c->sick_cali.sick_dis[2]=can->sickfed.raw_dis[2] ; return CHASSIS_OK; } @@ -119,7 +121,7 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) { case AUTO: switch (c->chassis_ctrl.mode) { case AUTO_MID360: - c->move_vec.Vw = ctrl->cmd_MID360.posw * 1000; + c->move_vec.Vw = ctrl->cmd_MID360.posw * 1000; c->move_vec.Vy = -ctrl->cmd_MID360.posy * 1000; c->move_vec.Vx = -ctrl->cmd_MID360.posx * 1000; abs_limit_fp(&c->move_vec.Vx, 5000.0f); diff --git a/User/Module/up.c b/User/Module/up.c index e5608d7..a99de06 100644 --- a/User/Module/up.c +++ b/User/Module/up.c @@ -3,6 +3,8 @@ #include "user_math.h" #include "bsp_buzzer.h" #include "bsp_delay.h" + + /*接线 @@ -80,30 +82,7 @@ int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) { return 0; } -int8_t DJ_processdata(DJmotor_feedback_t *f,fp32 ecd_to_angle) -{ - int8_t cnt=0; - fp32 angle ,delta; - - 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， */ @@ -128,7 +107,8 @@ int8_t VESC_M5065_Control(UP_t *u,fp32 speed) { u->motor_target .VESC_5065_M1_rpm =speed; u->motor_target .VESC_5065_M2_rpm =speed; - + + //根据正反加负号 u->final_out .final_VESC_5065_M1out =-u->motor_target .VESC_5065_M1_rpm; u->final_out .final_VESC_5065_M2out =u->motor_target .VESC_5065_M2_rpm; @@ -138,43 +118,7 @@ int8_t VESC_M5065_Control(UP_t *u,fp32 speed) -/*go电机控制*/ -int8_t GO_SendData(UP_t *u, float pos, float limit) -{ - static int is_calibration=0; - static fp32 error=0; //误差量 - - u->motorfeedback .go_data = get_GO_measure_point() ; - - - // 读取参数 - 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); - - /*限制最大输入来限制最大输出*/ - 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; // 允许位置 - } - - // 发送数据 - GO_M8010_send_data(&u->go_cmd); - - - return 0; -} int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out) { @@ -183,16 +127,21 @@ int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out) &u->pid .Shoot_M2006angle , &u->pid .Shoot_M2006speed , u->motor_target .Shoot_M2006_angle ); + + /*俯仰2006，双环，内使用oid角度环，外电机速度环 + + */ DJ_Speed_Control(u,0x206,&u->motorfeedback .DJmotor_feedback[5],&u->pid .Pitch_M2006_speed, (PID_calc (&(u->pid .Pitch_M2006_angle ), - u->motorfeedback .Encoder.angle,u->motor_target .Shoot_Pitch_angle)) + u->motorfeedback .Encoder.angle,u->motor_target .Pitch_angle)) ); - GO_SendData(u, - u->motor_target .go_shoot, - u->Pitch_Cfg .PitchConfig .pull_speed );//对应的拉动速度 + GO_SendData( + &u->motorfeedback.go_data, + &u->go_cmd,u->motor_target .go_shoot, + u->Pitch_Cfg .PitchConfig .pull_speed + );//对应的拉动速度 - for(int i=0;i<4;i++){ out ->motor_UP3508_Dribble.as_array[i]=u->final_out.DJfinal_out [i] ; } @@ -208,24 +157,31 @@ int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out) } - +fp32 posss=0; int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c) { - if(u ==NULL) return 0; + if(u ==NULL) return 0; if(out ==NULL) return 0; if(c ==NULL) return 0; /*指针简化*/ - PitchCfg_t *pitch_cfg = &u->Pitch_Cfg.PitchConfig; - + PitchCfg_t *pitch_cfg = &u->Pitch_Cfg.PitchConfig; up_motor_target_t *target=&u->motor_target ; - static int is_pitch=1; - + /*config值限位*/ abs_limit_min_max_fp(&u->Pitch_Cfg.PitchConfig.go_release_pos ,-215.0f,0.0f); + abs_limit_min_max_fp(&u->Pitch_Cfg.PitchConfig.Pitch_angle ,48.0f,67.0f); - + /*部分数据更新*/ + static int is_pitch=1; + posss=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos),3,4,&u->Pitch_Cfg.Curve); +// if (u->Pitch_Cfg.Curve == CURVE_58) { +// target->Pitch_angle = 58; +// } else { +// target->Pitch_angle = 66; +// } + switch (c->CMD_CtrlType ) { case RCcontrol: //在手动模式下 @@ -236,10 +192,11 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c) case Normal : /*投篮*/ if(is_pitch){ - target->go_shoot =pitch_cfg->go_init ; - target->Shoot_Pitch_angle=pitch_cfg->Pitch_angle; + target->Shoot_M2006_angle = pitch_cfg->go_init ; + target->Pitch_angle = pitch_cfg->Pitch_angle; is_pitch=0; } //让初始go位置只读一次，后面按调整模式更改的来,后面稳定了之后，可以跟随调整模式下一块删 + target->Shoot_M2006_angle =u->Pitch_Cfg .PitchConfig .m2006_init ; u->Pitch_Cfg .PitchState = PITCH_PREPARE; //状态更新,开始夹球 @@ -261,26 +218,31 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c) pitch_cfg->Pitch_angle += c->Vy/100; target->go_shoot=u->Pitch_Cfg.PitchConfig.go_release_pos; - target->Shoot_Pitch_angle=u->Pitch_Cfg.PitchConfig.Pitch_angle; + target->Pitch_angle=u->Pitch_Cfg.PitchConfig.Pitch_angle; break ; - - default: - break; } + break; case AUTO: + /*自动下数据更新，根据距离切换俯仰角,如需自动调整，放在*/ + if (u->Pitch_Cfg.Curve == CURVE_58) { + target->Pitch_angle = 58; + } else { + target->Pitch_angle = 66; + } + switch(c-> CMD_mode) { case AUTO_MID360_Pitch: - pitch_cfg ->go_init=LowPassFilter2p_Apply(&u->filled[0],c->pos); if (u->Pitch_Cfg .PitchState ==PITCH_PREPARE) { u->Pitch_Cfg .PitchState=PITCH_START;//置标志位用于启动投篮 } - /*根据距离实时计算所需pos*/ - pitch_cfg ->go_release_pos=c->pos; - + /*根据距离实时计算所需pos*/ + + pitch_cfg ->go_release_pos= + CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos),3,4,&u->Pitch_Cfg.Curve); Pitch_Process(u,out); break ; @@ -294,13 +256,20 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c) } - -return 0; + break ; + case RELAXED: +// target->go_shoot= pitch_cfg . + + target->go_shoot = pitch_cfg->go_init; + target->Pitch_angle = 58; + target->Shoot_M2006_angle =pitch_cfg->m2006_init; + + break ; } - +return 0; } @@ -320,10 +289,10 @@ int8_t Pitch_Process(UP_t *u, CAN_Output_t *out) case PITCH_START: // u->motor_target .Dribble_translate_M3508_angle =u->DribbleCont .DribbleConfig.m3508_translate_angle;//平行移动 - cfg->pull_speed=10; + cfg->pull_speed=12; target->go_shoot = cfg->go_pull_pos; - if(u->motorfeedback .go_data ->Pos < -209){ //检测go位置到达最上面，这里的检测条件可以更改 + if(u->motorfeedback .go_data .Pos < -209){ //检测go位置到达最上面，这里的检测条件可以更改 target->Shoot_M2006_angle = cfg->m2006_trig ;//设置2006角度，0 u->Pitch_Cfg .PitchConfig .pull_speed=6; *state=PITCH_PULL_TRIGGER; @@ -336,7 +305,7 @@ int8_t Pitch_Process(UP_t *u, CAN_Output_t *out) if( u->motorfeedback .DJmotor_feedback [4].total_angle>-1) //当2006的总角度小于1，可以认为已经勾上,误差为1 { target->go_shoot=cfg->go_release_pos; - if(is_reached (u->motorfeedback .go_data ->Pos ,target->go_shoot ,1.0f)) + if(is_reached (u->motorfeedback .go_data .Pos ,target->go_shoot ,1.0f)) { *state=PITCH_LAUNCHING; } diff --git a/User/Module/up.h b/User/Module/up.h index 219461a..51c96f5 100644 --- a/User/Module/up.h +++ b/User/Module/up.h @@ -13,6 +13,7 @@ #include "vofa.h" #include "GO_M8010_6_Driver.h" #include "bsp_usart.h" +#include "up_utils.h" /* 状态机 @@ -34,18 +35,17 @@ typedef enum { PITCH_START, //启动，拉扳机 PITCH_PULL_TRIGGER, PITCH_LAUNCHING, // 发射中 - PITCH_COMPLETE // 完成 } PitchState_t; /* 投球参数配置 */ typedef struct { - fp32 m2006_init; // M2006初始角度 + fp32 m2006_init; // M2006初始角度 fp32 m2006_trig; // M2006触发角度0,用来合并扳机 fp32 go_init; // GO电机触发位置,初始位置，后续更改用于发射的位置 fp32 go_pull_pos; // go上升去合并扳机，扳机位置 - fp32 Pitch_angle; + fp32 Pitch_angle; //俯仰角度，以oid为准 fp32 pull_speed;//go速度 fp32 go_release_pos;//go松开，发射位置 @@ -55,7 +55,7 @@ typedef struct { typedef struct { PitchState_t PitchState; PitchCfg_t PitchConfig; - + CurveType Curve; //当前函数，雷达距离->pos uint8_t is_init ; } Pitch_Cfg_t; @@ -101,11 +101,10 @@ typedef struct{ fp32 VESC_5065_M2_rpm; fp32 Shoot_M2006_angle; - fp32 Pitch_M2006_angle; fp32 go_shoot; - fp32 Shoot_Pitch_angle; + fp32 Pitch_angle; //以oid的角度为准,目标俯仰角 }up_motor_target_t; @@ -125,18 +124,6 @@ typedef struct{ }up_pid_t; -typedef struct -{ - fp32 ecd; - fp32 rpm; - uint8_t id; - fp32 orig_angle; - fp32 last_angle; - int32_t round_cnt; - int init_cnt; - fp32 total_angle; -}DJmotor_feedback_t; - typedef struct{ @@ -153,7 +140,7 @@ typedef struct{ fp32 VESC_5065_M1_rpm; fp32 VESC_5065_M2_rpm; }VESC; - GO_MotorData_t *go_data;//存放go电机数据 + GO_MotorData_t go_data;//存放go电机数据 DJmotor_feedback_t DJmotor_feedback[6]; @@ -191,7 +178,6 @@ 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 VESC_M5065_Control(UP_t *u,fp32 speed); -int8_t GO_SendData(UP_t *u,float pos,float limit); 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 DJ_processdata(DJmotor_feedback_t *f,fp32 ecd_to_angle); diff --git a/User/Module/up_utils.c b/User/Module/up_utils.c new file mode 100644 index 0000000..bbcbd26 --- /dev/null +++ b/User/Module/up_utils.c @@ -0,0 +1,101 @@ +#include "up_utils.h" + + +int8_t DJ_processdata(DJmotor_feedback_t *f,fp32 ecd_to_angle) +{ + int8_t cnt=0; + fp32 angle ,delta; + + 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; + +} +/*go电机控制*/ + +int8_t GO_SendData( GO_MotorData_t *go_data,GO_MotorCmd_t *go_cmd,float pos, float limit) +{ + + *go_data = *get_GO_measure_point() ; + + + // 读取参数 + float tff = go_cmd->T; // 前馈力矩 + float kp = go_cmd->K_P; // 位置刚度 + float kd = go_cmd->K_W; // 速度阻尼 + float q_desired = go_cmd->Pos; // 期望位置（rad） + float q_current = go_data->Pos; // 当前角度位置（rad） + float dq_desired = go_cmd->W; // 期望角速度（rad/s） + float dq_current = go_data->W; // 当前角速度（rad/s） + + // 计算输出力矩 tau + float tau = tff + kp * (q_desired - q_current) + kd * (dq_desired - dq_current); + + /*限制最大输入来限制最大输出*/ + if (pos - q_current > limit) { + go_cmd->Pos = q_current + limit; // 限制位置 + }else if (pos - q_current < -limit) { + go_cmd->Pos = q_current - limit; // 限制位置 + }else { + go_cmd->Pos = pos; // 允许位置 + } + + // 发送数据 + GO_M8010_send_data(go_cmd); + + + return 0; +} + + +// 计算58度曲线 +static float curve_58(float d) { + return 0.448f * d * d + 24.8604f * d - 177.99552f; +} + +// 计算66度曲线 +static float curve_66(float d) { + return 8.84935f * d * d - 10.5424f * d - 126.791f; +} + + +/* + 曲线切换,用于距离和pos拟合 + 迟滞区x-y + 曲线x重合区，根据当前函数和变化方向切换 +*/ +float CurveChange(float d, float x, float y, CurveType *cs) +{ + + if (*cs == CURVE_66) { + if (d > y) { + *cs = CURVE_58; + } + } else { // CURVE_58 + if (d < x) { + *cs = CURVE_66; + } + } + + // 根据当前曲线返回结果 + if (*cs == CURVE_58) { + return curve_58(d); + } else { + return curve_66(d); + } +} + diff --git a/User/Module/up_utils.h b/User/Module/up_utils.h new file mode 100644 index 0000000..c29ab78 --- /dev/null +++ b/User/Module/up_utils.h @@ -0,0 +1,56 @@ +#ifndef UP_UTILS_H +#define UP_UTILS_H + +#include "gpio.h" +#include "user_math.h" +#include "bsp_buzzer.h" +#include "bsp_delay.h" +#include "struct_typedef.h" +#include "pid.h" +#include "user_math.h" +#include "ahrs.h" +#include "filter.h" +#include "bsp_usart.h" +#include "GO_M8010_6_Driver.h" + + +#define CAN2_G3508_ID (0x200) +#define GEAR_RATIO_2006 (36) // 2006ٱ +#define GEAR_RATIO_3508 (19) + +#define CAN_MOTOR_ENC_RES 8191 // ֱ +#define MOTOR2006_ECD_TO_ANGLE (360.0 / 8191.0 / (GEAR_RATIO_2006)) //2006ֵתǶ +#define MOTOR3508_ECD_TO_ANGLE (360.0 / 8191.0 / (GEAR_RATIO_3508)) //3508ֵתǶ + + +typedef struct +{ + fp32 ecd; + fp32 rpm; + uint8_t id; + fp32 orig_angle; + fp32 last_angle; + int32_t round_cnt; + int init_cnt; + fp32 total_angle; +}DJmotor_feedback_t; + + +typedef enum { + CURVE_58, + CURVE_66 +} CurveType; + + +static float curve_58(float d) ; +static float curve_66(float d) ; +float CurveChange(float d, float x, float y, CurveType *cs); + +int8_t GO_SendData( GO_MotorData_t *go_data,GO_MotorCmd_t *go_cmd,float pos, float limit); + +int8_t DJ_processdata(DJmotor_feedback_t *f,fp32 ecd_to_angle); + + + + +#endif diff --git a/User/device/nuc.c b/User/device/nuc.c index 27eda9c..032a0db 100644 --- a/User/device/nuc.c +++ b/User/device/nuc.c @@ -62,7 +62,7 @@ int8_t NUC_Restart(void) { return DEVICE_OK; } bool_t NUC_WaitDmaCplt(void) { - return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll,100) == + return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll,20) == SIGNAL_NUC_RAW_REDY); } diff --git a/User/task/user_task.h b/User/task/user_task.h index 81c836e..2d0a387 100644 --- a/User/task/user_task.h +++ b/User/task/user_task.h @@ -18,7 +18,7 @@ #define TASK_FREQ_UP (900u) #define TASK_FREQ_CTRL_CMD (500u) -#define TASK_FREQ_NUC (500u) +#define TASK_FREQ_NUC (50u) #define TASK_FREQ_CAN (1000u) #define TASK_FREQ_RC (1000u)