使用 MATLAB Coder 工具箱直接转换为C语言后即可直接在实机代码中使用，在本项目主控代码中可以找到同名文件,除main函数和PID函数，其余都是MATLAB生成的。debug函数不用管，用于调试。
平衡算法主要参考了这个专栏文章，仅有少许不同，因此不在此处详述，仅描述大致过程，包含以下几个步骤：
注：代码中变量名称与该文章所述基本保持一致，读者可以对照查看
- 对腿部结构进行化简，求得化简前后各符号的对照关系，从而可以使用电机数据计算出腿部的姿态
- 求得化简后的虚拟腿受力到电机扭矩的映射关系，从而可以使用VMC思想对化简后的腿部进行控制
- 进行经典力学分析和符号化简，求得系统状态空间方程
- 设定LQR中的权重系数，求取反馈矩阵K，并对不同腿长求出的K进行拟合
- 进行状态反馈控制
主函数
/*******************主函数*******************/
%初始化各参数
void setup()
{
pinMode(9, INPUT_PULLUP); //按钮引脚
pinMode(10, OUTPUT); //LED引脚
//上电后等待5s
digitalWrite(10, HIGH);
vTaskDelay(5000);
digitalWrite(10, LOW);
//初始化所有模块
Serial_Init();
ADC_Init();
CAN_Init();
IMU_Init();
Motor_InitAll();
Ctrl_Init();
BT_Init();
}
%工作展示
void loop()
{
//LED闪烁次数表示电池电压
for(int i = 0; i < (sourceVoltage - 11) / 0.5f + 1; i++)
{
digitalWrite(10, HIGH);
vTaskDelay(100);
digitalWrite(10, LOW);
vTaskDelay(200);
}
digitalWrite(10, HIGH);
vTaskDelay(100);
digitalWrite(10, LOW);
vTaskDelay(600);
//检测到按钮按下，根据按下时长执行不同的操作
if(digitalRead(9) == LOW)
{
int cmdCnt = 0;
vTaskDelay(2000);
while(digitalRead(9) == LOW) //等待按钮松开，闪烁LED并计算按下时长
{
cmdCnt++;
digitalWrite(10, HIGH);
vTaskDelay(300);
digitalWrite(10, LOW);
vTaskDelay(500);
}
vTaskDelay(2000);
if(cmdCnt == 1) //功能1：设定目标航向角为当前值
{
target.yawAngle = imuData.yaw;
}
else if(cmdCnt == 2) //功能2：进入起立过程
{
xTaskCreate(Ctrl_StandupPrepareTask, "StandupPrepare_Task", 4096, NULL, 1, NULL);
}
else if(cmdCnt == 3) //功能3：向前运动2s
{
target.speedCmd = 1.0f;
vTaskDelay(2000);
target.speedCmd = 0.0f;
}
else if(cmdCnt == 4) //功能4：向前运动5s
{
target.speedCmd = 1.0f;
vTaskDelay(5000);
target.speedCmd = 0.0f;
}
else if(cmdCnt == 5) //功能5：旋转90°
{
target.yawAngle += M_PI;
}
}
}
//真正的入口函数，调用setup和loop，模仿Arduino结构
extern "C" void app_main()
{
setup();
while (1)
{
loop();
}
}
main
/* main.c
* 主代码文件，包含所有主要代码逻辑
* by L.B.W 2023
* */
#include <Arduino.h>
#include <PID.h>
#include "driver/twai.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "leg_pos.h"
#include "lqr_k.h"
#include "leg_conv.h"
#include "leg_spd.h"
#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLE2902.h>
#include <esp_task_wdt.h>
#include <string.h>
#include <debug.h>
/*******************宏定义*******************/
//蓝牙各UUID及设备名称
#define UUID_SERVICE "4c9a0001-fb2e-432e-92ec-c6f5316b4689" //服务UUID
#define UUID_RX_CHARA "4c9a0002-fb2e-432e-92ec-c6f5316b4689" //接收特征UUID
#define UUID_TX_CHARA "4c9a0003-fb2e-432e-92ec-c6f5316b4689" //发送特征UUID
#define BLE_NAME "BalanceBot"
//计时工具，计算运行周期或经过时间
uint32_t timeCnt[6] = {0}, lastTimeCnt[6] = {0}, passTimeCnt[6] = {0};
#define TIME_COUNTER(num) do{timeCnt[num] = micros()-lastTimeCnt[num]; lastTimeCnt[num] = micros();}while(0)
#define PASS_TIME_START(num) uint32_t passTimeStart##num = micros()
#define PASS_TIME_STOP(num) passTimeCnt[num] = micros() - passTimeStart##num
/*******************结构体类型和全局变量定义*******************/
//IMU对象及数据
MPU6050 mpu;
struct IMUData
{
float yaw, pitch, roll; // rad
float yawSpd, pitchSpd, rollSpd; // rad/s
float zAccel; // m/s^2
} imuData;
//电机结构体
//leftJoint[0]:左前关节电机, leftJoint[1]:左后关节电机, leftWheel:左车轮电机
//rightJoint[0]:右前关节电机, rightJoint[1]:右后关节电机, rightWheel:右车轮电机
struct Motor
{
float speed; // rad/s
float angle, offsetAngle; // rad
float voltage, maxVoltage; // V
float torque, torqueRatio; // Nm, voltage = torque / torqueRatio
float dir; // 1 or -1
float (*calcRevVolt)(float speed); // 指向反电动势计算函数
} leftJoint[2], rightJoint[2], leftWheel, rightWheel; //六个电机对象
//腿部姿态结构体
struct LegPos
{
float angle, length; // rad, m
float dAngle, dLength; // rad/s, m/s
float ddLength; // m/s^2
} leftLegPos, rightLegPos; //左右腿部姿态
//状态变量结构体
struct StateVar
{
float theta, dTheta;
float x, dx;
float phi, dPhi;
} stateVar;
//目标量结构体
struct Target
{
float position; // m
float speedCmd; // m/s
float speed; // m/s
float yawSpeedCmd; // rad/s
float yawAngle; // rad
float rollAngle; // rad
float legLength; // m
} target = {0, 0, 0, 0, 0, 0, 0.07f};
//触地检测数据结构体
struct GroundDetector
{
float leftSupportForce, rightSupportForce;
bool isTouchingGround, isCuchioning;
} groundDetector = {10, 10, true, false};
//站立过程状态枚举量
enum StandupState {
StandupState_None,
StandupState_Prepare,
StandupState_Standup,
} standupState = StandupState_None;
CascadePID legAnglePID, legLengthPID; //腿部角度和长度控制PID
CascadePID yawPID, rollPID; //机身yaw和roll控制PID
//蓝牙相关全局对象指针
BLECharacteristic *chara_tx, *chara_rx;
BLEServer *server;
BLEService *service;
float motorOutRatio = 1.0f; //电机输出电压比例，对所有电机同时有效
float sourceVoltage = 12; //当前电源电压
/*******************函数声明*******************/
void setup();
void loop();
void Motor_Init(Motor *motor, float offsetAngle, float maxVoltage, float torqueRatio, float dir);
float Motor_CalcRevVolt4010(float speed);
float Motor_CalcRevVolt2804(float speed);
void Motor_InitAll();
void Motor_Update(Motor *motor, uint8_t *data);
void Motor_SetTorque(Motor *motor, float torque);
void Motor_UpdateVoltage(Motor *motor);
void Motor_SendTask(void *arg);
void CAN_RecvCallback(uint32_t id, uint8_t *data);
void CAN_RecvTask(void *arg);
void CAN_Init();
void CAN_SendFrame(uint32_t id, uint8_t *data);
void IMU_Task(void *arg);
void IMU_Init();
void Ctrl_TargetUpdateTask(void *arg);
void LegPos_UpdateTask(void *arg);
void Ctrl_StandupPrepareTask(void *arg);
void Ctrl_Task(void *arg);
void Ctrl_Init();
void BT_SendSampleTask(void *arg);
void BT_Init();
void Serial_Task(void *pvParameters);
void Serial_Init();
void ADC_Task(void *pvParameters);
void ADC_Init();
/*******************各模块函数定义*******************/
/******* 电机模块 *******/
//初始化一个电机对象
void Motor_Init(Motor *motor, float offsetAngle, float maxVoltage, float torqueRatio, float dir, float (*calcRevVolt)(float speed))
{
motor->speed = motor->angle = motor->voltage = 0;
motor->offsetAngle = offsetAngle;
motor->maxVoltage = maxVoltage;
motor->torqueRatio = torqueRatio;
motor->dir = dir;
motor->calcRevVolt = calcRevVolt;
}
//4010电机反电动势计算函数(输入速度，输出反电动势)
//测量并拟合出不同电压下对应的电机空载转速，调换自变量和因变量就是本函数
//由于该测量方法忽略阻力对空载转速的影响，最终抵消反电动势时也会抵消大部分电机本身的阻力
float Motor_CalcRevVolt4010(float speed)
{
return 0.00008f * speed * speed * speed - 0.0035f * speed * speed + 0.2322f * speed;
}
//2804电机反电动势计算函数(输入速度，输出反电动势)，测量方法同上
float Motor_CalcRevVolt2804(float speed)
{
return 0.000004f * speed * speed * speed - 0.0003f * speed * speed + 0.0266f * speed;
}
//初始化所有电机对象
//各个参数需要通过实际测量或拟合得到
void Motor_InitAll()
{
Motor_Init(&leftJoint[0], 1.431, 7, 0.0316f, -1, Motor_CalcRevVolt4010);
Motor_Init(&leftJoint[1], -7.76, 7, 0.0317f, 1, Motor_CalcRevVolt4010);
Motor_Init(&leftWheel, 0, 4.0f, 0.0096f, 1, Motor_CalcRevVolt2804);
Motor_Init(&rightJoint[0], 0.343, 7, 0.0299f, -1, Motor_CalcRevVolt4010);
Motor_Init(&rightJoint[1], -2.446, 7, 0.0321f, -1, Motor_CalcRevVolt4010);
Motor_Init(&rightWheel, 0, 4.0f, 0.0101f, 1, Motor_CalcRevVolt2804);
xTaskCreate(Motor_SendTask, "Motor_SendTask", 2048, NULL, 5, NULL);
}
//从CAN总线接收到的数据中解析出电机角度和速度
void Motor_Update(Motor *motor, uint8_t *data)
{
motor->angle = (*(int32_t *)&data[0] / 1000.0f - motor->offsetAngle) * motor->dir;
motor->speed = (*(int16_t *)&data[4] / 10 * 2 * M_PI / 60) * motor->dir;
}
//设置电机扭矩
void Motor_SetTorque(Motor *motor, float torque)
{
motor->torque = torque;
}
//由设置的目标扭矩和当前转速计算补偿反电动势后的驱动输出电压，并进行限幅
//补偿的意义: 电机转速越快反电动势越大，需要加大驱动电压来抵消反电动势，使电流(扭矩)不随转速发生变化
void Motor_UpdateVoltage(Motor *motor)
{
float voltage = motor->torque / motor->torqueRatio * motorOutRatio;
if (motor->speed >= 0)
voltage += motor->calcRevVolt(motor->speed);
else if (motor->speed < 0)
voltage -= motor->calcRevVolt(-motor->speed);
if (voltage > motor->maxVoltage)
voltage = motor->maxVoltage;
else if (voltage < -motor->maxVoltage)
voltage = -motor->maxVoltage;
motor->voltage = voltage * motor->dir;
}
//电机指令发送任务
void Motor_SendTask(void *arg)
{
uint8_t data[8] = {0};
Motor* motorList[] = {&leftJoint[0], &leftJoint[1], &leftWheel, &rightJoint[0], &rightJoint[1], &rightWheel};
while (1)
{
for (int i = 0; i < 6; i++)
Motor_UpdateVoltage(motorList[i]); //计算补偿后的电机电压
*(int16_t *)&data[0] = ((int16_t)(leftJoint[0].voltage * 1000));
*(int16_t *)&data[2] = ((int16_t)(leftJoint[1].voltage * 1000));
*(int16_t *)&data[4] = ((int16_t)(leftWheel.voltage * 1000));
CAN_SendFrame(0x100, data);
*(int16_t *)&data[0] = ((int16_t)(rightJoint[0].voltage * 1000));
*(int16_t *)&data[2] = ((int16_t)(rightJoint[1].voltage * 1000));
*(int16_t *)&data[4] = ((int16_t)(rightWheel.voltage * 1000));
CAN_SendFrame(0x200, data);
vTaskDelay(2);
}
}
/******* CAN通信模块 *******/
//CAN收到数据后进入的回调函数
void CAN_RecvCallback(uint32_t id, uint8_t *data)
{
switch (id) //根据CAN ID更新各电机数据
{
case 0x101:
Motor_Update(&leftJoint[0], data);
break;
case 0x102:
Motor_Update(&leftJoint[1], data);
break;
case 0x103:
Motor_Update(&leftWheel, data);
break;
case 0x105:
Motor_Update(&rightJoint[0], data);
break;
case 0x106:
Motor_Update(&rightJoint[1], data);
break;
case 0x107:
Motor_Update(&rightWheel, data);
break;
}
}
//CAN数据帧轮询接收任务
void CAN_RecvTask(void *arg)
{
twai_message_t msg;
twai_status_info_t status;
TickType_t xLastWakeTime = xTaskGetTickCount();
while (1)
{
twai_get_status_info(&status);
for(uint8_t i = 0; i < status.msgs_to_rx; i++)
{
if(twai_receive(&msg, 0) == ESP_OK)
CAN_RecvCallback(msg.identifier, msg.data);
}
vTaskDelayUntil(&xLastWakeTime, 2); //2ms轮询一次
}
}
//CAN通信外设初始化
void CAN_Init()
{
twai_general_config_t twai_conf = {
.mode = TWAI_MODE_NORMAL,
.tx_io = GPIO_NUM_6,
.rx_io = GPIO_NUM_7,
.clkout_io = TWAI_IO_UNUSED,
.bus_off_io = TWAI_IO_UNUSED,
.tx_queue_len = 5,
.rx_queue_len = 10,
.alerts_enabled = TWAI_ALERT_NONE,
.clkout_divider = 0,
.intr_flags = ESP_INTR_FLAG_LEVEL1};
twai_timing_config_t twai_timing = TWAI_TIMING_CONFIG_1MBITS();
twai_filter_config_t twai_filter = {
.acceptance_code = 0x00000000,
.acceptance_mask = 0xFFFFFFFF,
.single_filter = true};
twai_driver_install(&twai_conf, &twai_timing, &twai_filter);
twai_start();
xTaskCreate(CAN_RecvTask, "CAN_RecvTask", 2048, NULL, 5, NULL);
}
//发送一帧CAN数据(data为8字节数据)
void CAN_SendFrame(uint32_t id, uint8_t *data)
{
twai_message_t msg;
msg.flags = 0;
msg.identifier = id;
msg.data_length_code = 8;
memcpy(msg.data, data, 8);
twai_transmit(&msg, 100);
}
/******* 陀螺仪模块 *******/
//陀螺仪数据获取任务
void IMU_Task(void *arg)
{
TickType_t xLastWakeTime = xTaskGetTickCount();
uint8_t fifoBuffer[64]; //dmp数据接收区
int16_t yawRound = 0; //统计yaw转过的整圈数
float lastYaw = 0;
while (1)
{
if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer))
{
//获取陀螺仪角速度
int16_t gyroData[3];
mpu.getRotation(&gyroData[0], &gyroData[1], &gyroData[2]);
imuData.rollSpd = gyroData[1] / 16.4f * M_PI / 180.0f;
imuData.pitchSpd = -gyroData[0] / 16.4f * M_PI / 180.0f;
imuData.yawSpd = gyroData[2] / 16.4f * M_PI / 180.0f;
//获取陀螺仪欧拉角
float ypr[3];
Quaternion q;
VectorFloat gravity;
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
float yaw = -ypr[0];
imuData.pitch = -ypr[2];
imuData.roll = -ypr[1];
if (yaw - lastYaw > M_PI)
yawRound--;
else if (yaw - lastYaw < -M_PI)
yawRound++;
lastYaw = yaw;
imuData.yaw = yaw + yawRound * 2 * M_PI; //imuData.yaw为累计转角
//获取陀螺仪Z轴加速度
VectorInt16 rawAccel;
mpu.dmpGetAccel(&rawAccel, fifoBuffer);
VectorInt16 accel;
mpu.dmpGetLinearAccel(&accel, &rawAccel, &gravity);
imuData.zAccel = accel.z / 8192.0f * 9.8f;
}
vTaskDelayUntil(&xLastWakeTime, 5); //5ms轮询一次
}
}
//陀螺仪模块初始化
void IMU_Init()
{
//初始化IIC
Wire.begin(5, 4);
Wire.setClock(400000);
//初始化陀螺仪
mpu.initialize();
Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
Serial.println("Address: 0x" + String(mpu.getDeviceID(), HEX));
mpu.setFullScaleAccelRange(MPU6050_IMU::MPU6050_ACCEL_FS_16);
while(mpu.dmpInitialize() != 0);
mpu.setXAccelOffset(-668);
mpu.setYAccelOffset(-1632);
mpu.setZAccelOffset(998);
mpu.setXGyroOffset(283);
mpu.setYGyroOffset(80);
mpu.setZGyroOffset(42);
// mpu.CalibrateAccel(6); //测量偏移数据
// mpu.CalibrateGyro(6);
// mpu.PrintActiveOffsets();
mpu.setDMPEnabled(true);
//开启陀螺仪数据获取任务
xTaskCreate(IMU_Task, "IMU_Task", 2048, NULL, 5, NULL);
}
/******* 运动控制模块 *******/
//目标量更新任务(根据蓝牙收到的目标量计算实际控制算法的给定量)
void Ctrl_TargetUpdateTask(void *arg)
{
TickType_t xLastWakeTime = xTaskGetTickCount();
float speedSlopeStep = 0.003f;
while (1)
{
//根据当前腿长计算速度斜坡步长(腿越短越稳定，加减速斜率越大)
float legLength = (leftLegPos.length + rightLegPos.length) / 2;
speedSlopeStep = -(legLength - 0.07f) * 0.02f + 0.002f;
//计算速度斜坡，斜坡值更新到target.speed
if(fabs(target.speedCmd - target.speed) < speedSlopeStep)
target.speed = target.speedCmd;
else
{
if(target.speedCmd - target.speed > 0)
target.speed += speedSlopeStep;
else
target.speed -= speedSlopeStep;
}
//计算位置目标，并限制在当前位置的±0.1m内
target.position += target.speed * 0.004f;
if(target.position - stateVar.x > 0.1f)
target.position = stateVar.x + 0.1f;
else if(target.position - stateVar.x < -0.1f)
target.position = stateVar.x - 0.1f;
//限制速度目标在当前速度的±0.3m/s内
if(target.speed - stateVar.dx > 0.3f)
target.speed = stateVar.dx + 0.3f;
else if(target.speed - stateVar.dx < -0.3f)
target.speed = stateVar.dx - 0.3f;
//计算yaw方位角目标
target.yawAngle += target.yawSpeedCmd * 0.004f;
vTaskDelayUntil(&xLastWakeTime, 4); //每4ms更新一次
}
}
//腿部姿态更新任务(根据关节电机数据计算腿部姿态)
void LegPos_UpdateTask(void *arg)
{
const float lpfRatio = 0.5f; //低通滤波系数(新值的权重)
float lastLeftDLength = 0, lastRightDLength = 0;
TickType_t xLastWakeTime = xTaskGetTickCount();
while (1)
{
float legPos[2], legSpd[2];
//计算左腿位置
leg_pos(leftJoint[1].angle, leftJoint[0].angle, legPos);
leftLegPos.length = legPos[0];
leftLegPos.angle = legPos[1];
//计算左腿速度
leg_spd(leftJoint[1].speed, leftJoint[0].speed, leftJoint[1].angle, leftJoint[0].angle, legSpd);
leftLegPos.dLength = legSpd[0];
leftLegPos.dAngle = legSpd[1];
//计算左腿腿长加速度
leftLegPos.ddLength = ((leftLegPos.dLength - lastLeftDLength) * 1000 / 4) * lpfRatio + leftLegPos.ddLength * (1 - lpfRatio);
lastLeftDLength = leftLegPos.dLength;
//计算右腿位置
leg_pos(rightJoint[1].angle, rightJoint[0].angle, legPos);
rightLegPos.length = legPos[0];
rightLegPos.angle = legPos[1];
//计算右腿速度
leg_spd(rightJoint[1].speed, rightJoint[0].speed, rightJoint[1].angle, rightJoint[0].angle, legSpd);
rightLegPos.dLength = legSpd[0];
rightLegPos.dAngle = legSpd[1];
//计算右腿腿长加速度
rightLegPos.ddLength = ((rightLegPos.dLength - lastRightDLength) * 1000 / 4) * lpfRatio + rightLegPos.ddLength * (1 - lpfRatio);
lastRightDLength = rightLegPos.dLength;
vTaskDelayUntil(&xLastWakeTime, 4); //每4ms更新一次
}
}
//站立准备任务(将机器人从任意姿态调整到准备站立前的劈叉状态)
void Ctrl_StandupPrepareTask(void *arg)
{
standupState = StandupState_Prepare;
//将左腿向后摆
Motor_SetTorque(&leftJoint[0], 0.2f);
Motor_SetTorque(&leftJoint[1], 0.2f);
while(leftLegPos.angle < M_3PI_4)
vTaskDelay(5);
Motor_SetTorque(&leftJoint[0], 0);
Motor_SetTorque(&leftJoint[1], 0);
vTaskDelay(1000);
//将右腿向前摆
Motor_SetTorque(&rightJoint[0], -0.2f);
Motor_SetTorque(&rightJoint[1], -0.2f);
while(rightLegPos.angle > M_PI_4)
vTaskDelay(5);
Motor_SetTorque(&rightJoint[0], 0);
Motor_SetTorque(&rightJoint[1], 0);
vTaskDelay(1000);
//完成准备动作，关闭电机结束任务
Motor_SetTorque(&leftJoint[0], 0);
Motor_SetTorque(&leftJoint[1], 0);
Motor_SetTorque(&leftWheel, 0);
Motor_SetTorque(&rightJoint[0], 0);
Motor_SetTorque(&rightJoint[1], 0);
Motor_SetTorque(&rightWheel, 0);
standupState = StandupState_Standup;
vTaskDelete(NULL);
}
//主控制任务
void Ctrl_Task(void *arg)
{
const float wheelRadius = 0.026f; //m，车轮半径
const float legMass = 0.05f; //kg，腿部质量
TickType_t xLastWakeTime = xTaskGetTickCount();
//手动为反馈矩阵和输出叠加一个系数，用于手动优化控制效果
float kRatio[2][6] = {{1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f},
{1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f}};
float lqrTpRatio = 1.0f, lqrTRatio = 1.0f;
//设定初始目标值
target.rollAngle = 0.0f;
target.legLength = 0.07f;
target.speed = 0.0f;
target.position = (leftWheel.angle + rightWheel.angle) / 2 * wheelRadius;
while (1)
{
//计算状态变量
stateVar.phi = imuData.pitch;
stateVar.dPhi = imuData.pitchSpd;
stateVar.x = (leftWheel.angle + rightWheel.angle) / 2 * wheelRadius;
stateVar.dx = (leftWheel.speed + rightWheel.speed) / 2 * wheelRadius;
stateVar.theta = (leftLegPos.angle + rightLegPos.angle) / 2 - M_PI_2 - imuData.pitch;
stateVar.dTheta = (leftLegPos.dAngle + rightLegPos.dAngle) / 2 - imuData.pitchSpd;
float legLength = (leftLegPos.length + rightLegPos.length) / 2;
float dLegLength = (leftLegPos.dLength + rightLegPos.dLength) / 2;
//如果正在站立准备状态，则不进行后续控制
if(standupState == StandupState_Prepare)
{
vTaskDelayUntil(&xLastWakeTime, 4);
continue;
}
//计算LQR反馈矩阵
float kRes[12] = {0}, k[2][6] = {0};
lqr_k(legLength, kRes);
if(groundDetector.isTouchingGround) //正常触地状态
{
for (int i = 0; i < 6; i++)
{
for (int j = 0; j < 2; j++)
k[j][i] = kRes[i * 2 + j] * kRatio[j][i];
}
}
else //腿部离地状态，手动修改反馈矩阵，仅保持腿部竖直
{
memset(k, 0, sizeof(k));
k[1][0] = kRes[1] * -2;
k[1][1] = kRes[3] * -10;
}
//准备状态变量
float x[6] = {stateVar.theta, stateVar.dTheta, stateVar.x, stateVar.dx, stateVar.phi, stateVar.dPhi};
//与给定量作差
x[2] -= target.position;
x[3] -= target.speed;
//矩阵相乘，计算LQR输出
float lqrOutT = k[0][0] * x[0] + k[0][1] * x[1] + k[0][2] * x[2] + k[0][3] * x[3] + k[0][4] * x[4] + k[0][5] * x[5];
float lqrOutTp = k[1][0] * x[0] + k[1][1] * x[1] + k[1][2] * x[2] + k[1][3] * x[3] + k[1][4] * x[4] + k[1][5] * x[5];
//计算yaw轴PID输出
PID_CascadeCalc(&yawPID, target.yawAngle, imuData.yaw, imuData.yawSpd);
//设定车轮电机输出扭矩，为LQR和yaw轴PID输出的叠加
if(groundDetector.isTouchingGround) //正常接地状态
{
Motor_SetTorque(&leftWheel, -lqrOutT * lqrTRatio - yawPID.output);
Motor_SetTorque(&rightWheel, -lqrOutT * lqrTRatio + yawPID.output);
}
else //腿部离地状态，关闭车轮电机
{
Motor_SetTorque(&leftWheel, 0);
Motor_SetTorque(&rightWheel, 0);
}
//根据离地状态修改目标腿长，并计算腿长PID输出
PID_CascadeCalc(&legLengthPID, (groundDetector.isTouchingGround && !groundDetector.isCuchioning) ? target.legLength : 0.12f, legLength, dLegLength);
//计算roll轴PID输出
PID_CascadeCalc(&rollPID, target.rollAngle, imuData.roll, imuData.rollSpd);
//根据离地状态计算左右腿推力，若离地则不考虑roll轴PID输出和前馈量
float leftForce = legLengthPID.output + ((groundDetector.isTouchingGround && !groundDetector.isCuchioning) ? 6-rollPID.output : 0);
float rightForce = legLengthPID.output + ((groundDetector.isTouchingGround && !groundDetector.isCuchioning) ? 6+rollPID.output : 0);
if(leftLegPos.length > 0.12f) //保护腿部不能伸太长
leftForce -= (leftLegPos.length - 0.12f) * 100;
if(rightLegPos.length > 0.12f)
rightForce -= (rightLegPos.length - 0.12f) * 100;
//计算左右腿的地面支持力
groundDetector.leftSupportForce = leftForce + legMass * 9.8f - legMass * (leftLegPos.ddLength - imuData.zAccel);
groundDetector.rightSupportForce = rightForce + legMass * 9.8f - legMass * (rightLegPos.ddLength - imuData.zAccel);
//更新离地检测器数据
static uint32_t lastTouchTime = 0;
bool isTouchingGround = groundDetector.leftSupportForce > 3 && groundDetector.rightSupportForce > 3; //判断当前瞬间是否接地
if(!isTouchingGround && millis() - lastTouchTime < 1000) //若上次触地时间距离现在不超过1s，则认为当前瞬间接地，避免弹跳导致误判
isTouchingGround = true;
if(!groundDetector.isTouchingGround && isTouchingGround) //判断转为接地状态，标记进入缓冲状态
{
target.position = stateVar.x;
groundDetector.isCuchioning = true;
lastTouchTime = millis();
}
if(groundDetector.isCuchioning && legLength < target.legLength) //缓冲状态直到腿长压缩到目标腿长结束
groundDetector.isCuchioning = false;
groundDetector.isTouchingGround = isTouchingGround;
//计算左右腿角度差PID输出
PID_CascadeCalc(&legAnglePID, 0, leftLegPos.angle - rightLegPos.angle, leftLegPos.dAngle - rightLegPos.dAngle);
//计算髋关节扭矩输出，为LQR输出和左右腿角度差PID输出的叠加
float leftTp = lqrOutTp * lqrTpRatio - legAnglePID.output * (leftLegPos.length / 0.07f);
float rightTp = lqrOutTp * lqrTpRatio + legAnglePID.output * (rightLegPos.length / 0.07f);
//使用VMC计算各关节电机输出扭矩
float leftJointTorque[2]={0};
leg_conv(leftForce, leftTp, leftJoint[1].angle, leftJoint[0].angle, leftJointTorque);
float rightJointTorque[2]={0};
leg_conv(rightForce, rightTp, rightJoint[1].angle, rightJoint[0].angle, rightJointTorque);
//保护腿部角度不超限
float leftTheta = leftLegPos.angle - imuData.pitch - M_PI_2;
float rightTheta = rightLegPos.angle - imuData.pitch - M_PI_2;
#define PROTECT_CONDITION (leftTheta < -M_PI_4 || leftTheta > M_PI_4 || \
rightTheta < -M_PI_4 || rightTheta > M_PI_4 || \
imuData.pitch > M_PI_4 || imuData.pitch < -M_PI_4) //腿部角度超限保护条件
if(PROTECT_CONDITION) //当前达到保护条件
{
if(standupState == StandupState_None) //未处于起立过程中
{
//关闭所有电机
Motor_SetTorque(&leftWheel, 0);
Motor_SetTorque(&rightWheel, 0);
Motor_SetTorque(&leftJoint[0], 0);
Motor_SetTorque(&leftJoint[1], 0);
Motor_SetTorque(&rightJoint[0], 0);
Motor_SetTorque(&rightJoint[1], 0);
//阻塞等待腿部角度回到安全范围，再等待4s后恢复控制(若中途触发了起立则在起立准备完成后直接跳出)
while(PROTECT_CONDITION && standupState == StandupState_None)
{
leftTheta = leftLegPos.angle - imuData.pitch - M_PI_2;
rightTheta = rightLegPos.angle - imuData.pitch - M_PI_2;
vTaskDelay(100);
}
if(standupState == StandupState_None)
vTaskDelay(4000);
//退出保护后设定目标位置和yaw角度为当前值
target.position = (leftWheel.angle + rightWheel.angle) / 2 * wheelRadius;
target.yawAngle = imuData.yaw;
continue;
}
if(standupState == StandupState_Standup && (leftTheta < -M_PI_4 || rightTheta > M_PI_4))
standupState = StandupState_None;
}
else
{
if(standupState == StandupState_Standup) //未达到保护条件且处于起立过程中，说明起立完成，退出起立过程
standupState = StandupState_None;
}
//设定关节电机输出扭矩
Motor_SetTorque(&leftJoint[0], -leftJointTorque[0]);
Motor_SetTorque(&leftJoint[1], -leftJointTorque[1]);
Motor_SetTorque(&rightJoint[0], -rightJointTorque[0]);
Motor_SetTorque(&rightJoint[1], -rightJointTorque[1]);
vTaskDelayUntil(&xLastWakeTime, 4); //4ms控制周期
}
}
//控制模块初始化
void Ctrl_Init()
{
//初始化各个PID参数
PID_Init(&yawPID.inner, 0.01, 0, 0, 0, 0.1);
PID_Init(&yawPID.outer, 10, 0, 0, 0, 2);
PID_Init(&rollPID.inner, 1, 0, 5, 0, 5);
PID_Init(&rollPID.outer, 20, 0, 0, 0, 3);
PID_SetErrLpfRatio(&rollPID.inner, 0.1f);
PID_Init(&legLengthPID.inner, 10.0f, 1, 30.0f, 2.0f, 10.0f);
PID_Init(&legLengthPID.outer, 5.0f, 0, 0.0f, 0.0f, 0.5f);
PID_SetErrLpfRatio(&legLengthPID.inner, 0.5f);
PID_Init(&legAnglePID.inner, 0.04, 0, 0, 0, 1);
PID_Init(&legAnglePID.outer, 12, 0, 0, 0, 20);
PID_SetErrLpfRatio(&legAnglePID.outer, 0.5f);
//触发各个控制任务
xTaskCreate(Ctrl_TargetUpdateTask, "Ctrl_TargetUpdateTask", 4096, NULL, 3, NULL);
xTaskCreate(LegPos_UpdateTask, "LegPos_UpdateTask", 4096, NULL, 2, NULL);
vTaskDelay(2);
xTaskCreate(Ctrl_Task, "Ctrl_Task", 4096, NULL, 1, NULL);
}
/******* 蓝牙模块 *******/
//通过蓝牙notify发送数据
extern "C" void BT_Send(uint8_t *data, uint32_t len)
{
chara_tx->setValue(data, len);
chara_tx->notify();
}
//通过蓝牙发送采样数据的任务
void BT_SendSampleTask(void *arg)
{
TickType_t xLastWakeTime = xTaskGetTickCount();
uint8_t runDuration = (*(uint8_t *)arg) * 1000;
uint32_t startTime = millis();
while (1)
{
String str = "";
str += String(target.speedCmd, 3) + "," + String(target.speed, 3) + "," + String(stateVar.dx, 3) + "," + String(stateVar.phi) + "," + String(stateVar.theta) + "\r\n";
// str += String(target.yawAngle, 3) + "," + String(imuData.yaw, 3) + "," + String(imuData.yawSpd, 3) + "\r\n";
BT_Send((uint8_t *)str.c_str(), str.length());
if (runDuration > 0 && millis() - startTime > runDuration)
vTaskDelete(NULL);
vTaskDelayUntil(&xLastWakeTime, 100);
}
}
//重写蓝牙服务回调类，处理连接和断连事件
class MyBleServerCallbacks : public BLEServerCallbacks
{
public:
void onConnect(BLEServer *server)
{
Serial.println("onConnect");
server->getAdvertising()->stop();
}
void onDisconnect(BLEServer *server)
{
Serial.println("onDisconnect");
server->getAdvertising()->start();
}
};
//重写蓝牙特征回调类，处理特征读写事件
class MyBleCharaCallbacks : public BLECharacteristicCallbacks
{
public:
void onWrite(BLECharacteristic *chara) //特征写入事件，收到蓝牙数据
{
uint32_t len = chara->getLength();
uint8_t *data = chara->getData();
// Serial.print("onWrite: " + String(len) + " bytes: "); //测试蓝牙收发
// for (int i = 0; i < len; i++)
// Serial.print(String(data[i], HEX) + " ");
// Serial.println();
// BT_Send(data, len);
if(data[0] == 0xAA && len == 4) //0xAA开头，表示目标值控制指令
{
target.speedCmd = (data[2] / 100.0f - 1) * 1.0f;
target.yawSpeedCmd = -(data[1] / 100.0f - 1) * 1.5f;
target.legLength = (data[3] / 200.0f) * 0.02f + 0.07f;
}
else if(data[0] == 0xAB) //0xAB开头，表示触发起立过程
{
xTaskCreate(Ctrl_StandupPrepareTask, "StandupPrepare_Task", 4096, NULL, 1, NULL);
}
else if(data[0] == 0xAC && len == 2) //0xAC开头，表示触发一次数据采样
{
xTaskCreate(BT_SendSampleTask, "BT_SendSampleTask", 4096, &data[1], 1, NULL);
}
else if(data[0] == 0xDB) //0xDB开头，表示是调试指令
{
Debug_SerialRecv(data, len);
}
}
};
//蓝牙模块初始化，创建设备、服务、特征并启动广播
void BT_Init()
{
BLEDevice::init(BLE_NAME);
server = BLEDevice::createServer();
server->setCallbacks(new MyBleServerCallbacks());
service = server->createService(UUID_SERVICE);
chara_tx = service->createCharacteristic(UUID_TX_CHARA, BLECharacteristic::PROPERTY_NOTIFY);
chara_tx->addDescriptor(new BLE2902());
chara_rx = service->createCharacteristic(UUID_RX_CHARA, BLECharacteristic::PROPERTY_WRITE_NR);
chara_rx->setCallbacks(new MyBleCharaCallbacks());
service->start();
server->getAdvertising()->start();
}
/******* 串口模块 *******/
//串口定时发送任务(调试用)
void Serial_Task(void *pvParameters)
{
Serial.begin(115200);
Serial.setTimeout(10);
while (1)
{
// Serial.printf("%f,%f",leftJoint[0].angle, leftJoint[1].angle);
// Serial.printf("%f,%f,%f,%f\r\n",leftLegPos.length, leftLegPos.angle, rightLegPos.length, rightLegPos.angle);
// Serial.printf("%f,%f,%f\n",imuData.yaw, imuData.pitch, imuData.roll);
// Serial.printf("%f,%f,%f,%f,%f,%f\r\n",leftJoint[0].angle, leftJoint[1].angle, leftWheel.angle, rightJoint[0].angle, rightJoint[1].angle, rightWheel.angle);
// Serial.printf("%f,%f,%f,%f,%f,%f\r\n",stateVar.theta,stateVar.dTheta,stateVar.x,stateVar.dx,stateVar.phi,stateVar.dPhi);
// Serial.printf("%f,%f\r\n", stateVar.theta, stateVar.dTheta);
// Serial.printf("%f,%f\r\n",imuData.pitch,imuData.pitchSpd);
// Serial.printf("%p\r\n", &speedPID.kp);
// Serial.printf("source voltage: %f\r\n", analogRead(0) / 4095.0f * 3.3f * 11 * 12 / 13.5f);
vTaskDelay(50);
}
}
//串口模块初始化
void Serial_Init()
{
xTaskCreate(Serial_Task, "Serial_Task", 4096, NULL, 1, NULL);
}
/******* ADC模块 *******/
//ADC定时采样任务，采样电池电压并计算电机输出比例
void ADC_Task(void *pvParameters)
{
while (1)
{
sourceVoltage = analogRead(0) / 4095.0f * 3.3f * 11 * 12 / 13.5f;
if(sourceVoltage < 8)
sourceVoltage = 12;
motorOutRatio = (12 - sourceVoltage) / 10.0f + 0.7f;
vTaskDelay(100);
}
}
//ADC模块初始化
void ADC_Init()
{
xTaskCreate(ADC_Task, "ADC_Task", 4096, NULL, 5, NULL);
}
/*******************主函数*******************/
void setup()
{
pinMode(9, INPUT_PULLUP); //按钮引脚
pinMode(10, OUTPUT); //LED引脚
//上电后等待5s
digitalWrite(10, HIGH);
vTaskDelay(5000);
digitalWrite(10, LOW);
//初始化所有模块
Serial_Init();
ADC_Init();
CAN_Init();
IMU_Init();
Motor_InitAll();
Ctrl_Init();
BT_Init();
}
void loop()
{
//LED闪烁次数表示电池电压
for(int i = 0; i < (sourceVoltage - 11) / 0.5f + 1; i++)
{
digitalWrite(10, HIGH);
vTaskDelay(100);
digitalWrite(10, LOW);
vTaskDelay(200);
}
digitalWrite(10, HIGH);
vTaskDelay(100);
digitalWrite(10, LOW);
vTaskDelay(600);
//检测到按钮按下，根据按下时长执行不同的操作
if(digitalRead(9) == LOW)
{
int cmdCnt = 0;
vTaskDelay(2000);
while(digitalRead(9) == LOW) //等待按钮松开，闪烁LED并计算按下时长
{
cmdCnt++;
digitalWrite(10, HIGH);
vTaskDelay(300);
digitalWrite(10, LOW);
vTaskDelay(500);
}
vTaskDelay(2000);
if(cmdCnt == 1) //功能1：设定目标航向角为当前值
{
target.yawAngle = imuData.yaw;
}
else if(cmdCnt == 2) //功能2：进入起立过程
{
xTaskCreate(Ctrl_StandupPrepareTask, "StandupPrepare_Task", 4096, NULL, 1, NULL);
}
else if(cmdCnt == 3) //功能3：向前运动2s
{
target.speedCmd = 1.0f;
vTaskDelay(2000);
target.speedCmd = 0.0f;
}
else if(cmdCnt == 4) //功能4：向前运动5s
{
target.speedCmd = 1.0f;
vTaskDelay(5000);
target.speedCmd = 0.0f;
}
else if(cmdCnt == 5) //功能5：旋转90°
{
target.yawAngle += M_PI;
}
}
}
//真正的入口函数，调用setup和loop，模仿Arduino结构
extern "C" void app_main()
{
setup();
while (1)
{
loop();
}
}
lqr_k
extern void lqr_k(float L0, float K[12]);
代入腿长 L0 后返回该腿长对应的反馈矩阵 K
.h
/*
* File: lqr_k.h
*
* MATLAB Coder version : 5.5
* C/C++ source code generated on : 27-Jan-2023 15:11:40
*/
#ifndef LQR_K_H
#define LQR_K_H
/* Include Files */
#include <stdint.h>
#include <stddef.h>
#include <stdlib.h>
#ifdef __cplusplus
extern "C" {
#endif
/* Function Declarations */
extern void lqr_k(float L0, float K[12]);
#ifdef __cplusplus
}
#endif
#endif
/*
* File trailer for lqr_k.h
*
* [EOF]
*/
这是一个 C 语言头文件，用于声明名为 lqr_k 的函数。该函数接受一个 L0 的浮点数参数和一个大小为 12 的浮点数数组 K，并且没有返回值。
通过这个头文件，你可以在 C/C++ 程序中调用名为 lqr_k 的函数，将参数传递给它，并获取对应的 K 数组作为输出。
如果你有一个与这个头文件对应的 C/C++ 源文件，那么你就可以在其中实现 lqr_k 函数的具体功能，并进行编译链接，以生成可执行程序或库文件。
.C
/*
* File: lqr_k.c
*
* MATLAB Coder version : 5.5
* C/C++ source code generated on : 27-Jan-2023 15:11:40
*/
/* Include Files */
#include "lqr_k.h"
#include <math.h>
/* Function Definitions */
/*
* LQR_K
* K = LQR_K(L0)
*
* Arguments : float L0
* float K[12]
* Return Type : void
*/
void lqr_k(float L0, float K[12])
{
float t2;
float t3;
/* This function was generated by the Symbolic Math Toolbox version 9.2.
*/
/* 07-Feb-2023 16:51:43 */
t2 = L0 * L0;
t3 = L0 * L0 * L0;
K[0] = ((L0 * -5.12699127F - t2 * 39.913887F) + t3 * 78.6044769F) +
0.0248099864F;
K[1] = ((L0 * 0.0164185613F + t2 * 0.0612591766F) - t3 * 0.362157434F) -
0.00201288541F;
K[2] = ((L0 * -0.713818431F - t2 * 7.75286484F) + t3 * 9.914464F) +
0.00633995887F;
K[3] = ((L0 * 0.00259846705F - t2 * 0.00533048F) + t3 * 0.00809646118F) -
1.80964162E-5F;
K[4] = ((L0 * -2.53893256F - t2 * 13.9123669F) + t3 * 39.0028343F) +
0.0272015687F;
K[5] = ((L0 * -0.0322025791F + t2 * 0.21739994F) - t3 * 0.565484107F) +
0.00397678185F;
K[6] = ((L0 * -2.33014488F - t2 * 10.921422F) + t3 * 27.9311523F) +
0.0158132948F;
K[7] = ((L0 * 0.00878450926F - t2 * 0.0480066165F) + t3 * 0.108919211F) +
0.000384154031F;
K[8] =
((L0 * -20.1008434F + t2 * 128.616592F) - t3 * 322.886169F) + 1.63913763F;
K[9] =
((L0 * 0.212965459F - t2 * 1.61672437F) + t3 * 4.40539169F) + 1.96408653F;
K[10] = ((L0 * -0.707069635F + t2 * 4.5913167F) - t3 * 11.6047363F) +
0.060216561F;
K[11] = ((L0 * 0.00349324F - t2 * 0.0262558721F) + t3 * 0.0712726861F) +
0.0721909478F;
}
/*
* File trailer for lqr_k.c
*
* [EOF]
*/
与MATLAB代码一致，是MATLAB生成的.c代码
leg_pos.h
%phi1 phi4 关节电机角度
%pos[2] 腿部姿态
extern void leg_pos(float phi1, float phi4, float pos[2]);
可由关节电机角度求得腿部姿态
.h
/*
* File: leg_pos.h
*
* MATLAB Coder version : 5.5
* C/C++ source code generated on : 27-Jan-2023 19:41:46
*/
#ifndef LEG_POS_H
#define LEG_POS_H
/* Include Files */
#include <stdint.h>
#include <stddef.h>
#include <stdlib.h>
#ifdef __cplusplus
extern "C" {
#endif
/* Function Declarations */
extern void leg_pos(float phi1, float phi4, float pos[2]);
#ifdef __cplusplus
}
#endif
#endif
/*
* File trailer for leg_pos.h
*
* [EOF]
*/
.c
/*
* File: leg_pos.c
*
* MATLAB Coder version : 5.5
* C/C++ source code generated on : 27-Jan-2023 19:41:46
*/
/* Include Files */
#include "leg_pos.h"
#include <math.h>
/* Function Definitions */
/*
* LEG_POS
* POS = LEG_POS(PHI1,PHI4)
*
* Arguments : float phi1
* float phi4
* float pos[2]
* Return Type : void
*/
void leg_pos(float phi1, float phi4, float pos[2])
{
float a;
float b_a;
float t14;
float t15;
float t2;
float t3;
float t4;
float t5;
float t6;
float t8_tmp;
/* This function was generated by the Symbolic Math Toolbox version 9.2.
*/
/* 27-Jan-2023 19:36:58 */
t2 = cosf(phi1);
t3 = cosf(phi4);
t4 = sinf(phi1);
t5 = sinf(phi4);
t6 = t2 / 20.0F;
t8_tmp = t4 / 20.0F;
t14 = t4 * 0.0105F;
t15 = t5 * 0.0105F;
t5 = t8_tmp - t5 / 20.0F;
a = (t3 / 20.0F - t6) + 0.06F;
b_a = t14 - t15;
t4 = t3 * 0.0105F - t2 * 0.0105F;
t5 = t5 * t5 + a * a;
t4 = atanf(1.0F / ((t4 + 0.0126F) + t5) *
((t15 - t14) +
sqrtf((b_a * b_a + (t4 + 0.0126F) * (t4 + 0.0126F)) - t5 * t5))) *
2.0F;
t5 = t8_tmp + sinf(t4) * 0.105F;
t4 = (t6 + cosf(t4) * 0.105F) - 0.03F;
pos[0] = sqrtf(t5 * t5 + t4 * t4);
pos[1] = atan2f(t5, t4);
}
/*
* File trailer for leg_pos.c
*
* [EOF]
*/
leg_pos.m：可由关节电机角度求得腿部姿态,MATLAB代码：
function pos = leg_pos(phi1,phi4)
%LEG_POS
% POS = LEG_POS(PHI1,PHI4)
% This function was generated by the Symbolic Math Toolbox version 23.2.
% 2024-03-01 10:36:48
t2 = cos(phi1);
t3 = cos(phi4);
t4 = sin(phi1);
t5 = sin(phi4);
t6 = t2./2.0e+1;
t7 = t3./2.0e+1;
t8 = t4./2.0e+1;
t9 = t5./2.0e+1;
t12 = t2.*1.05e-2;
t13 = t3.*1.05e-2;
t14 = t4.*1.05e-2;
t15 = t5.*1.05e-2;
t10 = -t6;
t11 = -t9;
t16 = -t12;
t17 = -t14;
t18 = -t15;
t19 = t8+t11;
t21 = t7+t10+3.0./5.0e+1;
t23 = t14+t18;
t25 = t13+t16+1.26e-2;
t20 = t19.^2;
t22 = t21.^2;
t24 = t23.^2;
t26 = t25.^2;
t27 = t20+t22;
t28 = t27.^2;
t30 = t25+t27;
t29 = -t28;
t31 = 1.0./t30;
t32 = t24+t26+t29;
t33 = sqrt(t32);
t34 = t15+t17+t33;
t35 = t31.*t34;
t36 = atan(t35);
t37 = t36.*2.0;
t38 = cos(t37);
t39 = sin(t37);
t40 = t38.*(2.1e+1./2.0e+2);
pos = [sqrt((t8+t39.*(2.1e+1./2.0e+2)).^2+(t6+t40-3.0./1.0e+2).^2);atan2(t4./2.0e+1+t39.*(2.1e+1./2.0e+2),t6+t40-3.0./1.0e+2)];
end
leg_spd
extern void leg_spd(float dphi1, float dphi4,
float phi1, float phi4, float spd[2]);
%可由关节电机角度速度求得腿部运动速度
.h
/*
* File: leg_spd.h
*
* MATLAB Coder version : 5.5
* C/C++ source code generated on : 26-Feb-2023 10:27:33
*/
#ifndef LEG_SPD_H
#define LEG_SPD_H
/* Include Files */
#include <stddef.h>
#include <stdlib.h>
#ifdef __cplusplus
extern "C" {
#endif
/* Function Declarations */
extern void leg_spd(float dphi1, float dphi4, float phi1, float phi4,
float spd[2]);
#ifdef __cplusplus
}
#endif
#endif
/*
* File trailer for leg_spd.h
*
* [EOF]
*/
.c
/*
* File: leg_spd.c
*
* MATLAB Coder version : 5.5
* C/C++ source code generated on : 26-Feb-2023 10:27:33
*/
/* Include Files */
#include "leg_spd.h"
#include <math.h>
/* Function Definitions */
/*
* LEG_SPD
* SPD = LEG_SPD(DPHI1,DPHI4,PHI1,PHI4)
*
* Arguments : float dphi1
* float dphi4
* float phi1
* float phi4
* float spd[2]
* Return Type : void
*/
void leg_spd(float dphi1, float dphi4, float phi1, float phi4, float spd[2])
{
float b_out_tmp;
float out_tmp;
float t10_tmp;
float t12_tmp;
float t2;
float t21;
float t22;
float t23;
float t24;
float t28;
float t3;
float t32;
float t4;
float t44;
float t47;
float t48;
float t5;
float t52;
float t53;
float t59;
float t60;
float t61;
float t70;
float t71;
float t76;
/* This function was generated by the Symbolic Math Toolbox version 9.2.
*/
/* 26-Feb-2023 10:23:21 */
t2 = cosf(phi1);
t3 = cosf(phi4);
t4 = sinf(phi1);
t5 = sinf(phi4);
t10_tmp = t2 / 20.0F;
t12_tmp = t4 / 20.0F;
t21 = t2 * 0.0105F;
t22 = t3 * 0.0105F;
t23 = t4 * 0.0105F;
t24 = t5 * 0.0105F;
t28 = t12_tmp - t5 / 20.0F;
out_tmp = t3 / 20.0F - t10_tmp;
t32 = t23 - t24;
b_out_tmp = t22 - t21;
t44 = t28 * t28 + (out_tmp + 0.06F) * (out_tmp + 0.06F);
t47 = t2 * t28 / 10.0F + t4 * (out_tmp + 0.06F) / 10.0F;
t48 = t3 * t28 / 10.0F + t5 * (out_tmp + 0.06F) / 10.0F;
t52 = 1.0F / ((b_out_tmp + 0.0126F) + t44);
t53 = t52 * t52;
t59 = sqrtf((t32 * t32 + (b_out_tmp + 0.0126F) * (b_out_tmp + 0.0126F)) -
t44 * t44);
t60 = 1.0F / t59;
t61 = (t24 - t23) + t59;
t28 = atanf(t52 * t61) * 2.0F;
t70 = cosf(t28);
t71 = sinf(t28);
t76 = 1.0F / (t53 * (t61 * t61) + 1.0F);
t47 = (t23 + t47) * t53 * t61 +
t52 * (t21 -
t60 *
((t2 * t32 * 0.021F + t4 * (b_out_tmp + 0.0126F) * 0.021F) -
t44 * t47 * 2.0F) /
2.0F);
t28 = (t24 + t48) * t53 * t61 +
t52 * (t22 -
t60 *
((t3 * t32 * 0.021F + t5 * (b_out_tmp + 0.0126F) * 0.021F) -
t44 * t48 * 2.0F) /
2.0F);
t21 = t12_tmp + t71 * 0.105F;
out_tmp = t10_tmp + t70 * 0.105F;
t59 = t70 * t76;
t23 = t59 * t28;
t2 = t71 * t76;
t4 = t2 * t28;
t28 = -t10_tmp - t70 * 0.105F;
t60 = (t28 + 0.03F) * (t28 + 0.03F);
t61 = 1.0F / (t28 + 0.03F);
t48 = 1.0F / sqrtf(t21 * t21 + (out_tmp - 0.03F) * (out_tmp - 0.03F));
t52 = 1.0F / (t21 * t21 + t60);
t53 = t21 * (1.0F / t60);
t59 = t10_tmp - t59 * t47 * 0.21F;
t28 = t12_tmp - t2 * t47 * 0.21F;
spd[0] =
dphi4 * t48 * (t21 * t23 * 0.42F - (out_tmp - 0.03F) * t4 * 0.42F) /
2.0F +
dphi1 * t48 * (t21 * t59 * 2.0F - (out_tmp - 0.03F) * t28 * 2.0F) / 2.0F;
spd[1] =
dphi4 * t60 * t52 * (t61 * (0.0F - t23 * 0.21F) + t53 * (t4 * 0.21F)) -
dphi1 * t60 * t52 * (t61 * t59 - t53 * t28);
}
/*
* File trailer for leg_spd.c
*
* [EOF]
*/
MATLAB代码：
function spd = leg_spd(dphi1,dphi4,phi1,phi4)
%LEG_SPD
% SPD = LEG_SPD(DPHI1,DPHI4,PHI1,PHI4)
% This function was generated by the Symbolic Math Toolbox version 23.2.
% 2024-03-01 10:36:49
t2 = cos(phi1);
t3 = cos(phi4);
t4 = sin(phi1);
t5 = sin(phi4);
t6 = imag(t2);
t7 = real(t2);
t8 = imag(t4);
t9 = real(t4);
t10 = t2./2.0e+1;
t11 = t3./2.0e+1;
t12 = t4./2.0e+1;
t13 = t5./2.0e+1;
t21 = t2.*1.05e-2;
t22 = t3.*1.05e-2;
t23 = t4.*1.05e-2;
t24 = t5.*1.05e-2;
t14 = -t10;
t15 = -t13;
t16 = t6./2.0e+1;
t17 = t7./2.0e+1;
t18 = t8./2.0e+1;
t19 = t9./2.0e+1;
t25 = -t21;
t26 = -t23;
t27 = -t24;
t20 = -t17;
t28 = t12+t15;
t30 = t11+t14+3.0./5.0e+1;
t32 = t23+t27;
t38 = t22+t25+1.26e-2;
t29 = t28.^2;
t31 = t30.^2;
t33 = (t2.*t28)./1.0e+1;
t34 = (t3.*t28)./1.0e+1;
t35 = t32.^2;
t36 = (t4.*t30)./1.0e+1;
t37 = (t5.*t30)./1.0e+1;
t39 = t38.^2;
t40 = t2.*t32.*(2.1e+1./1.0e+3);
t41 = t3.*t32.*(2.1e+1./1.0e+3);
t42 = t4.*t38.*(2.1e+1./1.0e+3);
t43 = t5.*t38.*(2.1e+1./1.0e+3);
t44 = t29+t31;
t47 = t33+t36;
t48 = t34+t37;
t45 = t44.^2;
t49 = t23+t47;
t50 = t24+t48;
t51 = t38+t44;
t54 = t44.*t47.*2.0;
t55 = t44.*t48.*2.0;
t46 = -t45;
t52 = 1.0./t51;
t56 = -t54;
t57 = -t55;
t53 = t52.^2;
t58 = t35+t39+t46;
t63 = t40+t42+t56;
t64 = t41+t43+t57;
t59 = sqrt(t58);
t60 = 1.0./t59;
t61 = t24+t26+t59;
t62 = t61.^2;
t65 = t52.*t61;
t88 = t49.*t53.*t61;
t89 = t50.*t53.*t61;
t90 = (t60.*t63)./2.0;
t91 = (t60.*t64)./2.0;
t66 = atan(t65);
t67 = t53.*t62;
t92 = -t90;
t93 = -t91;
t68 = t66.*2.0;
t69 = t67+1.0;
t94 = t21+t92;
t95 = t22+t93;
t70 = cos(t68);
t71 = sin(t68);
t76 = 1.0./t69;
t96 = t52.*t94;
t97 = t52.*t95;
t72 = imag(t70);
t73 = real(t70);
t74 = imag(t71);
t75 = real(t71);
t77 = t70.*(2.1e+1./2.0e+2);
t78 = t71.*(2.1e+1./2.0e+2);
t106 = t88+t96;
t107 = t89+t97;
t79 = t72.*(2.1e+1./2.0e+2);
t80 = t73.*(2.1e+1./2.0e+2);
t81 = t74.*(2.1e+1./2.0e+2);
t82 = t75.*(2.1e+1./2.0e+2);
t84 = t12+t78;
t86 = t10+t77-3.0./1.0e+2;
t110 = t70.*t76.*t106;
t111 = t70.*t76.*t107;
t112 = t71.*t76.*t106;
t113 = t71.*t76.*t107;
t83 = -t80;
t85 = t84.^2;
t87 = t86.^2;
t98 = t16+t19+t79+t82;
t99 = t98.^2;
t100 = t18+t20+t81+t83+3.0./1.0e+2;
t101 = t85+t87;
t102 = t100.^2;
t103 = 1.0./t100;
t105 = 1.0./sqrt(t101);
t104 = 1.0./t102;
t108 = t99+t102;
t109 = 1.0./t108;
spd = [(dphi4.*t105.*(t84.*t111.*(2.1e+1./5.0e+1)-t86.*t113.*(2.1e+1./5.0e+1)))./2.0+(dphi1.*t105.*(t84.*(t10-t110.*(2.1e+1./1.0e+2)).*2.0-t86.*(t12-t112.*(2.1e+1./1.0e+2)).*2.0))./2.0;-dphi1.*t102.*t109.*(t103.*(t17-t18+imag(t112).*(2.1e+1./1.0e+2)-real(t110).*(2.1e+1./1.0e+2))-t98.*t104.*(t16+t19-imag(t110).*(2.1e+1./1.0e+2)-real(t112).*(2.1e+1./1.0e+2)))+dphi4.*t102.*t109.*(t103.*(imag(t113).*(2.1e+1./1.0e+2)-real(t111).*(2.1e+1./1.0e+2))+t98.*t104.*(imag(t111).*(2.1e+1./1.0e+2)+real(t113).*(2.1e+1./1.0e+2)))];
end
leg_conv
%可由虚拟腿目标扭矩和推力求得电机所需输出的力矩T[2]
extern void leg_conv(float F, float Tp, float phi1,
float phi4, float T[2]);
.h
/*
* File: leg_conv.h
*
* MATLAB Coder version : 5.5
* C/C++ source code generated on : 27-Jan-2023 14:46:28
*/
#ifndef LEG_CONV_H
#define LEG_CONV_H
/* Include Files */
#include <stdint.h>
#include <stddef.h>
#include <stdlib.h>
#ifdef __cplusplus
extern "C" {
#endif
/* Function Declarations */
extern void leg_conv(float F, float Tp, float phi1, float phi4, float T[2]);
#ifdef __cplusplus
}
#endif
#endif
/*
* File trailer for leg_conv.h
*
* [EOF]
*/
.c
/*
* File: leg_conv.c
*
* MATLAB Coder version : 5.5
* C/C++ source code generated on : 27-Jan-2023 14:46:28
*/
/* Include Files */
#include "leg_conv.h"
#include <math.h>
/* Function Definitions */
/*
* LEG_CONV
* T = LEG_CONV(F,TP,PHI1,PHI4)
*
* Arguments : float F
* float Tp
* float phi1
* float phi4
* float T[2]
* Return Type : void
*/
void leg_conv(float F, float Tp, float phi1, float phi4, float T[2])
{
float a;
float b_out;
float b_out_tmp;
float out;
float out_tmp;
float t10_tmp;
float t11;
float t12_tmp;
float t13_tmp;
float t14_tmp_tmp;
float t16;
float t17_tmp_tmp;
float t19;
float t21_tmp;
float t25;
float t26;
float t27_tmp;
float t28;
float t2_tmp;
float t30;
float t3_tmp;
float t42;
float t4_tmp;
float t50;
float t52;
float t53;
float t56_tmp;
float t57;
float t58;
float t59_tmp;
float t60;
float t63;
float t65;
float t69;
float t6_tmp;
float t72_tmp;
float t73;
float t77;
float t7_tmp;
float t8;
float t80_tmp;
float t81_tmp;
float t89;
float t9;
float t99;
/* This function was generated by the Symbolic Math Toolbox version 8.1.
*/
/* 17-Jan-2023 12:16:59 */
t3_tmp = cosf(phi1);
t4_tmp = cosf(phi4);
t12_tmp = t3_tmp * 0.0105F;
t13_tmp = t4_tmp * 0.0105F;
t2_tmp = -t12_tmp + t13_tmp;
t14_tmp_tmp = t3_tmp * 0.05F;
out_tmp = -t14_tmp_tmp + t4_tmp * 0.05F;
t6_tmp = sinf(phi1);
t7_tmp = sinf(phi4);
t17_tmp_tmp = t6_tmp * 0.05F;
t8 = t17_tmp_tmp - t7_tmp * 0.05F;
t16 = (out_tmp + 0.06F) * (out_tmp + 0.06F);
t19 = t8 * t8;
t9 = t16 + t19;
t10_tmp = t7_tmp * 0.0105F;
t21_tmp = t6_tmp * 0.0105F;
t11 = -t10_tmp + t21_tmp;
t25 = ((t2_tmp + 0.0126F) * (t2_tmp + 0.0126F) - t9 * t9) + t11 * t11;
t26 = sqrtf(t25);
t27_tmp = ((t2_tmp + t16) + t19) + 0.0126F;
t28 = 1.0F / t27_tmp;
a = t17_tmp_tmp +
sinf(atanf(t28 * ((t6_tmp * -0.0105F + t10_tmp) + t26)) * 2.0F) * 0.105F;
t30 = (t10_tmp - t21_tmp) + t26;
t16 = atanf(t28 * t30) * 2.0F;
t80_tmp = cosf(t16);
out = t14_tmp_tmp + t80_tmp * 0.105F;
t42 = t17_tmp_tmp - t7_tmp * 0.05F;
b_out_tmp = -t14_tmp_tmp + t4_tmp * 0.05F;
t19 = t42 * t42;
t50 = (b_out_tmp + 0.06F) * (b_out_tmp + 0.06F);
t52 = t6_tmp * (b_out_tmp + 0.06F) * 0.1F;
t53 = t3_tmp * t42 * 0.1F;
t56_tmp = -t12_tmp + t13_tmp;
t89 = ((t56_tmp + t19) + t50) + 0.0126F;
t57 = (-t21_tmp + t26) + t10_tmp;
t58 = 1.0F / (t89 * t89);
t59_tmp = sinf(t16);
t60 = 1.0F / t26;
t63 = t21_tmp - t10_tmp;
t65 = t19 + t50;
t69 = 1.0F / t89;
t72_tmp = t57 * t58;
t73 = (t12_tmp -
t60 *
((t6_tmp * (t56_tmp + 0.0126F) * 0.021F + t3_tmp * t63 * 0.021F) -
t65 * (t52 + t53) * 2.0F) *
0.5F) *
t69 +
t72_tmp * ((t21_tmp + t52) + t53);
t77 = 1.0F / (t58 * (t57 * t57) + 1.0F);
t81_tmp = t17_tmp_tmp + t59_tmp * 0.105F;
b_out = -t14_tmp_tmp - t80_tmp * 0.105F;
t19 = (out_tmp + 0.06F) * t6_tmp * 0.1F;
t89 = t3_tmp * t8 * 0.1F;
t16 = 1.0F / (t27_tmp * t27_tmp);
t26 = 1.0F / (t16 * (t30 * t30) + 1.0F);
t99 = 1.0F / sqrtf(t25);
t50 = t30 * t16;
t30 = t28 * (t12_tmp -
(((t2_tmp + 0.0126F) * t6_tmp * 0.021F + t3_tmp * t11 * 0.021F) -
t9 * (t19 + t89) * 2.0F) *
t99 * 0.5F) +
t50 * ((t21_tmp + t19) + t89);
t25 = t80_tmp * t26;
t27_tmp = t59_tmp * t26;
t57 = (b_out + 0.03F) * (b_out + 0.03F);
t58 = (out - 0.03F) * (out - 0.03F);
t19 = t7_tmp * (b_out_tmp + 0.06F) * 0.1F;
t16 = t42 * t4_tmp * 0.1F;
t52 = t69 * (t13_tmp - t60 *
((t7_tmp * (t56_tmp + 0.0126F) * 0.021F +
t4_tmp * t63 * 0.021F) -
t65 * (t19 + t16) * 2.0F) *
0.5F) +
t72_tmp * ((t10_tmp + t19) + t16);
t53 = t17_tmp_tmp + t59_tmp * 0.105F;
t19 = (out_tmp + 0.06F) * t7_tmp * 0.1F;
t16 = t4_tmp * t8 * 0.1F;
t89 = t28 * (t13_tmp - t99 *
(((t2_tmp + 0.0126F) * t7_tmp * 0.021F +
t4_tmp * t11 * 0.021F) -
t9 * (t19 + t16) * 2.0F) *
0.5F) +
t50 * ((t10_tmp + t19) + t16);
t26 = 1.0F / (b_out + 0.03F);
t50 = t81_tmp * (1.0F / t57);
t16 = Tp * t57 * (1.0F / (t57 + t81_tmp * t81_tmp));
t19 = (t14_tmp_tmp + t80_tmp * 0.105F) - 0.03F;
T[0] = F *
(t19 * (t17_tmp_tmp - t59_tmp * t73 * t77 * 0.21F) * 2.0F -
t53 * (t14_tmp_tmp - t73 * t77 * t80_tmp * 0.21F) * 2.0F) *
-0.5F / sqrtf(t58 + a * a) -
t16 * (t26 * (t14_tmp_tmp - t25 * t30 * 0.21F) -
t50 * (t17_tmp_tmp - t27_tmp * t30 * 0.21F));
T[1] =
F *
(t59_tmp * t77 * t19 * t52 * 0.42F -
t77 * t80_tmp * t52 * t53 * 0.42F) *
-0.5F / sqrtf(t58 + t81_tmp * t81_tmp) +
t16 * (t26 * (0.0F - t25 * t89 * 0.21F) + t50 * (t27_tmp * t89 * 0.21F));
}
/*
* File trailer for leg_conv.c
*
* [EOF]
*/
PID
.h
#ifndef _USER_PID_H_
#define _USER_PID_H_
#ifdef __cplusplus
extern "C" {
#endif
#include "stdint.h"
#define LIMIT(x,min,max) (x)=(((x)<=(min))?(min):(((x)>=(max))?(max):(x)))
#ifndef ABS
#define ABS(x) ((x)>=0?(x):-(x))
#endif
//PID参数结构体
typedef struct _PID
{
float kp,ki,kd;
float error,lastError;
float integral,maxIntegral;
float output,maxOutput;
float deadzone;
float errLpfRatio;
}PID;
//串级PID参数结构体
typedef struct _CascadePID
{
PID inner;
PID outer;
float output;
}CascadePID;
void PID_Init(PID *pid,float p,float i,float d,float maxSum,float maxOut);
void PID_SingleCalc(PID *pid,float reference,float feedback);
void PID_CascadeCalc(CascadePID *pid,float angleRef,float angleFdb,float speedFdb);
void PID_Clear(PID *pid);
void PID_SetMaxOutput(PID *pid,float maxOut);
void PID_SetDeadzone(PID *pid,float deadzone);
void PID_SetErrLpfRatio(PID *pid,float ratio);
#ifdef __cplusplus
}
#endif
#endif
.c
/****************PID运算****************/
#include "./PID.h"
//初始化pid参数
void PID_Init(PID *pid,float p,float i,float d,float maxI,float maxOut)
{
pid->kp=p;
pid->ki=i;
pid->kd=d;
pid->maxIntegral=maxI;
pid->maxOutput=maxOut;
pid->deadzone=0;
pid->errLpfRatio=1;
}
//单级pid计算
void PID_SingleCalc(PID *pid,float reference,float feedback)
{
//更新数据
pid->lastError=pid->error;
if(ABS(reference-feedback) < pid->deadzone)//若误差在死区内则error直接置0
pid->error=0;
else
pid->error=reference-feedback;
//低通滤波
pid->error=pid->error*pid->errLpfRatio+pid->lastError*(1-pid->errLpfRatio);
//计算微分
pid->output=(pid->error-pid->lastError)*pid->kd;
//计算比例
pid->output+=pid->error*pid->kp;
//计算积分
pid->integral+=pid->error*pid->ki;
LIMIT(pid->integral,-pid->maxIntegral,pid->maxIntegral);//积分限幅
pid->output+=pid->integral;
//输出限幅
LIMIT(pid->output,-pid->maxOutput,pid->maxOutput);
}
//串级pid计算
void PID_CascadeCalc(CascadePID *pid,float angleRef,float angleFdb,float speedFdb)
{
PID_SingleCalc(&pid->outer,angleRef,angleFdb);//计算外环(角度环)
PID_SingleCalc(&pid->inner,pid->outer.output,speedFdb);//计算内环(速度环)
pid->output=pid->inner.output;
}
//清空一个pid的历史数据
void PID_Clear(PID *pid)
{
pid->error=0;
pid->lastError=0;
pid->integral=0;
pid->output=0;
}
//重新设定pid输出限幅
void PID_SetMaxOutput(PID *pid,float maxOut)
{
pid->maxOutput=maxOut;
}
//设置PID死区
void PID_SetDeadzone(PID *pid,float deadzone)
{
pid->deadzone=deadzone;
}
//设置LPF比例
void PID_SetErrLpfRatio(PID *pid,float ratio)
{
pid->errLpfRatio=ratio;
}
![反序列化逃逸 [安洵杯 2019]easy_serialize_php1](https://img-blog.csdnimg.cn/direct/5af1d13f81a04da19418135d0a693818.png)
