xm/arm
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

添加多轮底盘

Browse Source
This commit is contained in:
xxxxm 2026-03-20 04:08:29 +08:00
parent bf2633c3de
commit e0b108b4af
10 changed files with 746 additions and 620 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

716
User/module/chassis.c
View File

@ -1,314 +1,460 @@
/*
µ×ÅÌÄ£×é
底盘任务
*/
/*
舵轮底盘模组
example：
Chassis_t chassis;
Chassis_CMD_t cmd_chassis;
chassis_init(&chassis,&Config_GetRobotParam()->chassis,CHASSIS_FREQ);
osMessageQueueGet(task_runtime.msgq.imu.eulr, &chassis.pos088.imu_eulr, NULL, 0);
osMessageQueueGet(task_runtime.msgq.imu.gyro, &chassis.pos088.bmi088.gyro, NULL, 0);
这个是用来调防翻的，可以不用，而且需要c板放在地盘不能在云台上面
osMessageQueueGet(task_runtime.msgq.gimbal.yaw6020,&chassis.motorfeedback.gimbal_yaw_encoder,NULL,0);
if(osMessageQueueGet(task_runtime.msgq.cmd.chassis, &cmd_chassis, NULL, 0)==osOK)
{
Chassis_update(&chassis);
Chassis_Control(&chassis, &cmd_chassis,tick);
}else
{
Chassis_CMD_t safe_cmd = {.mode = STOP, .Vx = 0, .Vy = 0, .Vw = 0};
Chassis_Control(&chassis, &safe_cmd);
}
Chassis_Setoutput(&chassis);
*/
#include "cmsis_os2.h"
#include <stdlib.h>
#include "bsp/mm.h"
#include "bsp/can.h"
#include "component/ahrs.h"
/* Includes ----------------------------------------------------------------- */
#include "chassis.h"
#include "device/motor_rm.h"
#include "device/motor.h"
#include "module/chassis.h"
#include "component/PowerControl.h"
#include "config.h"
#include "bsp/time.h"
#include "bsp/can.h"
#include "math.h"
/**
* @brief µ×ÅÌСÍÓÂÝģʽÏà¹Ø²ÎÊý
*/
#define CHASSIS_ROTOR_WZ_MIN 0.6f //СÍÓÂÝ×îСËÙ¶È
#define CHASSIS_ROTOR_WZ_MAX 0.8f //СÍÓÂÝ×î´óËÙ¶È
#define M_7OVER72PI (M_2PI * 7.0f / 72.0f) //½Ç¶ÈÆ«ÒÆÁ¿£¨ÓÃÔÚ¸úËæÔÆ̨35¡ã£©
#define CHASSIS_ROTOR_OMEGA 0.001f //½ÇËٶȱ仯ƵÂÊ
#include "component/pid.h"
#include "component/filter.h"
#include "stdlib.h"
/*舵轮舵向校准方法：注释掉关于6020反馈角度的处理以及6020数据的发送这两处(define.h里有快捷方法)，
进debug将四个轮子编码器朝右（左右无所谓，可能会导致5065方向反，在解算里加个负号就行）
查看6020反馈值，将6020反馈值放入motor_offset中*/
/*chassis_t结构体中的内容现在有 move_vec (最终输出速度)
hopemotorout(期望的底盘输出值)舵轮解算出的各个电机的期望输出值 包括四个6020 和四个3508
final_out;(经PID计算后的实际发送给电机的实时输出值) 四个6020 四个3508
motorfeedback(电机反馈数据) 四个3508 四个6020
pid 各个电机的pid参数
ChassisImu_t pos088; 088的实时姿态
MotorOffset_t motoroffset; //6020校准数据
const Chassis_Param_t *param; //一些固定的参数
fp32 vofa_send[8]; //vofa输出数据
LowPassFilter2p_t filled[9]; //低通滤波器
float keep_angle[4]; // 保持的 6020 角度
*/
/* Private typedef ---------------------------------------------------------- */
#define CHASSIS_ROTOR_OMEGA 0.001
#define CHASSIS_ROTOR_WZ_MIN 0.6f /* 小陀螺旋转位移下界 */
#define CHASSIS_ROTOR_WZ_MAX 0.8f /* 小陀螺旋转位移上界 */
#define ONE_MINUTE 60.0f /* 一分钟时间 */
/**
* @brief ÉèÖõ×ÅÌģʽ
* @param c µ×Å̽ṹÌåÖ¸Õë
* @param mode Ä¿±ê¿ØÖÆģʽ
* @param now µ±Ç°Ê±¼ä´Á(ms)
* @return CHASSIS_OK:³É¹¦ CHASSIS_ERR_NULL:¿Õ
*/
static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode, uint32_t now) {
if (!c)
return CHASSIS_ERR_NULL;
if (mode == c->mode)
return CHASSIS_OK;
//Ëæ»úÖÖ×Ó£¬Ð¡ÍÓÂÝģʽËæ»úÉèÖÃÐýת·½Ïò
if (mode == CHASSIS_MODE_ROTOR && c->mode != CHASSIS_MODE_ROTOR) {
srand(now);
c->wz_multi = (rand() % 2) ? -1 : 1;
}
//ÖØÖÃPIDºÍÂ˲¨
for (uint8_t i = 0; i < c->num_wheel; i++) {
PID_Reset(&c->pid.motor[i]);
LowPassFilter2p_Reset(&c->filter.in[i], 0.0f);
LowPassFilter2p_Reset(&c->filter.out[i], 0.0f);
}
static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode ,uint32_t now)
{
if (c == NULL)
return CHASSIS_ERR_NULL; /* 主结构体不能为空 */
if (mode == c->mode) return CHASSIS_OK; /* 模式未改变直接返回 */
c->mode = mode;
return CHASSIS_OK;
if (mode == CHASSIS_MODE_ROTOR && c->mode != CHASSIS_MODE_ROTOR) {
srand(now);
c->wz_multi =1;
// c->wz_multi = (rand() % 2) ? -1 : 1;
}
for (int i = 0; i < 4; i++)
{
PID_Reset(&c->pid.Radder_DIR_omega[i]);
PID_Reset(&c->pid.Radder_DIR_angle[i]);
PID_Reset(&c->pid.Wheel_DIR_omega[i]);
PID_Reset(&c->pid.Radder_DIR_omega[i]);
}
c->mode = mode;
return CHASSIS_OK;
}
/**
* @brief СÍÓÂÝģʽ¯Ì¬½ÇËÙÈ
* @param min ×îСËÙÈ
* @param max ×î´óËÙÈ
* @param now µ±Ç°Ê±¼ä´Á(ms)
* @return ½ÇËÙÈÖµ
* @brief 产生小陀螺wz随机速度
*
* @param min wz产生最小速度
* @param max wz产生最大速度
* @param now ctrl_chassis的tick数
* @return float
*/
static float Chassis_CalcWz(const float min, const float max, uint32_t now) {
float wz_vary = fabsf(0.2f * sinf(CHASSIS_ROTOR_OMEGA * (float)now)) + min;
return (wz_vary > max) ? max : wz_vary;
/* wz在min和max之间，上限0.6f */
float wz_vary = fabs(0.2f * sinf(CHASSIS_ROTOR_OMEGA * (float)now)) + min;
return wz_vary > 0.8f ? max : wz_vary;
}
/**
* @brief µ×ÅÌģʽ³õʼ»¯
* @param c µ×Å̽ṹÌåÖ¸Õë
* @param param µ×Å̲ÎÊý½á¹¹ÌåÖ¸Õë
* @param mech_zero »úеÁãµãÅ·À­½Ç
* @param target_freq ¿ØÖÆƵÂÊ(Hz)
* @return CHASSIS_OK:³É¹¦ CHASSIS_ERR_NULL:¿Õ CHASSIS_ERR_TYPE:²»Ö§³ÖµÄģʽ
*/
int8_t Chassis_Init(Chassis_t *c, const Chassis_Params_t *param,
float target_freq) {
if (!c) return CHASSIS_ERR_NULL;
//³õʼ»¯CANͨÐÅ
BSP_CAN_Init();
c->param = param;
c->mode = CHASSIS_MODE_RELAX;
//¸ù¾Ýµ×Å̲»Í¬ÉèÖÃģʽÂÖ×ÓÓë»ìºÏÆ÷
Mixer_Mode_t mixer_mode;
switch (param->type) {
case CHASSIS_TYPE_MECANUM://ÂóÂÖ
c->num_wheel = 4;
mixer_mode = MIXER_MECANUM;
break;
case CHASSIS_TYPE_PARLFIX4:
c->num_wheel = 4;
mixer_mode = MIXER_PARLFIX4;
break;
case CHASSIS_TYPE_PARLFIX2:
c->num_wheel = 2;
mixer_mode = MIXER_PARLFIX2;
break;
case CHASSIS_TYPE_OMNI_CROSS:
c->num_wheel = 4;
mixer_mode = MIXER_OMNICROSS;
break;
case CHASSIS_TYPE_OMNI_PLUS: //È«ÏòÂÖ£¨Àϲ½±øÀàÐÍ£©
c->num_wheel = 4;
mixer_mode = MIXER_OMNIPLUS;
break;
case CHASSIS_TYPE_SINGLE:
c->num_wheel = 1;
mixer_mode = MIXER_SINGLE;
break;
default:
return CHASSIS_ERR_TYPE;
}
//³õʼ»¯Ê±¼ä´Á
c->last_wakeup = 0;
c->dt = 0.0f;
//³õʼ»¯PIDºÍÂ˲¨
for (uint8_t i = 0; i < c->num_wheel; i++) {
PID_Init(&c->pid.motor[i], KPID_MODE_NO_D, target_freq, &param->pid.motor_pid_param);
LowPassFilter2p_Init(&c->filter.in[i], target_freq, param->low_pass_cutoff_freq.in);
LowPassFilter2p_Init(&c->filter.out[i], target_freq, param->low_pass_cutoff_freq.out);
//ÇåÁãµç»ú·´À¡
c->feedback.motor[i].rotor_speed = 0;
c->feedback.motor[i].torque_current = 0;
c->feedback.motor[i].rotor_abs_angle = 0;
c->feedback.motor[i].temp = 0;
}
//³õʼ»¯PIDºÍ»ìºÏÆ÷
PID_Init(&c->pid.follow, KPID_MODE_CALC_D, target_freq, &param->pid.follow_pid_param);
Mixer_Init(&c->mixer, mixer_mode);
//ÇåÁãÔ˶¯ÏòÁ¿ºÍÊä³ö
c->move_vec.vx = c->move_vec.vy = c->move_vec.wz = 0.0f;
for (uint8_t i = 0; i < c->num_wheel; i++) {
c->out.motor[i] = 0.0f;
}
//×¢²á´ó½®µç»ú
for (int i = 0; i < c->num_wheel; i++) {
MOTOR_RM_Register(&(c->param->motor_param[i]));
}
return CHASSIS_OK;
}
/*底盘初始化*/
/**
* @brief ¸üеç»ú·´À¡(IMU+µç»ú״̬)
* @param c µ×Å̽ṹÌåÖ¸Õë
* @param feedback µ×ÅÌ·´À¡Ö¸Õë½á¹¹Ìå
* @return CHASSIS_OK:³É¹¦ CHASSIS_ERR_NULL:¿Õ
*/
int8_t Chassis_UpdateFeedback(Chassis_t *c) {
//¸üÐÂËùÓеç»ú·´À¡
for (uint8_t i = 0; i < c->num_wheel; i++) {
MOTOR_RM_Update(&(c->param->motor_param[i]));
MOTOR_RM_t *rm_motor = MOTOR_RM_GetMotor(&(c->param->motor_param[i]));
c->motors[i] = rm_motor;
MOTOR_RM_t *rm = c->motors[i];
if (rm_motor != NULL) {
c->feedback.motor[i] = rm_motor->feedback;
}else
{
return CHASSIS_ERR_NULL;
}
}
return CHASSIS_OK;
}
/**
* @brief µ×Å̵ç»ú¿ØÖÆ
* @param c µ×Å̽ṹÌåÖ¸Õë
* @param c_cmd ¿ØÖÆÃüÁî
* @param now µ±Ç°Ê±¼ä´Á(ms)
* @return CHASSIS_OK:³É¹¦ CHASSIS_ERR_NULL:¿Õ
*/
int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd, uint32_t now) {
if (!c || !c_cmd) return CHASSIS_ERR_NULL;
//¼ÆËã¿ØÖÆÖÜÆÚ
c->dt = (float)(now - c->last_wakeup) / 1000.0f;
c->last_wakeup = now;
if (!isfinite(c->dt) || c->dt <= 0.0f) {
c->dt = 0.001f;
}
if (c->dt < 0.0005f) c->dt = 0.0005f;
if (c->dt > 0.050f) c->dt = 0.050f;
//ÉèÖÃģʽ
Chassis_SetMode(c, c_cmd->mode, now);
//²»Í¬Ä£Ê½Ï¶ÔÓ¦½âË㣨Ô˶¯ÏòÁ¿£©
switch (c->mode) {
case CHASSIS_MODE_BREAK:
c->move_vec.vx = c->move_vec.vy = 0.0f;
break;
case CHASSIS_MODE_INDEPENDENT:
c->move_vec.vx = c_cmd->ctrl_vec.vx;
c->move_vec.vy = c_cmd->ctrl_vec.vy;
break;
default: { //Ò£¿ØÆ÷×ø±ê->»úÌå×ø±êϵ
float beta = c->feedback.encoder_gimbalYawMotor - c->mech_zero;
float cosb = cosf(beta);
float sinb = sinf(beta);
c->move_vec.vx = cosb * c_cmd->ctrl_vec.vx - sinb * c_cmd->ctrl_vec.vy;
c->move_vec.vy = sinb * c_cmd->ctrl_vec.vx + cosb * c_cmd->ctrl_vec.vy;
break;
}
}
//¸ù¾Ýģʽ¼ÆËãµ×Å̽ÇËÙ¶È
switch (c->mode) {
case CHASSIS_MODE_RELAX:
case CHASSIS_MODE_BREAK:
case CHASSIS_MODE_INDEPENDENT:
c->move_vec.wz = c_cmd->ctrl_vec.wz;
break;
case CHASSIS_MODE_OPEN:
c->move_vec.wz = c_cmd->ctrl_vec.wz;
break;
//ÔÆ̨¸úËæ
case CHASSIS_MODE_FOLLOW_GIMBAL:
c->move_vec.wz = PID_Calc(&c->pid.follow, c->mech_zero, c->feedback.encoder_gimbalYawMotor, 0.0f, c->dt);
break;
//ÔÆ̨¸úË棨ƫÒÆ£©
case CHASSIS_MODE_FOLLOW_GIMBAL_35:
c->move_vec.wz = PID_Calc(&c->pid.follow,c->mech_zero +M_7OVER72PI, c->feedback.encoder_gimbalYawMotor, 0.0f, c->dt);
break;
//СÍÓÂÝ
case CHASSIS_MODE_ROTOR:
c->move_vec.wz = c->wz_multi * Chassis_CalcWz(CHASSIS_ROTOR_WZ_MIN,CHASSIS_ROTOR_WZ_MAX, now);
break;
}
//Ô˶¯Ñ§Äæ½âË㣬Ô˶¯ÏòÁ¿·Ö½âΪµç»úתËÙ
Mixer_Apply(&c->mixer, &c->move_vec, c->setpoint.motor_rpm, c->num_wheel, 500.0f);
for (uint8_t i = 0; i < c->num_wheel; i++) {
float rf = c->setpoint.motor_rpm[i];///Ä¿±êתËÙ
float fb = LowPassFilter2p_Apply(&c->filter.in[i], (float)c->feedback.motor[i].rotor_speed);
float out_current;
switch (c->mode) {
case CHASSIS_MODE_BREAK:
case CHASSIS_MODE_FOLLOW_GIMBAL:
case CHASSIS_MODE_FOLLOW_GIMBAL_35:
case CHASSIS_MODE_ROTOR:
case CHASSIS_MODE_INDEPENDENT:
out_current = PID_Calc(&c->pid.motor[i], c->setpoint.motor_rpm[i], fb, 0.0f, c->dt);
break;
case CHASSIS_MODE_OPEN:
out_current = c->setpoint.motor_rpm[i] / 7000.0f;
break;
case CHASSIS_MODE_RELAX:
out_current = 0.0f;
break;
}
//µÍͨÂ˲¨ºÍÏÞ·ù
c->out.motor[i] = LowPassFilter2p_Apply(&c->filter.out[i], out_current);
Clip(&c->out.motor[i], -c->param->limit.max_current, c->param->limit.max_current);
}
return CHASSIS_OK;
}
void Chassis_Power_Control(Chassis_t *c,float max_power)
int8_t chassis_init(Chassis_t *c, Chassis_Param_t *param, float target_freq)
{
float* rpm=(float[4]){c->motors[0]->feedback.rotor_speed,c->motors[1]->feedback.rotor_speed,c->motors[2]->feedback.rotor_speed,c->motors[3]->feedback.rotor_speed};
power_model_t* param = (c->mode == CHASSIS_MODE_ROTOR) ? &cha2 : &cha;
power_calu(param,(float[4]){c->out.motor[0],c->out.motor[1],c->out.motor[2],c->out.motor[3]},rpm);
float scale = power_scale_calu(&param,1,max_power);
power_out_calu(param,scale,(float[4]){c->out.motor[0],c->out.motor[1],c->out.motor[2],c->out.motor[3]},rpm,c->out.motor);
if (c == NULL || param == NULL || target_freq <= 0.0f)
{
return CHASSIS_ERR; // 参数错误
}
c->param = param;
c->mode = CHASSIS_MODE_RELAX; // 默认模式为停止锁死底盘
c->mech_zero = c->param->mech_zero;/*云台6020的机械中点*/
c->travel = c->param->travel ;/*云台6020的机械行程*/
// c->mech_zero_4310 = c->param->mech_zero_4310;/*云台4310的机械中点*/
/*初始化can*/
BSP_CAN_Init();
/*注册轮向电机*/
for (int i = 0; i < 4; i++)
{
MOTOR_RM_Register(&(c->param->motor.Wheel_DIR[i]));
}
/*注册舵向电机*/
for (int i = 0; i < 4; i++)
{
MOTOR_RM_Register(&(c->param->motor.Radder_DIR[i]));
}
// 舵轮安装时的6020机械误差，机械校准时1号轮在左前方，所有轮的编码器朝向右面
MotorOffset_t motor_offset = {{5.24851561/M_PI * 180.0f, 1.00092244/ M_PI * 180.0f,
5.8199234/ M_PI * 180.0f, 4.74536991/ M_PI * 180.0f}}; // 右右右右
c->motoroffset = motor_offset;
/*对3508的速度环和6020的角速度以及位置环pid进行初始化*/
for (int i = 0; i < 4; i++)
{
PID_Init(&c->pid.Wheel_DIR_omega[i], KPID_MODE_NO_D, target_freq, &c->param->pid.Wheel_DIR_Omega);
}
for (int i = 0; i < 4; i++)
{
PID_Init(&c->pid.Radder_DIR_omega[i], KPID_MODE_CALC_D, target_freq, &c->param->pid.Radder_DIR_Omega);
PID_Init(&c->pid.Radder_DIR_angle[i], KPID_MODE_CALC_D, target_freq, &c->param->pid.Radder_DIR_Angle);
}
LowPassFilter2p_Init(&c->filled[0], target_freq, 20.0f); // vx
LowPassFilter2p_Init(&c->filled[1], target_freq, 20.0f); // vy
LowPassFilter2p_Init(&c->filled[2], target_freq, 20.0f); // vw
LowPassFilter2p_Init(&c->filled[3], target_freq, 20.0f); // 3508-1
LowPassFilter2p_Init(&c->filled[4], target_freq, 20.0f); // 3508-2
LowPassFilter2p_Init(&c->filled[5], target_freq, 20.0f); // 3508-3
LowPassFilter2p_Init(&c->filled[6], target_freq, 20.0f); // 3508-4
LowPassFilter2p_Init(&c->filled[7], target_freq, 20.0f); // 6020-1
LowPassFilter2p_Init(&c->filled[8], target_freq, 20.0f); // 6020-2
LowPassFilter2p_Init(&c->filled[9], target_freq, 20.0f); // 6020-3
LowPassFilter2p_Init(&c->filled[10], target_freq, 20.0f); // 6020-4
return CHASSIS_OK;
}
/**
* @brief µç»úÊä³ö
* @param c µ×Å̽ṹÌåÖ¸Õë
*/
void Chassis_Output(Chassis_t *c) {
if (!c)
return;
for (uint8_t i = 0; i < c->num_wheel; i++) {
MOTOR_RM_t *rm = c->motors[i];
if (!rm) continue;
MOTOR_RM_SetOutput(&rm->param, c->out.motor[i]);
// 底盘解算
void Chassis_speed_calculate(Chassis_t *c, Chassis_CMD_t *c_cmd)
{
// RC模式下松开遥控器防止6020回到默认位置导致侧翻
if (c->mode == RC && fabs(c->move_vec.Vx) < 0.1 && fabs(c->move_vec.Vy) < 0.1 && fabs(c->move_vec.Vw) < 0.1)
{
// 如果之前不处于保持模式，则记录当前角度
if (!c->keeping_angle_flag)
{
c->keeping_angle_flag = 1; // 进入保持模式
}
//µ÷ÓÃctrl
// for (uint8_t i = 0; i < c->num_wheel; i++) {
// MOTOR_RM_t *rm = c->motors[0];
// if (rm) {
// MOTOR_RM_Ctrl(&rm->param);
// }
// }
MOTOR_RM_t *rm = c->motors[0];
if (rm) {
MOTOR_RM_Ctrl(&rm->param);
// 使用保持的角度，而不是实时反馈值,速度为0
for (uint8_t i = 0; i < 4; i++)
{
c->hopemotorout.Radder_DIR_Solving_1[i] = c->keep_angle[i];
c->hopemotorout.Wheel_DIR_Solving_1[i] = 0;
}
// c->hopemotorout.Radder_DIR_Solving_1[0] = 315;
// c->hopemotorout.Radder_DIR_Solving_1[1] = 45;
// c->hopemotorout.Radder_DIR_Solving_1[2] = 315;
// c->hopemotorout.Radder_DIR_Solving_1[3] = 45;
}
else
{
// 如果有速度输入，则退出保持模式
c->keeping_angle_flag = 0;
// 让保持角度实时等于进入保持阈值前的最后一次角度值
for (uint8_t i = 0; i < 4; i++)
{
c->keep_angle[i] = c->hopemotorout.Radder_DIR_Solving_1[i];
}
switch (c->mode)
{
case RC:
case CHASSIS_MODE_ROTOR:
case CHASSIS_MODE_FOLLOW_GIMBAL:
// const double radians = atan(1.0f * 330 / 330);
c->hopemotorout.Wheel_DIR_Solving_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.Wheel_DIR_Solving_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.Wheel_DIR_Solving_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.Wheel_DIR_Solving_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)));
c->hopemotorout.Radder_DIR_Solving_1[0] = atan2((c->move_vec.Vy + c->move_vec.Vw * cos(radians)),
(c->move_vec.Vx + c->move_vec.Vw * sin(radians))) *
(180.0f / M_PI);
c->hopemotorout.Radder_DIR_Solving_1[1] = atan2((c->move_vec.Vy - c->move_vec.Vw * cos(radians)),
(c->move_vec.Vx + c->move_vec.Vw * sin(radians))) *
(180.0f / M_PI);
c->hopemotorout.Radder_DIR_Solving_1[2] = atan2((c->move_vec.Vy - c->move_vec.Vw * cos(radians)),
(c->move_vec.Vx - c->move_vec.Vw * sin(radians))) *
(180.0f / M_PI);
c->hopemotorout.Radder_DIR_Solving_1[3] = atan2((c->move_vec.Vy + c->move_vec.Vw * cos(radians)),
(c->move_vec.Vx - c->move_vec.Vw * sin(radians))) *
(180.0f / M_PI);
break;
case CHASSIS_MODE_BREAK:
case STOP:
case CHASSIS_MODE_RELAX:
for (int i = 0; i < 4; i++)
{
c->hopemotorout.Wheel_DIR_Solving_1[i] = 0.0f;
}
// c->hopemotorout.Radder_DIR_Solving_1[0] = 6.26554489/M_PI*180.0f;
// c->hopemotorout.Radder_DIR_Solving_1[1] = 2.1099906/M_PI*180.0f;
// c->hopemotorout.Radder_DIR_Solving_1[2] = 2.08391285/M_PI*180.0f;
// c->hopemotorout.Radder_DIR_Solving_1[3] = 5.26845694/M_PI*180.0f;
// c->hopemotorout.Radder_DIR_Solving_1[0] = 315;
// c->hopemotorout.Radder_DIR_Solving_1[1] = 45;
// c->hopemotorout.Radder_DIR_Solving_1[2] = 315;
// c->hopemotorout.Radder_DIR_Solving_1[3] = 45;
c->hopemotorout.Radder_DIR_Solving_1[0] = 0;
c->hopemotorout.Radder_DIR_Solving_1[1] = 0;
c->hopemotorout.Radder_DIR_Solving_1[2] = 0;
c->hopemotorout.Radder_DIR_Solving_1[3] = 0;
break;
}
}
// 角度归化到0°——360°
for (uint8_t i = 0; i < 4; i++)
{
if (c->hopemotorout.Radder_DIR_Solving_1[i] < 0)
{
c->hopemotorout.Radder_DIR_Solving_1[i] += 360;
}
}
for (uint8_t i = 0; i < 4; i++)
{
float angle_error[4]; // 角度误差
angle_error[i] = c->hopemotorout.Radder_DIR_Solving_1[i] - c->feedback.motor_transformation.Radder_DIR_Angle[i];
// 误差角度归化到-180°——+180°
while (angle_error[i] > 180)
angle_error[i] -= 360;
while (angle_error[i] < -180)
angle_error[i] += 360;
/*这里发现如果下面的c->motorfeedback.rotor_angle6020[i]+angle_error[i]变为
c->hopemotorout.Radder_DIR_Solving_1[i]会导致6020出现故障*/
if (angle_error[i] > 90 && angle_error[i] <= 180)
{
c->hopemotorout.Wheel_DIR_Solving_2[i] = c->hopemotorout.Wheel_DIR_Solving_1[i];
c->hopemotorout.Radder_DIR_Solving_2[i] = c->feedback.motor_transformation.Radder_DIR_Angle[i] + angle_error[i] - 180;
}
else if (angle_error[i] < -90 && angle_error[i] >= -180)
{
c->hopemotorout.Wheel_DIR_Solving_2[i] = c->hopemotorout.Wheel_DIR_Solving_1[i];
c->hopemotorout.Radder_DIR_Solving_2[i] = c->feedback.motor_transformation.Radder_DIR_Angle[i] + angle_error[i] + 180;
}
else
{
c->hopemotorout.Wheel_DIR_Solving_2[i] = -c->hopemotorout.Wheel_DIR_Solving_1[i];
c->hopemotorout.Radder_DIR_Solving_2[i] = c->feedback.motor_transformation.Radder_DIR_Angle[i] + angle_error[i];
}
}
}
/**
* @brief ÖØÖõ×ÅÌÊä³ö
* @param c µ×Å̽ṹÌåÖ¸Õë
*/
void Chassis_ResetOutput(Chassis_t *c) {
if (!c) return;
for (uint8_t i = 0; i < c->num_wheel; i++) {
MOTOR_RM_t *m = c->motors[i];
if (m) {
MOTOR_RM_Relax(&(m->param));
}
}
/*
更新底盘电机反馈，并且进行归一化处理，但是这里的归一化是我自己测的电机最大转速
近似于一进行处理，并不是准确的-1到1
*/
int8_t Chassis_update(Chassis_t *c)
{
if (c == NULL)
{
return CHASSIS_ERR_NULL; // 参数错误
}
MOTOR_RM_t *Wheel_DIR_MOTOR[4];
MOTOR_RM_t *Radder_DIR_MOTOR[4];
/*更新电机反馈*/
for (int i = 0; i < 4; i++)
{
/* 轮向电机更新 */
MOTOR_RM_Update(&(c->param->motor.Wheel_DIR[i]));
Wheel_DIR_MOTOR[i]= MOTOR_RM_GetMotor(&(c->param->motor.Wheel_DIR[i]));
c->feedback.motor.Wheel_DIR[i] = Wheel_DIR_MOTOR[i]->feedback;
/* 舵向电机 */
MOTOR_RM_Update(&(c->param->motor.Radder_DIR[i]));
Radder_DIR_MOTOR[i]= MOTOR_RM_GetMotor(&(c->param->motor.Radder_DIR[i]));
c->feedback.motor.Radder_DIR[i] = Radder_DIR_MOTOR[i]->feedback;
/* 单位转换 */
c->feedback.motor_transformation.Radder_DIR_Angle[i]=c->feedback.motor.Radder_DIR[i].rotor_abs_angle/ M_PI * 180.0f;
c->feedback.motor_transformation.Radder_DIR_Omega[i]= c->feedback.motor.Radder_DIR[i].rotor_speed/320;
c->feedback.motor_transformation.Wheel_DIR_Omega[i]= c->feedback.motor.Wheel_DIR[i].rotor_speed/10000;
c->feedback.motor_transformation.Radder_DIR_Angle[i] = fmod(c->feedback.motor_transformation.Radder_DIR_Angle[i] - c->motoroffset.MOTOR_OFFSET[i], 360.0);
if (c->feedback.motor_transformation.Radder_DIR_Angle[i] < 0)
{
c->feedback.motor_transformation.Radder_DIR_Angle[i] += 360;
}
}
return CHASSIS_OK;
}
// 防止侧翻
/*具体开始上车实验之后在进行编写修改，先暂时不使用*/
void ChassisrolPrevent(Chassis_t *c)
{
}
int8_t Chassis_Control(Chassis_t *c, Chassis_CMD_t *c_cmd,uint32_t now)
{
if (c == NULL || c_cmd == NULL)
{
return CHASSIS_ERR_NULL; // 参数错误
}
// c->dt = (BSP_TIME_Get_us() - c->last_wakeup) / 1000000.0f; /* 计算两次调用的时间间隔，单位秒 */
// c->last_wakeup = BSP_TIME_Get_us();
c->dt = (float)(now - c->last_wakeup) / 1000.0f; /* 计算两次调用的时间间隔，单位秒 */
c->last_wakeup = now;
/*设置底盘模式*/
if (Chassis_SetMode(c, c_cmd->mode,now) != CHASSIS_OK)
{
return CHASSIS_ERR_MODE; /* 设置模式失败 */
}
float beta;
/*根据底盘模式进行不同的控制*/
switch (c->mode)
{
case CHASSIS_MODE_BREAK:
case CHASSIS_MODE_RELAX:
case STOP:
// 停止模式
c->move_vec.Vx = 0.0f;
c->move_vec.Vy = 0.0f;
c->move_vec.Vw = 0.0f;
break;
case RC:
// 遥控模式
// c->move_vec.Vx = c_cmd->Vx * c_cmd->throttle * 1000.0f;
// c->move_vec.Vy = c_cmd->Vy * c_cmd->throttle * 1000.0f;
c->move_vec.Vx = c_cmd->Vx * c_cmd->throttle;
c->move_vec.Vy = c_cmd->Vy * c_cmd->throttle;
c->move_vec.Vw = c_cmd->Vw * c_cmd->throttle;
c->move_vec.mul = c_cmd->throttle;
break;
case CHASSIS_MODE_ROTOR:
// 小陀螺模式
beta = (c->feedback.motor.gimbal_yaw.rotor_abs_angle / 180.0f * M_PI) - c->mech_zero; // 云台当前角度转弧度
float cos_beta = cosf(beta);
float sin_beta = sinf(beta);
c->move_vec.Vx =cos_beta * c_cmd->x_l - sin_beta * c_cmd->y_l;
c->move_vec.Vy =sin_beta* c_cmd->x_l + cos_beta * c_cmd->y_l;
c->move_vec.Vw =c->wz_multi* Chassis_CalcWz(CHASSIS_ROTOR_WZ_MIN,CHASSIS_ROTOR_WZ_MAX, now);
break;
case CHASSIS_MODE_FOLLOW_GIMBAL:
// 跟随云台模式
c->move_vec.Vx =-c_cmd->y_l;
c->move_vec.Vy =-c_cmd->x_l;
c->move_vec.Vw = PID_Calc(&c->pid.chassis_follow_gimbal_pid, 2.06f ,c->feedback.motor.gimbal_yaw.rotor_abs_angle, 0.0f, c->dt);
// c->move_vec.Vw = -PID_Calc(&c->pid.chassis_follow_gimbal_pid, c->motorfeedback.gimbal_yaw_encoder-2.07694483f ,c->motorfeedback.gimbal_yaw_encoder, 0.0f, c->dt);
break;
default:
return CHASSIS_ERR_MODE; /* 未知模式 */
}
/*给输出的Vx，Vy，Vw进行滤波*/
c->move_vec.Vx = LowPassFilter2p_Apply(&c->filled[0], c->move_vec.Vx);
c->move_vec.Vy = LowPassFilter2p_Apply(&c->filled[1], c->move_vec.Vy);
c->move_vec.Vw = LowPassFilter2p_Apply(&c->filled[2], c->move_vec.Vw);
Chassis_speed_calculate(c, c_cmd);
for (int i = 0; i < 4; i++)
{
float chassis6020_detangle[4]; // 6020解算出的角度
c->hopemotorout.Radder_DIR_target[i] = c->hopemotorout.Radder_DIR_Solving_2[i];
chassis6020_detangle[i] = PID_Calc(&(c->pid.Radder_DIR_angle[i]), c->hopemotorout.Radder_DIR_target[i],
c->feedback.motor_transformation.Radder_DIR_Angle[i], 0.0f, c->dt);
// c->final_out.final_6020out[i] = chassis6020_detangle[i] ; //单环控制就用这个
c->chassis6020_detangle[i]=chassis6020_detangle[i];
c->final_out.final_Radder_DIR[i] = PID_Calc(&(c->pid.Radder_DIR_omega[i]), chassis6020_detangle[i],
c->feedback.motor_transformation.Radder_DIR_Omega[i], 0.0f, c->dt);
c->out.Radder_DIR[i] = LowPassFilter2p_Apply(&c->filled[7+i], c->final_out.final_Radder_DIR[i]);
c->hopemotorout.Wheel_DIR_target[i] = c->hopemotorout.Wheel_DIR_Solving_2[i];
c->final_out.final_Wheel_DIR[i] = PID_Calc(&(c->pid.Wheel_DIR_omega[i]), c->hopemotorout.Wheel_DIR_target[i],
c->feedback.motor_transformation.Wheel_DIR_Omega[i], 0.0f, c->dt);
c->out.Wheel_DIR[i] = LowPassFilter2p_Apply(&c->filled[3+i], c->final_out.final_Wheel_DIR[i]);
}
return CHASSIS_OK;
}
/*电机输出设定和发送*/
void Chassis_Setoutput(Chassis_t *c)
{
for (uint8_t i = 0; i < 4; i++) {
MOTOR_RM_SetOutput(&(c->param->motor.Wheel_DIR[i]), c->out.Wheel_DIR[i]);
MOTOR_RM_SetOutput(&(c->param->motor.Radder_DIR[i]), c->out.Radder_DIR[i]);
}
MOTOR_RM_Ctrl(&(c->param->motor.Wheel_DIR[0]));
MOTOR_RM_Ctrl(&(c->param->motor.Radder_DIR[0]));
}

431
User/module/chassis.h
View File

@ -1,239 +1,240 @@
/*
µ×ÅÌÄ£×é
*/
#pragma once
#ifdef __cplusplus
extern "C" {
extern "C"
{
#endif
/* Includes ----------------------------------------------------------------- */
#include <cmsis_os2.h>
#include "bsp/can.h"
#include "struct_typedef.h"
#include "component/filter.h"
#include "component/mixer.h"
#include "component/pid.h"
#include "component/ahrs.h"
//#include "device/buzzer.h"
#include "component/user_math.h"
#include "component/filter.h"
#include "device/motor_rm.h"
/* Exported constants ------------------------------------------------------- */
#define CHASSIS_OK (0) /* ÔËÐÐÕý³£ */
#define CHASSIS_ERR (-1) /* ÔËÐÐʱ³öÏÖÁËһЩС´íÎó */
#define CHASSIS_ERR_NULL (-2) /* ÔËÐÐʱ·¢ÏÖNULLÖ¸Õë */
#define CHASSIS_ERR_MODE (-3) /* ÔËÐÐʱ³öÅäÖÃÁË´íÎóµÄChassisMode_t */
#define CHASSIS_ERR_TYPE (-4) /* ÔËÐÐʱ³öÅäÖÃÁË´íÎóµÄChassis_Type_t */
#define MAX_MOTOR_CURRENT 20.0f
/* µ×ÅÌ¿ØÖÆģʽ */
typedef enum {
CHASSIS_MODE_RELAX, /* ·ÅËÉģʽ,µç»ú²»Êä³ö¡£Ò»°ãÇé¿öµ×Å̳õʼ»¯Ö®ºóµÄģʽ */
CHASSIS_MODE_BREAK, /* ɲ³µÄ£Ê½£¬µç»ú±Õ»·¿ØÖƾ²Ö¹¡£ÓÃÓÚ»úÆ÷ÈËֹͣ״̬ */
CHASSIS_MODE_FOLLOW_GIMBAL, /* ͨ¹ý±Õ»·¿ØÖÆʹ³µÍ··½Ïò¸úËæÔÆ̨ */
CHASSIS_MODE_FOLLOW_GIMBAL_35, /* ͨ¹ý±Õ»·¿ØÖÆʹ³µÍ··½Ïò35¡ã¸úËæÔÆ̨ */
CHASSIS_MODE_ROTOR, /* СÍÓÂÝģʽ£¬Í¨¹ý±Õ»·¿ØÖÆʹµ×Å̲»Í£Ðýת */
CHASSIS_MODE_INDEPENDENT, /*¶ÀÁ¢Ä£Ê½¡£µ×ÅÌÔËÐв»ÊÜÔÆ̨ӰÏì */
CHASSIS_MODE_OPEN, /* ¿ª»·Ä£Ê½¡£µ×ÅÌÔËÐв»ÊÜPID¿ØÖÆ£¬Ö±½ÓÊä³öµ½µç»ú */
} Chassis_Mode_t;
/* СÍÓÂÝת¶¯Ä£Ê½ */
typedef enum {
ROTOR_MODE_CW, /* ˳ʱÕëת¶¯ */
ROTOR_MODE_CCW, /* ÄæʱÕëת¶¯ */
ROTOR_MODE_RAND, /* Ëæ»úת¶¯ */
} Chassis_RotorMode_t;
/* µ×ÅÌ¿ØÖÆÃüÁî */
typedef struct {
Chassis_Mode_t mode; /* µ×ÅÌÔËÐÐģʽ */
Chassis_RotorMode_t mode_rotor; /* СÍÓÂÝת¶¯Ä£Ê½ */
MoveVector_t ctrl_vec; /* µ×ÅÌ¿ØÖÆÏòÁ¿ */
float max_velocity;
float acceleration;
float deceleration;
} TrapezoidalProfile;
#define CHASSIS_OK (0)
#define CHASSIS_ERR (-1)
#define CHASSIS_ERR_NULL (-2)
#define CHASSIS_ERR_MODE (-3) /*CMD_ChassisMode_t */
#define CHASSIS_ERR_TYPE (-4) /*Chassis_Type_t */
// 纵向/横向
#define radians atan(1.0f * 390 /390) // 角度制
/*底盘姿态模式*/
typedef enum
{
STOP_MODE,/* 急停模式 */
EXPAND_MODE,/* 展开模式 */
REDUCE_MODE,/* 收缩模式 */
} CHASSIS_ATTITUDE_MODE_t;
/*底盘状态*/
typedef enum
{
STOP_STATE,/* 急停状态 */
EXPAND_STATE,/* 展开状态 */
REDUCE_STATE,/* 收缩状态 */
} CHASSIS_STATE_t;
/*底盘模式*/
typedef enum
{
STOP, // 底盘平行
RC, // 遥控模式
CHASSIS_MODE_BREAK, /* 刹车模式，电机闭环控制保持静止。用于机器人停止状态 */
CHASSIS_MODE_RELAX, /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */
CHASSIS_MODE_FOLLOW_GIMBAL, /* 通过闭环控制使车头方向跟随云台 */
CHASSIS_MODE_ROTOR, /* 小陀螺模式，通过闭环控制使底盘不停旋转 */
} Chassis_Mode_t;
typedef struct
{
int cmd_power_on_safe; // 上电安全标志
Chassis_Mode_t mode;
CHASSIS_ATTITUDE_MODE_t attitude_mode;
// 遥控器输出值
fp32 Vx;
fp32 Vy;
fp32 Vw;
fp32 throttle;
fp32 x_l;
fp32 y_l;
} Chassis_CMD_t;
/* ÏÞλ */
typedef struct {
float max;
float min;
} Chassis_Limit_t;
/* µ×ÅÌÀàÐÍ£¨µ×Å̵ÄÎïÀíÉè¼Æ£© */
typedef enum {
CHASSIS_TYPE_MECANUM, /* Âó¿ËÄÉÄ·ÂÖ */
CHASSIS_TYPE_PARLFIX4, /* ƽÐаڷŵÄËĸöÇý¶¯ÂÖ */
CHASSIS_TYPE_PARLFIX2, /* ƽÐаڷŵÄÁ½¸öÇý¶¯ÂÖ */
CHASSIS_TYPE_OMNI_CROSS, /* ²æÐͰڷŵÄËĸöÈ«ÏòÂÖ */
CHASSIS_TYPE_OMNI_PLUS, /* Ê®×ÖÐÍ°ÚÉèµÄËĸöÈ«ÏòÂÖ */
CHASSIS_TYPE_DRONE, /* ÎÞÈË»úµ×ÅÌ */
CHASSIS_TYPE_SINGLE, /* µ¥¸öĦ²ÁÂÖ */
} Chassis_Type_t;
/* µ×Å̲ÎÊý½á¹¹Ìå,ALL³õʼ»¯²ÎÊý */
typedef struct {
MOTOR_RM_Param_t motor_param[4];
struct {
KPID_Params_t motor_pid_param; /* µ×Å̵ç»úPID²ÎÊý */
KPID_Params_t follow_pid_param; /* ¸úËæÔÆ̨PID²ÎÊý */
} pid;
Chassis_Type_t type; /* µ×ÅÌÀàÐÍ£¬µ×Å̵ĻúеÉè¼ÆºÍÂÖ×ÓÑ¡ÐÍ */
/* µÍͨÂ˲¨Æ÷½ØÖÁƵÂÊ*/
struct {
float in; /* ÊäÈë */
float out; /* Êä³ö */
} low_pass_cutoff_freq;
/* µç»ú·´×°£¬Ó¦¸ÃºÍÔÆ̨ÉèÖÃÏàͬ*/
struct {
bool yaw;
} reverse;
struct {
float max_vx, max_vy, max_wz;
float max_current;
} limit;
} Chassis_Params_t;
typedef struct {
AHRS_Gyro_t gyro;
AHRS_Eulr_t eulr;
} Chassis_IMU_t;
typedef struct {
MOTOR_Feedback_t motor[4]; // Ëĸö 3508µç»ú ·´À¡
float encoder_gimbalYawMotor;
} Chassis_Feedback_t;
/* µ×ÅÌÊä³ö½á¹¹Ìå*/
typedef struct {
float motor[4];
} Chassis_Output_t;
/*
* ÔËÐеÄÖ÷½á¹¹Ìå£þ
* °üº¬³õʼ»¯²ÎÊý,Öмä±äÁ¿,Êä³ö±äÁ¿
*/
typedef struct {
uint64_t last_wakeup;
float dt;
Chassis_Params_t *param; /* µ×Å̲ÎÊý,ÓÃChassis_InitÉ趨 */
/* Ä£¿éͨÓà */
Chassis_Mode_t mode; /* µ×ÅÌģʽ */
/* µ×ÅÌÉè¼Æ */
int8_t num_wheel; /* µ×ÅÌÂÖ×ÓÊýÁ¿ */
Mixer_t mixer; /* »ìºÏÆ÷,Òƶ¯ÏòÁ¿->µç»úÄ¿±êÖµ */
MoveVector_t move_vec; /* µ×ÅÌʵ¼ÊµÄÔ˶¯ÏòÁ¿ */
MOTOR_RM_t *motors[4];/*Ö¸Ïòµ×ÅÌÿ¸öµç»ú²ÎÊý*/
float mech_zero;
float wz_multi; /* СÍÓÂÝÐýתģʽ */
/* PID¼ÆËãÄ¿±êÖµ */
struct {
float motor_rpm[4]; /* µç»úתËٵĶ¯Ì¬Êý×é,µ¥Î»:RPM */
} setpoint;
/* ·´À¡¿ØÖÆÓõÄPID */
struct {
KPID_t motor[4]; /* ¿ØÖÆÂÖ×Óµç»úÓõÄPIDµÄ¶¯Ì¬Êý×é */
KPID_t follow; /* ¸úËæÔÆ̨ÓõÄPID */
} pid;
struct {
Chassis_Limit_t vx, vy, wz;
} limit;
// 四个舵轮的安装误差
typedef struct
{
fp32 MOTOR_OFFSET[4];
} MotorOffset_t;
/* Â˲¨Æ÷ */
struct {
LowPassFilter2p_t in[4]; /* ·´À¡ÖµÂ˲¨Æ÷ */
LowPassFilter2p_t out[4]; /* Êä³öÖµÂ˲¨Æ÷ */
} filter;
typedef struct
{
fp32 TELESCOPE_MOTOR_OFFSET[2];
} TelescopeMotorOffset_t;
typedef struct
{
/*可通过该枚举类型来决定Imu的数据量纲*/
enum
{
IMU_DEGREE, // 角度值（0-360）
IMU_RADIAN // 弧度制（0-2pi)
} angle_mode;
Chassis_Output_t out; /* µç»úÊä³ö */
Chassis_Feedback_t feedback;
//float out_motor[4];
} Chassis_t;
// AHRS_Eulr_t imu_eulr; // 解算后存放欧拉角（弧度制）
} ChassisImu_t;
/* Exported functions prototypes -------------------------------------------- */
/**
* \brief µ×Å̳õʼ»¯
*
* \param c °üº¬µ×ÅÌÊý¾ÝµÄ½á¹¹Ìå
* \param param °üº¬µ×Å̲ÎÊýµÄ½á¹¹ÌåÖ¸Õë
* \param target_freq ÈÎÎñÔ¤ÆÚµÄÔËÐÐƵÂÊ
*
* \return ÔËÐнá¹û
*/
int8_t Chassis_Init(Chassis_t *c, const Chassis_Params_t *param,
float target_freq);
/**
* \brief ¸üе×ÅÌ·´À¡ÐÅÏ¢
*
* \param c °üº¬µ×ÅÌÊý¾ÝµÄ½á¹¹Ìå
* \param can CANÉ豸½á¹¹Ìå
*
* \return ÔËÐнá¹û
*/
int8_t Chassis_UpdateFeedback(Chassis_t *c);
/**
* \brief ÔËÐе×ÅÌ¿ØÖÆÂß¼­
*
* \param c °üº¬µ×ÅÌÊý¾ÝµÄ½á¹¹Ìå
* \param c_cmd µ×ÅÌ¿ØÖÆÖ¸Áî
* \param dt_sec Á½´Îµ÷ÓõÄʱ¼ä¼ä¸ô
*
* \return ÔËÐнá¹û
*/
int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd,
uint32_t now);
/*底盘参数结构体*/
typedef struct
{
/**
* \brief ¸´ÖƵ×ÅÌÊä³öÖµ
*
* \param s °üº¬µ×ÅÌÊý¾ÝµÄ½á¹¹Ìå
* \param out CANÉ豸µ×ÅÌÊä³ö½á¹¹Ìå
*/
void Chassis_Output(Chassis_t *c);
struct{
/*该部分决定PID的参数整定在config中修改*/
KPID_Params_t Radder_DIR_Omega;
KPID_Params_t Radder_DIR_Angle;
KPID_Params_t Wheel_DIR_Omega;
KPID_Params_t chassis_follow_gimbal;
}pid;
struct{
MOTOR_RM_Param_t Wheel_DIR[4]; // 四个轮向电机
MOTOR_RM_Param_t Radder_DIR[4]; // 四个舵向电机
MOTOR_RM_Param_t chassis_follow_gimbal; // 底盘跟随云台
/**
* \brief Çå¿ÕChassisÊä³öÊý¾Ý
*
* \param out CANÉ豸µ×ÅÌÊä³ö½á¹¹Ìå
*/
void Chassis_ResetOutput(Chassis_t *c);
void Chassis_Power_Control(Chassis_t *c,float max_power);
/**
* @brief µ×Å̹¦ÂÊÏÞÖÆ
*
* @param c µ×ÅÌÊý¾Ý
* @param cap µçÈÝÊý¾Ý
* @param ref ²ÃÅÐϵͳÊý¾Ý
* @return º¯ÊýÔËÐнá¹û
*/
//»¹Ã»ÓмÓÈ룬waiting¡£¡£¡£¡£¡£¡£int8_t Chassis_PowerLimit(Chassis_t *c, const CAN_Capacitor_t *cap,
// const Referee_ForChassis_t *ref);
/**
* @brief µ¼³öµ×ÅÌÊý¾Ý
*
* @param chassis µ×ÅÌÊý¾Ý½á¹¹Ìå
* @param ui UIÊý¾Ý½á¹¹Ìå
*/
//void Chassis_DumpUI(const Chassis_t *c, Referee_ChassisUI_t *ui);
}motor;
float mech_zero;/*云台6020的机械中点*/
float travel ;/*云台6020的机械行程*/
float mech_zero_4310;/*云台4310的机械中点*/
} Chassis_Param_t;
typedef struct
{
fp32 Vx;
fp32 Vy;
fp32 Vw;
fp32 mul; // 油门倍率
} ChassisMove_Vec;
typedef struct
{
float Wheel_DIR[4];
float Radder_DIR[4];
} Chassis_out_t;
typedef struct
{
uint32_t last_wakeup;
float dt;
float chassis6020_detangle[4];
Chassis_Mode_t mode;
ChassisMove_Vec move_vec; // 最终输入速度
TrapezoidalProfile telescope_profile;
/* 设立角度和跟随角度 */
float Set_TelescopeAngle;
float Last_Set_TelescopeAngle;
float Follow_TelescopeAngle;
float feedfrward;
float Telescope_err;
/*期望的底盘输出值(此处为舵轮解算出的各个电机的期望输出值)ֵ*/
struct{
uint32_t last_wakeup;
float dt;
}accl_time;
struct
{
fp32 Wheel_DIR_Solving_1[4];
fp32 Wheel_DIR_Solving_2[4];
fp32 Radder_DIR_Solving_1[4];
fp32 Radder_DIR_Solving_2[4];
// fp32 rotor6020_elur_yaw;
fp32 Radder_DIR_target[4];
fp32 Wheel_DIR_target[4];
} hopemotorout;
struct
{
fp32 final_Telescope;
fp32 final_Radder_DIR[4];
fp32 final_Wheel_DIR[4];
} final_out;
/* 原始数据 */
struct
{
struct{
MOTOR_Feedback_t Wheel_DIR[4]; // 四个轮向电机
MOTOR_Feedback_t Radder_DIR[4];
MOTOR_Feedback_t gimbal_yaw;
}motor;
/* 转化数据 */
struct{
float Radder_DIR_Angle[4];
float Radder_DIR_Omega[4];
float Wheel_DIR_Angle[4];
float Wheel_DIR_Omega[4];
}motor_transformation;
} feedback;
struct
{
KPID_t Radder_DIR_angle[4];
KPID_t Radder_DIR_omega[4];
KPID_t Wheel_DIR_omega[4];
KPID_t chassis_follow_gimbal_pid;
} pid;
uint8_t keeping_angle_flag; // 是否处于保持角度模式
// AHRS_Eulr_t set_point; // 底盘纠正目标角
// fp32 angle_current; // 当前角度
// fp32 angle_piancha; // 偏差角度
// fp32 yaw_out; // 角度pid输出值
ChassisImu_t pos088; // 088的实时姿态
MotorOffset_t motoroffset; // 5065校准数据
TelescopeMotorOffset_t telescope_motor_offset;
Chassis_Param_t *param; // 一些固定的参数
LowPassFilter2p_t filled[11]; // 低通滤波器
LowPassFilter2p_t accl_filled[2]; // 加速度滤波器
float keep_angle[4]; // 保持的 舵向 角度
Chassis_out_t out;
float wz_multi; /* 小陀螺模式旋转方向 */
float mech_zero;/*云台6020的机械中点*/
float travel ;/*云台6020的机械行程*/
// float mech_zero_4310;/*云台4310的机械中点*/
} Chassis_t;
int8_t chassis_init(Chassis_t *c, Chassis_Param_t *param, float target_freq);
int8_t Chassis_update(Chassis_t *c);
int8_t Chassis_Control(Chassis_t *c, Chassis_CMD_t *c_cmd,uint32_t now);
void Chassis_Setoutput(Chassis_t *c);
#ifdef __cplusplus
}
#endif

40
User/module/cmd/cmd.c
View File

@ -16,15 +16,16 @@ static void CMD_RC_BuildChassisCmd(CMD_t *ctx) {
CMD_RCModeMap_t *map = &ctx->config->rc_mode_map;
/* 根据左拨杆位置选择模式 */
switch (ctx->input.rc.sw[0]) {
switch (ctx->input.rc.sw[1]) {
case CMD_SW_UP:
ctx->output.chassis.cmd.mode = map->sw_left_up;
break;
case CMD_SW_MID:
ctx->output.chassis.cmd.throttle = 1;
ctx->output.chassis.cmd.mode = map->sw_left_mid;
break;
case CMD_SW_DOWN:
ctx->output.chassis.cmd.mode = map->sw_left_down;
break;
default:
ctx->output.chassis.cmd.mode = CHASSIS_MODE_RELAX;
@ -32,9 +33,9 @@ static void CMD_RC_BuildChassisCmd(CMD_t *ctx) {
}
/* 摇杆控制移动 */
ctx->output.chassis.cmd.ctrl_vec.vx = ctx->input.rc.joy_right.x;
ctx->output.chassis.cmd.ctrl_vec.vy = ctx->input.rc.joy_right.y;
ctx->output.chassis.cmd.ctrl_vec.wz = ctx->input.rc.joy_left.x;
ctx->output.chassis.cmd.Vx =- ctx->input.rc.joy_right.y;
ctx->output.chassis.cmd.Vy = -ctx->input.rc.joy_right.x;
}
#endif /* CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_CHASSIS */
@ -76,19 +77,19 @@ static void CMD_RC_BuildShootCmd(CMD_t *ctx) {
/* 根据右拨杆控制射击 */
switch (ctx->input.rc.sw[1]) {
case CMD_SW_DOWN:
ctx->output.shoot.cmd.ready = true;
ctx->output.shoot.cmd.firecmd = true;
break;
case CMD_SW_MID:
ctx->output.shoot.cmd.ready = true;
ctx->output.shoot.cmd.firecmd = false;
break;
case CMD_SW_UP:
default:
ctx->output.shoot.cmd.ready = false;
ctx->output.shoot.cmd.firecmd = false;
break;
// case CMD_SW_DOWN:
// ctx->output.shoot.cmd.ready = true;
// ctx->output.shoot.cmd.firecmd = true;
// break;
// case CMD_SW_MID:
// ctx->output.shoot.cmd.ready = true;
// ctx->output.shoot.cmd.firecmd = false;
// break;
// case CMD_SW_UP:
// default:
// ctx->output.shoot.cmd.ready = false;
// ctx->output.shoot.cmd.firecmd = false;
// break;
}
}
#endif /* CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_SHOOT */
@ -140,8 +141,7 @@ static void CMD_PC_BuildChassisCmd(CMD_t *ctx) {
ctx->output.chassis.cmd.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
/* WASD控制移动 */
ctx->output.chassis.cmd.ctrl_vec.vx = 0.0f;
ctx->output.chassis.cmd.ctrl_vec.vy = 0.0f;
CMD_Behavior_ProcessAll(ctx, &ctx->input, &ctx->last_input, CMD_MODULE_CHASSIS);
}
#endif /* CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_CHASSIS */

18
User/module/cmd/cmd_behavior.c
View File

@ -15,44 +15,44 @@
/* 行为处理函数实现 */
int8_t CMD_Behavior_Handle_FORE(CMD_t *ctx) {
#if CMD_ENABLE_MODULE_CHASSIS
ctx->output.chassis.cmd.ctrl_vec.vy += ctx->config->sensitivity.move_sens;
ctx->output.chassis.cmd.Vy += ctx->config->sensitivity.move_sens;
#endif
return CMD_OK;
}
int8_t CMD_Behavior_Handle_BACK(CMD_t *ctx) {
#if CMD_ENABLE_MODULE_CHASSIS
ctx->output.chassis.cmd.ctrl_vec.vy -= ctx->config->sensitivity.move_sens;
ctx->output.chassis.cmd.Vy -= ctx->config->sensitivity.move_sens;
#endif
return CMD_OK;
}
int8_t CMD_Behavior_Handle_LEFT(CMD_t *ctx) {
#if CMD_ENABLE_MODULE_CHASSIS
ctx->output.chassis.cmd.ctrl_vec.vx -= ctx->config->sensitivity.move_sens;
ctx->output.chassis.cmd.Vx -= ctx->config->sensitivity.move_sens;
#endif
return CMD_OK;
}
int8_t CMD_Behavior_Handle_RIGHT(CMD_t *ctx) {
#if CMD_ENABLE_MODULE_CHASSIS
ctx->output.chassis.cmd.ctrl_vec.vx += ctx->config->sensitivity.move_sens;
ctx->output.chassis.cmd.Vx += ctx->config->sensitivity.move_sens;
#endif
return CMD_OK;
}
int8_t CMD_Behavior_Handle_ACCELERATE(CMD_t *ctx) {
#if CMD_ENABLE_MODULE_CHASSIS
ctx->output.chassis.cmd.ctrl_vec.vx *= ctx->config->sensitivity.move_fast_mult;
ctx->output.chassis.cmd.ctrl_vec.vy *= ctx->config->sensitivity.move_fast_mult;
ctx->output.chassis.cmd.Vx *= ctx->config->sensitivity.move_fast_mult;
ctx->output.chassis.cmd.Vy *= ctx->config->sensitivity.move_fast_mult;
#endif
return CMD_OK;
}
int8_t CMD_Behavior_Handle_DECELERATE(CMD_t *ctx) {
#if CMD_ENABLE_MODULE_CHASSIS
ctx->output.chassis.cmd.ctrl_vec.vx *= ctx->config->sensitivity.move_slow_mult;
ctx->output.chassis.cmd.ctrl_vec.vy *= ctx->config->sensitivity.move_slow_mult;
ctx->output.chassis.cmd.Vx *= ctx->config->sensitivity.move_slow_mult;
ctx->output.chassis.cmd.Vy *= ctx->config->sensitivity.move_slow_mult;
#endif
return CMD_OK;
}
@ -74,7 +74,7 @@ int8_t CMD_Behavior_Handle_FIRE_MODE(CMD_t *ctx) {
int8_t CMD_Behavior_Handle_ROTOR(CMD_t *ctx) {
#if CMD_ENABLE_MODULE_CHASSIS
ctx->output.chassis.cmd.mode = CHASSIS_MODE_ROTOR;
ctx->output.chassis.cmd.mode_rotor = ROTOR_MODE_RAND;
// ctx->output.chassis.cmd.mode_rotor = ROTOR_MODE_RAND;
#endif
#if CMD_ENABLE_MODULE_GIMBAL
ctx->output.gimbal.cmd.mode = GIMBAL_MODE_RELATIVE;

118
User/module/config.c
View File

@ -19,81 +19,51 @@
// theta(变量), d(mm), a(mm), alpha(rad), theta_offset(rad)
// 零位姿态: 机械臂竖直向上
Config_RobotParam_t robot_config = {
.chassis_param = {
/* DJI3508µç»ú*/
.motor_param = {
{
.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
},
.chassis_param={
.pid={
.Radder_DIR_Omega={
.k=1.0f,
.p=0.5f,
.i=0.15f,
.d=0.0f,
.i_limit=1.0f,
.out_limit=1.0f,
.d_cutoff_freq= -1.0f,
.range=-1.0f
},
.Radder_DIR_Angle={
.k=0.15f,
.p=0.20f,
.i=0.1f,
.d=0.001f,
.i_limit=1.0f,
.out_limit=30.0f,
.d_cutoff_freq= -1.0f,
.range=360
},
.Wheel_DIR_Omega={
.k=0.5f,
.p=2.0f,
.i=0.1f,
.d=0.7f,
.i_limit=1.0f,
.out_limit=1.0f,
.d_cutoff_freq= -1.0f,
.range=-1.0f
},
},
.type = CHASSIS_TYPE_MECANUM,
/* PID */
.pid = {
/* µ×Å̵ç»ú PID */
.motor_pid_param = {
.k = 0.0015f,
.p = 1.0f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 1.0f,
.out_limit = 1.0f,
.d_cutoff_freq = -1.0f,
.range = -1.0f,
},
.motor={
.Wheel_DIR[0]={BSP_CAN_2,0x203,MOTOR_M3508,false,false},
.Wheel_DIR[1]={BSP_CAN_2,0x204,MOTOR_M3508,true,false},
.Wheel_DIR[2]={BSP_CAN_2,0x202,MOTOR_M3508,true,false},
.Wheel_DIR[3]={BSP_CAN_2,0x201,MOTOR_M3508,false,false},
/* ¸úËæ */
.follow_pid_param = {
.k = 1.2f,
.p = 1.0f,
.i = 0.5f,
.d = 0.01f,
.i_limit = 1.0f,
.out_limit = 1.0f,
.d_cutoff_freq = -1.0f,
.range = M_2PI,
},
.Radder_DIR[0]={BSP_CAN_2,0x205,MOTOR_GM6020,false,false},
.Radder_DIR[1]={BSP_CAN_2,0x206,MOTOR_GM6020,false,false},
.Radder_DIR[2]={BSP_CAN_2,0x207,MOTOR_GM6020,false,false},
.Radder_DIR[3]={BSP_CAN_2,0x208,MOTOR_GM6020,false,false},
},
.low_pass_cutoff_freq = {
.in = 50.0f,
.out = 50.0f,
},
.reverse = {
.yaw = true,
},
.limit = {
.max_vx = 3.0f,
.max_vy = 3.0f,
.max_wz = 2.0f,
.max_current = 16000.0f
},
},
},
.cmd_param={
.source_priority = {
CMD_SRC_RC,
@ -107,8 +77,8 @@ Config_RobotParam_t robot_config = {
},
.rc_mode_map = {
.sw_left_up = CHASSIS_MODE_RELAX,
.sw_left_mid = CHASSIS_MODE_RELAX,
.sw_left_down = CHASSIS_MODE_INDEPENDENT,
.sw_left_mid = RC,
// .sw_left_down = ,
},
},

2
User/module/config.h
View File

@ -17,7 +17,7 @@ extern "C" {
#include "component/PowerControl.h"
typedef struct {
// Arm_Params_t arm_param;
Chassis_Params_t chassis_param;
Chassis_Param_t chassis_param;
CMD_Config_t cmd_param;
} Config_RobotParam_t;

14
User/module/struct_typedef.h Normal file
View File

@ -0,0 +1,14 @@
#ifndef STRUCT_TYPEDEF_H
#define STRUCT_TYPEDEF_H
#include <stdint.h>
typedef unsigned char bool_t;
typedef float fp32;
typedef double fp64;
////
#endif

21
User/task/ctrl_chassis.c
View File

@ -18,7 +18,7 @@
/* USER STRUCT BEGIN */
Chassis_t chassis;
Chassis_CMD_t chassis_cmd;
Chassis_IMU_t chassis_imu;
//Chassis_IMU_t chassis_imu;
float max =50.0f;
/* USER STRUCT END */
@ -37,25 +37,20 @@ void Task_ctrl_chassis(void *argument) {
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
/* USER CODE INIT BEGIN */
Config_RobotParam_t *cfg = Config_GetRobotParam();
Chassis_Init(&chassis, &cfg->chassis_param, (float)CTRL_CHASSIS_FREQ);
chassis.mech_zero=0.57f;
chassis_init(&chassis, &Config_GetRobotParam()->chassis_param, CTRL_CHASSIS_FREQ);
/* USER CODE INIT END */
while (1) {
tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */
float encoder_gimbalYawMotor;
osMessageQueueGet(task_runtime.msgq.chassis.yaw, &encoder_gimbalYawMotor, NULL, 0);
osMessageQueueGet(task_runtime.msgq.chassis.cmd, &chassis_cmd, NULL, 0);
chassis.feedback.encoder_gimbalYawMotor=encoder_gimbalYawMotor;
Chassis_UpdateFeedback(&chassis);
Chassis_Control(&chassis, &chassis_cmd, osKernelGetTickCount());
Chassis_Power_Control(&chassis,max);
osMessageQueueGet(task_runtime.msgq.chassis.cmd, &chassis_cmd, NULL, 0);
Chassis_update(&chassis);
Chassis_Control(&chassis, &chassis_cmd,tick);
Chassis_Setoutput(&chassis);
Chassis_Output(&chassis);
/* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
}

2
User/task/init.c
View File

@ -33,7 +33,7 @@ void Task_Init(void *argument) {
/* 创建任务线程 */
task_runtime.thread.blink = osThreadNew(Task_blink, NULL, &attr_blink);
task_runtime.thread.arm_main = osThreadNew(Task_arm_main, NULL, &attr_arm_main);
//task_runtime.thread.arm_main = osThreadNew(Task_arm_main, NULL, &attr_arm_main);
task_runtime.thread.rc = osThreadNew(Task_rc, NULL, &attr_rc);
task_runtime.thread.cmd = osThreadNew(Task_cmd, NULL, &attr_cmd);
task_runtime.thread.ctrl_chassis = osThreadNew(Task_ctrl_chassis, NULL, &attr_ctrl_chassis);

4
ozone/arm.jdebug.user
View File

@ -32,7 +32,7 @@ OpenWindow="Memory 1", DockArea=BOTTOM, x=0, y=0, w=463, h=544, FilterBarShown=0
OpenWindow="Watched Data 1", DockArea=RIGHT, x=0, y=0, w=903, h=930, TabPos=1, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Functions", DockArea=LEFT, x=0, y=0, w=322, h=930, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Data Sampling", DockArea=BOTTOM, x=2, y=0, w=725, h=525, TabPos=1, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0
OpenWindow="Timeline", DockArea=BOTTOM, x=1, y=0, w=1370, h=544, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="200 ms / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="0;268", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="1122;-71", CodeGraphLegendShown=0, CodeGraphLegendPosition="1138;0"
OpenWindow="Timeline", DockArea=BOTTOM, x=1, y=0, w=1370, h=544, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="200 ms / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="0;268", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="1122;-69", CodeGraphLegendShown=0, CodeGraphLegendPosition="1138;0"
OpenWindow="Console", DockArea=BOTTOM, x=2, y=0, w=725, h=525, TabPos=0, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
SmartViewPlugin="", Page="", Toolbar="Hidden", Window="SmartView 1"
TableHeader="Registers 1", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Description"], ColWidths=[120;144;639]
@ -42,7 +42,7 @@ TableHeader="RegisterSelectionDialog", SortCol="None", SortOrder="ASCENDING", Vi
TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[215;100;100;100;1334]
TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh";"Access"], ColWidths=[250;229;145;279;100]
TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";" ik_test_ret";" ik_test_ret_analytical";" ik_test_ret_numerical";" ik_test_solver_dbg"], ColWidths=[100;100;100;100;100;100]
TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[118;100;100;100;100;134;110;126;126]
TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[118;100;124;124;124;104;110;126;126]
TableHeader="TargetExceptionDialog", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Address";"Description"], ColWidths=[200;100;100;340]
WatchedExpression="robot_arm", RefreshRate=5, Window=Watched Data 1
WatchedExpression="ik_from_fk_result", RefreshRate=5, Window=Watched Data 1