209 lines
6.1 KiB
C
209 lines
6.1 KiB
C
|
||
#include "chassis.h"
|
||
|
||
#include "can.h"
|
||
#include "device/at9s_pro.h"
|
||
#include "component/pid.h"
|
||
#include "device/motor_rm.h"
|
||
#include <math.h>
|
||
#include "bsp/time.h"
|
||
|
||
KPID_t pid_wheel[4];
|
||
KPID_Params_t pid_wheel_params={1, 0.8, 0.0, 0.0, 0.0, 0.8,-1.0,-1.0};
|
||
|
||
KPID_t pid_lockagl[4];
|
||
KPID_Params_t pid_lockagl_params={1, 0.8, 0.05, 0.0, 0.1, 0.8,-1.0,M_2PI};
|
||
|
||
KPID_t pid_lockomg[4];
|
||
KPID_Params_t pid_lockomg_params={2, 1, 0.0, 0.01, 0.0, 0.8,-1.0,-1.0};
|
||
|
||
|
||
KPID_t pid_follow;
|
||
KPID_Params_t pid_follow_params={0.5, 1, 0, 0.0, 0, 0.0, -1.0, -1};
|
||
|
||
MOTOR_RM_Param_t motor1to4_param[4]={
|
||
{.can=BSP_CAN_1,.id=0x201,.module=MOTOR_M3508,.reverse=false,.gear=true,},
|
||
{.can=BSP_CAN_1,.id=0x202,.module=MOTOR_M3508,.reverse=false,.gear=true,},
|
||
{.can=BSP_CAN_1,.id=0x203,.module=MOTOR_M3508,.reverse=false,.gear=true,},
|
||
{.can=BSP_CAN_1,.id=0x204,.module=MOTOR_M3508,.reverse=false,.gear=true,}
|
||
};
|
||
|
||
MOTOR_Feedback_t motor1to4_measure[4];
|
||
float lockagl_out[4];
|
||
float chassis_out[4];
|
||
float Wheel_status[4];
|
||
void Inverse_resolve(float vx,float vy,float w)
|
||
{
|
||
Wheel_status[0]=vx-vy+w;
|
||
Wheel_status[1]=vx+vy+w;
|
||
Wheel_status[2]=-vx+vy+w;
|
||
Wheel_status[3]=-vx-vy+w;
|
||
}
|
||
|
||
void Motor_Init(float freq)
|
||
{ for(int i=0;i<4;i++)
|
||
{
|
||
PID_Init(&pid_wheel[i], KPID_MODE_CALC_D, freq,&pid_wheel_params);
|
||
PID_Init(&pid_lockagl[i], KPID_MODE_CALC_D, freq,&pid_lockagl_params);
|
||
PID_Init(&pid_lockomg[i], KPID_MODE_CALC_D, freq,&pid_lockomg_params);
|
||
|
||
}
|
||
|
||
|
||
PID_Init(&pid_follow, KPID_MODE_NO_D, freq,&pid_follow_params);
|
||
BSP_CAN_Init();
|
||
for(int i=0;i<4;i++){
|
||
MOTOR_RM_Register(&motor1to4_param[i]);
|
||
}
|
||
}
|
||
int64_t now,last_wakeup;
|
||
float dt;
|
||
float vx,vy,w;float chassis_yaw;
|
||
float speed[4];
|
||
float lockagl[4]={0,0,0,0};
|
||
void chassis_control(chassis_ctrl_eulr_t eulr, COMP_AT9S_CMD_t cmd_rc)//k:ת<><D7AA><EFBFBD><EFBFBD>
|
||
{
|
||
//float vx,vy,w;
|
||
now =BSP_TIME_Get_us() / 1000000.0f;
|
||
dt =(BSP_TIME_Get_us() - last_wakeup) / 1000000.0f;
|
||
last_wakeup =BSP_TIME_Get_us();
|
||
float delta_angle;
|
||
float cos_delta_angle;
|
||
float sin_delta_angle;
|
||
for(int i=0;i<4;i++){
|
||
MOTOR_RM_Update(&motor1to4_param[i]);
|
||
MOTOR_RM_t *motor_fed = MOTOR_RM_GetMotor(&motor1to4_param[i]);
|
||
if(motor_fed!=NULL)
|
||
{
|
||
motor1to4_measure[i]=motor_fed->motor.feedback;
|
||
}
|
||
}
|
||
switch(cmd_rc.mode){
|
||
case 0:
|
||
vx=0;vy=0;w=0;
|
||
break;
|
||
case 1:
|
||
delta_angle = eulr.chassis_encoder_yaw - eulr.chassis_mech_zero_yaw;
|
||
cos_delta_angle = cosf(delta_angle);
|
||
sin_delta_angle = sinf(delta_angle);
|
||
vx =cos_delta_angle * cmd_rc.chassis_ctrl_vec.x - sin_delta_angle * cmd_rc.chassis_ctrl_vec.y;
|
||
vy =sin_delta_angle * cmd_rc.chassis_ctrl_vec.x + cos_delta_angle * cmd_rc.chassis_ctrl_vec.y;
|
||
w=PID_Calc(&pid_follow, eulr.chassis_mech_zero_yaw,
|
||
eulr.chassis_encoder_yaw, 0.0f, dt);
|
||
break;
|
||
case 2:
|
||
PID_ResetIntegral(&pid_follow);
|
||
|
||
delta_angle = eulr.chassis_encoder_yaw - eulr.chassis_mech_zero_yaw;
|
||
cos_delta_angle = cosf(delta_angle);
|
||
sin_delta_angle = sinf(delta_angle);
|
||
vx =cos_delta_angle * cmd_rc.chassis_ctrl_vec.x - sin_delta_angle * cmd_rc.chassis_ctrl_vec.y;
|
||
vy =sin_delta_angle * cmd_rc.chassis_ctrl_vec.x + cos_delta_angle * cmd_rc.chassis_ctrl_vec.y;
|
||
|
||
w=PID_Calc(&pid_follow, eulr.chassis_mech_zero_yaw,
|
||
eulr.chassis_encoder_yaw, 0.0f, dt);
|
||
break;
|
||
case 3:
|
||
delta_angle = eulr.chassis_encoder_yaw - eulr.chassis_mech_zero_yaw;
|
||
cos_delta_angle = cosf(delta_angle);
|
||
sin_delta_angle = sinf(delta_angle);
|
||
vx =cos_delta_angle * cmd_rc.chassis_ctrl_vec.x - sin_delta_angle * cmd_rc.chassis_ctrl_vec.y;
|
||
vy =sin_delta_angle * cmd_rc.chassis_ctrl_vec.x + cos_delta_angle * cmd_rc.chassis_ctrl_vec.y;
|
||
|
||
w=0.6f;
|
||
break;
|
||
default :
|
||
break;
|
||
}
|
||
|
||
|
||
Inverse_resolve(vx, vy, w);
|
||
float abs_max = 0.0f;
|
||
for (int8_t i = 0; i < 4; i++) {
|
||
const float abs_val = fabsf(Wheel_status[i]);
|
||
abs_max = (abs_val > abs_max) ? abs_val : abs_max;
|
||
}
|
||
if (abs_max > 1.0f) {
|
||
for (int8_t i = 0; i < 4; i++) {
|
||
Wheel_status[i] /= abs_max;
|
||
}
|
||
}
|
||
|
||
// if(vx==0&&vy==0)PID_ResetIntegral(&pid_wheel);
|
||
switch(cmd_rc.mode){
|
||
case 0:
|
||
PID_ResetIntegral(&pid_wheel[0]);
|
||
PID_ResetIntegral(&pid_wheel[1]);
|
||
PID_ResetIntegral(&pid_wheel[2]);
|
||
PID_ResetIntegral(&pid_wheel[3]);
|
||
chassis_out[0] = 0;
|
||
chassis_out[1] = 0;
|
||
chassis_out[2] = 0;
|
||
chassis_out[3] = 0;
|
||
break;
|
||
case 1:
|
||
PID_ResetIntegral(&pid_wheel[0]);
|
||
PID_ResetIntegral(&pid_wheel[1]);
|
||
PID_ResetIntegral(&pid_wheel[2]);
|
||
PID_ResetIntegral(&pid_wheel[3]);
|
||
// chassis_out[0] = 0;
|
||
// chassis_out[1] = 0;
|
||
// chassis_out[2] = 0;
|
||
// chassis_out[3] = 0;
|
||
for(int i=0;i<4;i++){
|
||
speed[i]=motor1to4_measure[i].rotor_speed*19.2032/8000.0f;
|
||
|
||
if(speed[i]>1)speed[i]=1;
|
||
else if(speed[i]<-1)speed[i]=-1;
|
||
lockagl_out[i] = PID_Calc(&pid_lockagl[i],lockagl[i],motor1to4_measure[i].rotor_abs_angle, 0,dt);
|
||
chassis_out[i] = PID_Calc(&pid_lockomg[i],lockagl_out[i],speed[i], 0,dt);
|
||
if(chassis_out[i]>0.8f)chassis_out[i]=0.8f;
|
||
}
|
||
break;
|
||
case 2:
|
||
for(int i=0;i<4;i++){
|
||
|
||
lockagl[i]=motor1to4_measure[i].rotor_abs_angle;
|
||
|
||
speed[i]=motor1to4_measure[i].rotor_speed*19.2032/8000.0f;
|
||
if(speed[i]>1)speed[i]=1;
|
||
else if(speed[i]<-1)speed[i]=-1;
|
||
chassis_out[i] = PID_Calc(&pid_wheel[i],Wheel_status[i],speed[i], 0,dt);
|
||
|
||
}
|
||
|
||
break;
|
||
case 3:
|
||
for(int i=0;i<4;i++){
|
||
|
||
lockagl[i]=motor1to4_measure[i].rotor_abs_angle;
|
||
|
||
speed[i]=motor1to4_measure[i].rotor_speed*19.2032/8000.0f;
|
||
if(speed[i]>1)speed[i]=1;
|
||
else if(speed[i]<-1)speed[i]=-1;
|
||
chassis_out[i] = PID_Calc(&pid_wheel[i],Wheel_status[i],speed[i], 0,dt);
|
||
|
||
}
|
||
break;
|
||
default :
|
||
break;
|
||
}
|
||
|
||
// for(int i;i<4;i++){
|
||
// chassis_out[i]*=0.6;
|
||
// }
|
||
MOTOR_RM_SetOutput(&motor1to4_param[0], chassis_out[0]);
|
||
MOTOR_RM_SetOutput(&motor1to4_param[1], chassis_out[1]);
|
||
MOTOR_RM_SetOutput(&motor1to4_param[2], chassis_out[2]);
|
||
MOTOR_RM_SetOutput(&motor1to4_param[3], chassis_out[3]);
|
||
//MOTOR_RM_SetOutput(&motor1to4_param[1], 0);
|
||
// MOTOR_RM_SetOutput(&motor1to4_param[2], 0);
|
||
MOTOR_RM_Ctrl(&motor1to4_param[0]);
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|