Yodel/25_R1_chassis
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
3f167b1b24
25_R1_chassis/User/Module/Chassis.c

296 lines
14 KiB
C
Raw Blame History

This file contains ambiguous Unicode characters

This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

#include "Chassis.h"
/*舵轮舵向校准方法：注释掉关于6020反馈角度的处理以及6020数据的发送这两处，进debug将四个轮子编码器朝左查看6020反馈值，
将6020反馈值放入motor_offset中*/
static int8_t Chassis_SetCtrl(Chassis_t *c,CMD_t *ctrl){
c->mode =ctrl->C_cmd.mode;
c->pos =ctrl->C_cmd.pos;
return 0;
}
//底盘解算
void Chassis_speed_calculate(Chassis_t *c,Action_POS_t*pos) {
// RC模式下松开遥控器防止6020回到默认位置导致侧翻
if (c->mode == RC&&fabs(c->move_vec.Vx) < 3000 && fabs(c->move_vec.Vy) < 3000 && fabs(c->move_vec.Vw) < 3000) {
// 如果之前不处于保持模式，则记录当前角度
if (!c->keeping_angle_flag) {
for (uint8_t i = 0; i < 4; i++) {
c->keep_angle[i] = c->motorfeedback.rotor_angle6020[i];
}
c->keeping_angle_flag = 1; // 进入保持模式
}
// 使用保持的角度，而不是实时反馈值
for (uint8_t i = 0; i < 4; i++) {
c->hopemotorout.rotor6020_jiesuan_1[i] = c->keep_angle[i];
}
// 速度设为 0
for (uint8_t i = 0; i < 4; i++) {
c->hopemotorout.rotor5065_jiesuan_1[i] = 0;
}
} else {
// 如果有速度输入，则退出保持模式
c->keeping_angle_flag = 0;
switch(c->mode){
case RC: //RC模式下遥控器控制底盘运动，操控为机器人坐标系 x朝前，y朝右
//解算得到5065速度，加-号是因为vesc校准时轮向电机默认转动正方向不同，可从上位机里改
c->hopemotorout.rotor5065_jiesuan_1[0]=sqrt(
(c->move_vec.Vx+c->move_vec.Vw*sin(radians))*(c->move_vec.Vx+c->move_vec.Vw*sin(radians))+
(c->move_vec.Vy+c->move_vec.Vw*cos(radians))*(c->move_vec.Vy+c->move_vec.Vw*cos(radians)));
c->hopemotorout.rotor5065_jiesuan_1[1]=-sqrt(
(c->move_vec.Vx-c->move_vec.Vw*sin(radians))*(c->move_vec.Vx-c->move_vec.Vw*sin(radians))+
(c->move_vec.Vy+c->move_vec.Vw*cos(radians))*(c->move_vec.Vy+c->move_vec.Vw*cos(radians)));
c->hopemotorout.rotor5065_jiesuan_1[2]=sqrt(
(c->move_vec.Vx+c->move_vec.Vw*sin(radians))*(c->move_vec.Vx+c->move_vec.Vw*sin(radians))+
(c->move_vec.Vy-c->move_vec.Vw*cos(radians))*(c->move_vec.Vy-c->move_vec.Vw*cos(radians)));
c->hopemotorout.rotor5065_jiesuan_1[3]=-sqrt(
(c->move_vec.Vx-c->move_vec.Vw*sin(radians))*(c->move_vec.Vx-c->move_vec.Vw*sin(radians))+
(c->move_vec.Vy-c->move_vec.Vw*cos(radians))*(c->move_vec.Vy-c->move_vec.Vw*cos(radians)));
//解算得到6020角度（-180°——+180°）
c->hopemotorout.rotor6020_jiesuan_1[0]=atan2((c->move_vec.Vy+c->move_vec.Vw*cos(radians)),
(c->move_vec.Vx+c->move_vec.Vw*sin(radians)))* (180 / M_PI);
c->hopemotorout.rotor6020_jiesuan_1[1]=atan2((c->move_vec.Vy+c->move_vec.Vw*cos(radians)),
(c->move_vec.Vx-c->move_vec.Vw*sin(radians)))* (180 / M_PI);
c->hopemotorout.rotor6020_jiesuan_1[2]=atan2((c->move_vec.Vy-c->move_vec.Vw*cos(radians)),
(c->move_vec.Vx+c->move_vec.Vw*sin(radians)))* (180 / M_PI);
c->hopemotorout.rotor6020_jiesuan_1[3]=atan2((c->move_vec.Vy-c->move_vec.Vw*cos(radians)),
(c->move_vec.Vx-c->move_vec.Vw*sin(radians)))* (180 / M_PI);
break;
case STOP: //停止模式下轮子锁死
for(int i =0 ; i < 4 ; i++){
c->hopemotorout.rotor5065_jiesuan_1[i]=0;
}
c->hopemotorout.rotor6020_jiesuan_1[0]=315;
c->hopemotorout.rotor6020_jiesuan_1[1]=45;
c->hopemotorout.rotor6020_jiesuan_1[2]=45;
c->hopemotorout.rotor6020_jiesuan_1[3]=315;
break;
case NAVI: //导航模式下操控为世界坐标系，能实现舵轮小陀螺前进
// c->chassis_yaw = pos->pos_yaw* (M_PI / 180.0f);
// float cos_yaw = cosf(c->chassis_yaw);
// float sin_yaw = sinf(c->chassis_yaw);
//
// // 将速度从世界坐标系转换到底盘坐标系
// float Vx_local = c->move_vec.Vx * cos_yaw + c->move_vec.Vy * sin_yaw;
// float Vy_local = -c->move_vec.Vx * sin_yaw + c->move_vec.Vy * cos_yaw;
//
// //解算得到5065速度，加-号是因为vesc校准时轮向电机默认转动正方向不同，可从上位机里改
// c->hopemotorout.rotor5065_jiesuan_1[0]=sqrt(
// (Vx_local + c->move_vec.Vw * sin(radians)) * (Vx_local + c->move_vec.Vw * sin(radians)) +
// (Vy_local + c->move_vec.Vw * cos(radians)) * (Vy_local + c->move_vec.Vw * cos(radians)));
//
// c->hopemotorout.rotor5065_jiesuan_1[1]=-sqrt(
// (Vx_local - c->move_vec.Vw * sin(radians)) * (Vx_local - c->move_vec.Vw * sin(radians)) +
// (Vy_local + c->move_vec.Vw * cos(radians)) * (Vy_local + c->move_vec.Vw * cos(radians)));
//
// c->hopemotorout.rotor5065_jiesuan_1[2]=sqrt(
// (Vx_local + c->move_vec.Vw * sin(radians)) * (Vx_local + c->move_vec.Vw * sin(radians)) +
// (Vy_local - c->move_vec.Vw * cos(radians)) * (Vy_local - c->move_vec.Vw * cos(radians)));
//
// c->hopemotorout.rotor5065_jiesuan_1[3]=-sqrt(
// (Vx_local - c->move_vec.Vw * sin(radians)) * (Vx_local - c->move_vec.Vw * sin(radians)) +
// (Vy_local - c->move_vec.Vw * cos(radians)) * (Vy_local - c->move_vec.Vw * cos(radians)));
//
// //解算得到6020角度（-180°——+180°）
// c->hopemotorout.rotor6020_jiesuan_1[0]= atan2((Vy_local + c->move_vec.Vw * cos(radians)),
// (Vx_local + c->move_vec.Vw * sin(radians))) * (180 / M_PI);
// c->hopemotorout.rotor6020_jiesuan_1[1]= atan2((Vy_local + c->move_vec.Vw * cos(radians)),
// (Vx_local - c->move_vec.Vw * sin(radians))) * (180 / M_PI);
// c->hopemotorout.rotor6020_jiesuan_1[2]= atan2((Vy_local - c->move_vec.Vw * cos(radians)),
// (Vx_local + c->move_vec.Vw * sin(radians))) * (180 / M_PI);
// c->hopemotorout.rotor6020_jiesuan_1[3]= atan2((Vy_local - c->move_vec.Vw * cos(radians)),
// (Vx_local - c->move_vec.Vw * sin(radians))) * (180 / M_PI);
//相机纠正时就用正常解算
c->hopemotorout.rotor5065_jiesuan_1[0]=sqrt(
(c->move_vec.Vx+c->move_vec.Vw*sin(radians))*(c->move_vec.Vx+c->move_vec.Vw*sin(radians))+
(c->move_vec.Vy+c->move_vec.Vw*cos(radians))*(c->move_vec.Vy+c->move_vec.Vw*cos(radians)));
c->hopemotorout.rotor5065_jiesuan_1[1]=-sqrt(
(c->move_vec.Vx-c->move_vec.Vw*sin(radians))*(c->move_vec.Vx-c->move_vec.Vw*sin(radians))+
(c->move_vec.Vy+c->move_vec.Vw*cos(radians))*(c->move_vec.Vy+c->move_vec.Vw*cos(radians)));
c->hopemotorout.rotor5065_jiesuan_1[2]=sqrt(
(c->move_vec.Vx+c->move_vec.Vw*sin(radians))*(c->move_vec.Vx+c->move_vec.Vw*sin(radians))+
(c->move_vec.Vy-c->move_vec.Vw*cos(radians))*(c->move_vec.Vy-c->move_vec.Vw*cos(radians)));
c->hopemotorout.rotor5065_jiesuan_1[3]=-sqrt(
(c->move_vec.Vx-c->move_vec.Vw*sin(radians))*(c->move_vec.Vx-c->move_vec.Vw*sin(radians))+
(c->move_vec.Vy-c->move_vec.Vw*cos(radians))*(c->move_vec.Vy-c->move_vec.Vw*cos(radians)));
//解算得到6020角度（-180°——+180°）
c->hopemotorout.rotor6020_jiesuan_1[0]=atan2((c->move_vec.Vy+c->move_vec.Vw*cos(radians)),
(c->move_vec.Vx+c->move_vec.Vw*sin(radians)))* (180 / M_PI);
c->hopemotorout.rotor6020_jiesuan_1[1]=atan2((c->move_vec.Vy+c->move_vec.Vw*cos(radians)),
(c->move_vec.Vx-c->move_vec.Vw*sin(radians)))* (180 / M_PI);
c->hopemotorout.rotor6020_jiesuan_1[2]=atan2((c->move_vec.Vy-c->move_vec.Vw*cos(radians)),
(c->move_vec.Vx+c->move_vec.Vw*sin(radians)))* (180 / M_PI);
c->hopemotorout.rotor6020_jiesuan_1[3]=atan2((c->move_vec.Vy-c->move_vec.Vw*cos(radians)),
(c->move_vec.Vx-c->move_vec.Vw*sin(radians)))* (180 / M_PI);
break;
}
}
//角度归化到0°——360°
for (uint8_t i = 0; i < 4; i++) {
if(c->hopemotorout.rotor6020_jiesuan_1[i]<0){
c->hopemotorout.rotor6020_jiesuan_1[i]+=360;
}
}
//舵向电机就近转位
float angle_error[4];//角度误差
for (uint8_t i = 0; i < 4; i++) {
angle_error[i]=c->hopemotorout.rotor6020_jiesuan_1[i]-c->motorfeedback.rotor_angle6020[i];
//误差角度归化到-180°——+180°
while (angle_error[i] > 180) angle_error[i] -= 360;
while (angle_error[i] < -180) angle_error[i] += 360;
if(angle_error[i]>90&&angle_error[i]<=180){
c->hopemotorout.rotor5065_jiesuan_2[i]=-c->hopemotorout.rotor5065_jiesuan_1[i];
c->hopemotorout.rotor6020_jiesuan_2[i]=c->motorfeedback.rotor_angle6020[i]+angle_error[i]-180;
}
else if(angle_error[i]<-90&&angle_error[i]>=-180){
c->hopemotorout.rotor5065_jiesuan_2[i]=-c->hopemotorout.rotor5065_jiesuan_1[i];
c->hopemotorout.rotor6020_jiesuan_2[i]=c->motorfeedback.rotor_angle6020[i]+angle_error[i]+180;
}
else{
c->hopemotorout.rotor5065_jiesuan_2[i]=c->hopemotorout.rotor5065_jiesuan_1[i];
c->hopemotorout.rotor6020_jiesuan_2[i]=c->motorfeedback.rotor_angle6020[i]+angle_error[i];
}
}
}
//该函数用来更新can任务获得的电机反馈值
int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can) {
for (uint8_t i = 0; i < 4; i++) {
c->motorfeedback.rotor_rpm3508[i] = can->motor.motor3508.as_array[i].rotor_speed;
c->motorfeedback.rotor_current3508[i] = can->motor.motor3508.as_array[i].torque_current;
c->motorfeedback.rotor_rpm6020[i] = can->motor.chassis6020.as_array[i].rotor_speed;
c->motorfeedback.rotor_current6020[i] = can->motor.chassis6020.as_array[i].torque_current;
c->motorfeedback.rotor_angle6020[i] = can->motor.chassis6020.as_array[i].rotor_angle;
//由于安装不能保证0点朝向我们想要朝向的方向，所以进行零点偏移
c->motorfeedback.rotor_angle6020[i] = fmod(can->motor.chassis6020.as_array[i].rotor_angle -
c->motoroffset.MOTOR_OFFSET[i], 360.0);
if(c->motorfeedback.rotor_angle6020[i]<0){
c->motorfeedback.rotor_angle6020[i]+=360;
}
c->motorfeedback.rotor_rpm5065[i] = can->motor.chassis5065.as_array[i].rotor_speed;
c->motorfeedback.torque_current5065[i] = can->motor.chassis5065.as_array[i].torque_current;
}
return CHASSIS_OK;
}
//底盘初始化
int8_t Chassis_init(Chassis_t *c,const Chassis_Param_t *param,float target_freq){
c->param = param; /*初始化参数 */
//舵轮安装时的6020机械误差，机械校准时1号轮在左前方，所有轮的编码器朝向左面
MotorOffset_t motor_offset = { {209.281338, 329.226851, 328.404256, 208.594717} };
c->motoroffset = motor_offset; // 将 motor_offset 的值赋给 c->motoroffset
PID_init(&(c->pid.chassis_6020OmegaPid), PID_POSITION,&(c->param->C6020Omega_param));
PID_init(&(c->pid.chassis_6020anglePid), PID_POSITION,&(c->param->C6020Angle_param));
PID_init(&(c->pid.chassis_CameraanglePID),PID_POSITION,&(c->param->CameraAngle_param));
PID_init(&(c->pid.chassis_CameraspeedPID),PID_POSITION,&(c->param->CameraSpeed_param));
LowPassFilter2p_Init(&(c->filled[1]),target_freq,80.0f); //给w 做滤波
LowPassFilter2p_Init(&(c->filled[2]),target_freq,80.0f); //给y做滤波
LowPassFilter2p_Init(&(c->filled[3]),target_freq,80.0f); //给x 做滤波
return CHASSIS_OK;
}
//底盘控制
int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out, Action_POS_t*pos){
Chassis_SetCtrl(c,ctrl);
fp32 chassis6020_detangle[4];
//对相机发送的像素点偏移量进行双环pid
fp32 camera_detangle;
camera_detangle = -PID_calc(&(c->pid.chassis_CameraanglePID),ctrl->camera_yaw,0);
c->chassis_yaw_pid = PID_calc(&(c->pid.chassis_CameraspeedPID),c->pos088.bmi088.gyro.z,camera_detangle);
switch (c->mode){
case RC:
c->move_vec.Vx = ctrl->Vx*95000;
c->move_vec.Vy = ctrl->Vy*95000;
c->move_vec.Vw = ctrl->Vw*95000;
break;
case STOP:
c->move_vec.Vx =0;
c->move_vec.Vy =0;
c->move_vec.Vw =0;
break;
case NAVI:
c->move_vec.Vx =ctrl->C_navi.vx ;
c->move_vec.Vy =ctrl->C_navi.vy ;
c->move_vec.Vw =ctrl->C_navi.wz ;
// //像素点偏移量
// if(ctrl->camera_yaw != 0){
//// if(c->chassis_yaw_pid<500 && c->chassis_yaw_pid>-500){
//// c->chassis_yaw_pid = 0;
//// }
//// if(c->chassis_yaw_pid>500){
//// c->chassis_yaw_pid = map_fp32(c->chassis_yaw_pid,0.0,7000.0,4000.0,7000.0);
//// }
//// else if(c->chassis_yaw_pid<-500){
//// c->chassis_yaw_pid = map_fp32(c->chassis_yaw_pid,-7000.0,0.0,-7000.0,-4000.0);
//// }
//
// c->move_vec.Vw = c->chassis_yaw_pid;
// }
break;
}
//进行滤波
c->move_vec.Vw =LowPassFilter2p_Apply(&(c->filled[1]),c->move_vec.Vw);
c->move_vec.Vy =LowPassFilter2p_Apply(&(c->filled[2]),c->move_vec.Vy);
c->move_vec.Vx =LowPassFilter2p_Apply(&(c->filled[3]),c->move_vec.Vx);
Chassis_speed_calculate(c,pos);
for (uint8_t i = 0 ; i <4 ; i++){
c->hopemotorout.motor6020_target[i]=c->hopemotorout.rotor6020_jiesuan_2[i];
chassis6020_detangle[i] = PID_calc(&(c->pid.chassis_6020anglePid),c->motorfeedback.rotor_angle6020[i],
c->hopemotorout.motor6020_target[i]);
c->final_out.final_6020out[i] = PID_calc(&(c->pid.chassis_6020OmegaPid),c->motorfeedback.rotor_rpm6020[i],
chassis6020_detangle[i]);
out->chassis6020.as_array[i] = c->final_out.final_6020out[i];
c->hopemotorout.motor5065_target[i]=c->hopemotorout.rotor5065_jiesuan_2[i];
c->final_out.final_5065out[i]=c->hopemotorout.motor5065_target[i];
out->chassis5065.erpm[i] = c->final_out.final_5065out[i];
}
// c->vofa_send[0] =c->motorfeedback.rotor_angle6020[0];
// c->vofa_send[1] =c->hopemotorout.motor6020_target[0];
// c->vofa_send[2] =;
// c->vofa_send[3] =;
// c->vofa_send[4] =;
// c->vofa_send[5] =;
// c->vofa_send[6] =;
// c->vofa_send[7] =;
return CHASSIS_OK;
}
Reference in New Issue View Git Blame Copy Permalink