优化跳跃功能，可以支持连续跳跃，不倒地。
This commit is contained in:
myperworld committed
1 parent
9f73afb56b
commit
5a0bbef43b
5 files changed
+165
-1631
No files matched your search
@@ -10,6 +10,9 @@
|
|||||||
2025-08-16
|
2025-08-16
|
||||||
对几种运动中倒地的极限状态进行优化，实现自平衡起身。
|
对几种运动中倒地的极限状态进行优化，实现自平衡起身。
|
||||||
|
|
||||||
|
2025-08-17
|
||||||
|
优化跳跃功能，可以支持连续跳跃，不倒地。
|
||||||
|
|
||||||
世博开源轮足优化版-TOMMY版本项目地址：
|
世博开源轮足优化版-TOMMY版本项目地址：
|
||||||
https://gitee.com/myperworld/wrobot
|
https://gitee.com/myperworld/wrobot
|
||||||
|
|
||||||
|
|||||||
@@ -138,6 +138,15 @@ bool lastbuttonLB = false;
|
|||||||
void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
|
void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
|
||||||
memcpy(&remoteData, incomingData, sizeof(remoteData));
|
memcpy(&remoteData, incomingData, sizeof(remoteData));
|
||||||
|
|
||||||
|
// 跳跃状态不接受指令
|
||||||
|
if(jump_flag > 0){
|
||||||
|
wrobot.dir = 4; // 不跳跃
|
||||||
|
wrobot.joyx = 0;
|
||||||
|
wrobot.joyy = 0;
|
||||||
|
wrobot.roll = 0;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
// 摇杆输出范围0-255，转换到 -100 到 100
|
// 摇杆输出范围0-255，转换到 -100 到 100
|
||||||
// roll 左右摇摆
|
// roll 左右摇摆
|
||||||
int convertedJ1PotX = (remoteData.j1PotX - 127.5) * (30.0 / 127.5); // 转到 -30 到 30
|
int convertedJ1PotX = (remoteData.j1PotX - 127.5) * (30.0 / 127.5); // 转到 -30 到 30
|
||||||
@@ -151,6 +160,8 @@ void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
|
|||||||
// 取反按钮值，使得默认显示 0 false，按下显示 1 true
|
// 取反按钮值，使得默认显示 0 false，按下显示 1 true
|
||||||
int convertedbuttonLB = !remoteData.buttonLB; // 顶部左
|
int convertedbuttonLB = !remoteData.buttonLB; // 顶部左
|
||||||
int convertedbuttonRB = !remoteData.buttonRB; // 顶部右
|
int convertedbuttonRB = !remoteData.buttonRB; // 顶部右
|
||||||
|
// int convertedbuttonR1 = !remoteData.buttonR1; // 底部右
|
||||||
|
// int convertedbuttonR2 = !remoteData.buttonR2; // 底部左
|
||||||
|
|
||||||
// 跳跃
|
// 跳跃
|
||||||
if (convertedbuttonRB) { // 1=true，0=false
|
if (convertedbuttonRB) { // 1=true，0=false
|
||||||
@@ -158,6 +169,7 @@ void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
|
|||||||
}else{
|
}else{
|
||||||
wrobot.dir = 4; // 不跳跃
|
wrobot.dir = 4; // 不跳跃
|
||||||
}
|
}
|
||||||
|
// Serial.print(convertedbuttonRB);
|
||||||
|
|
||||||
// led蓝灯爆闪
|
// led蓝灯爆闪
|
||||||
if (lastbuttonLB && !convertedbuttonLB) {
|
if (lastbuttonLB && !convertedbuttonLB) {
|
||||||
@@ -295,43 +307,31 @@ void loop()
|
|||||||
mpu6050.update(); // IMU数据更新
|
mpu6050.update(); // IMU数据更新
|
||||||
lqr_balance_loop(); // lqr自平衡控制，更新LQR_u
|
lqr_balance_loop(); // lqr自平衡控制，更新LQR_u
|
||||||
yaw_loop(); // yaw轴转向控制，更新YAW_output
|
yaw_loop(); // yaw轴转向控制，更新YAW_output
|
||||||
leg_loop(); // 腿部动作控制，控制舵机SyncWritePosEx，包括判断jump_loop
|
leg_loop(); // 腿部动作控制，控制舵机SyncWritePosEx
|
||||||
|
|
||||||
// 记录上一次的遥控数据数据
|
// 记录上一次的遥控数据数据
|
||||||
wrobot.dir_last = wrobot.dir;
|
wrobot.dir_last = wrobot.dir;
|
||||||
wrobot.joyx_last = wrobot.joyx;
|
wrobot.joyx_last = wrobot.joyx;
|
||||||
wrobot.joyy_last = wrobot.joyy;
|
wrobot.joyy_last = wrobot.joyy;
|
||||||
|
|
||||||
/* 将自平衡计算输出转矩赋给电机
|
// 将自平衡计算输出转矩赋给电机
|
||||||
motor.target 负数小车向前转，正数小车向后转，
|
// motor.target 负数小车向前转，正数小车向后转，
|
||||||
LQR_u 越大，电机输出的转矩越大，电机向后转，默认0-8V
|
|
||||||
LQR_u = angle_control + gyro_control + distance_control + speed_control
|
|
||||||
|
|
||||||
angle_control = pid_angle(LQR_angle - angle_zeropoint) //LQR_u与实际LQR_angle正相关
|
|
||||||
= pid_angle( mpu6050.getAngleY() - angle_zeropoint) //LQR_u与实际pitch正相关
|
|
||||||
gyro_control = pid_gyro(LQR_gyro) //LQR_u与实际LQR_gyro正相关
|
|
||||||
= pid_gyro( mpu6050.getGyroY() ) //LQR_u与实际pitch 角速度 正相关
|
|
||||||
|
|
||||||
所以这里LQR_distance和LQR_speed的计算乘以-0.5,不能单看负号
|
|
||||||
distance_control = pid_distance(LQR_distance - distance_zeropoint) //LQR_u与电LQR_distance正相关
|
|
||||||
= pid_distance( (-0.5)*(motor1.shaft_angle + motor2.shaft_angle) - distance_zeropoint)
|
|
||||||
//在平衡车中，通常设定电机向后转（使车前进）为负，所以LQR_u与电机实际shaft_angle角度正相关，和负号没关系
|
|
||||||
|
|
||||||
speed_control = pid_speed(LQR_speed - 0.1 * lpf_joyy(wrobot.joyy)) // 实际速度-目标速度
|
|
||||||
//LQR_speed代表实际速度，LQR_u与实际LQR_speed速度正相关
|
|
||||||
= pid_speed( (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity) - 0.1 * lpf_joyy(wrobot.joyy))
|
|
||||||
//LQR_u与实际shaft_velocity速度正相关
|
|
||||||
*/
|
|
||||||
motor1.target = (-0.5) * (LQR_u + YAW_output); // 负数小车向前，正数小车向后，取
|
motor1.target = (-0.5) * (LQR_u + YAW_output); // 负数小车向前，正数小车向后，取
|
||||||
motor2.target = (-0.5) * (LQR_u - YAW_output);
|
motor2.target = (-0.5) * (LQR_u - YAW_output);
|
||||||
|
|
||||||
// 查看角度信息, 屁股向后倒 -33~-34度,舵机2048的时候-42度，头向前到底 75度，完全垂直88度
|
// 查看角度信息, 屁股向后倒 -33~-34度,舵机2048的时候-42度，头向前到底 75度，完全垂直88度
|
||||||
// Serial.println(LQR_angle);
|
// Serial.println(LQR_angle);
|
||||||
|
|
||||||
|
// 检测跳跃状态
|
||||||
|
// if(jump_flag>0){
|
||||||
|
// Serial.println(jump_flag);
|
||||||
|
// }
|
||||||
|
|
||||||
|
|
||||||
// 检测小车是否向后马上要倒地了，在中间过程重置所有参数
|
// 检测小车是否向后马上要倒地了，在中间过程重置所有参数
|
||||||
loopCounter++;
|
loopCounter++;
|
||||||
// loopCounter作用：在期初的800次循环内视为电源开通后第一次自平衡，不能进入resetToDefault()函数重置参数
|
// loopCounter作用：在刚开机的800次loop循环，不能进入resetToDefault()函数
|
||||||
// resetToDefaultCounterz作用：防止小车倒地后某个角度，陷入循环，motor.target一直=0
|
// resetToDefaultCounter作用：防止小车倒地后某个角度，陷入循环，motor.target一直等于0，就躺地上不动了
|
||||||
if ((LQR_angle < -35.0f || LQR_angle > 70.0f )&& loopCounter > 800 && resetToDefaultCounter < 130) {
|
if ((LQR_angle < -35.0f || LQR_angle > 70.0f )&& loopCounter > 800 && resetToDefaultCounter < 130) {
|
||||||
resetToDefault();
|
resetToDefault();
|
||||||
resetToDefaultCounter++;
|
resetToDefaultCounter++;
|
||||||
@@ -359,6 +359,9 @@ void loop()
|
|||||||
// 设置轮部电机输出
|
// 设置轮部电机输出
|
||||||
motor1.move();
|
motor1.move();
|
||||||
motor2.move();
|
motor2.move();
|
||||||
|
|
||||||
|
jump_loop(); // 跳跃控制
|
||||||
|
|
||||||
} // loop()
|
} // loop()
|
||||||
|
|
||||||
// lqr自平衡控制
|
// lqr自平衡控制
|
||||||
@@ -376,9 +379,31 @@ void lqr_balance_loop()
|
|||||||
// + LQR_k4*LQR_speed;
|
// + LQR_k4*LQR_speed;
|
||||||
|
|
||||||
// 给负值是因为按照当前的电机接线，正转矩驱动小车电机向后转
|
// 给负值是因为按照当前的电机接线，正转矩驱动小车电机向后转
|
||||||
// 编码器反馈shaft_angle角度，shaft_velocity速度
|
|
||||||
// 不操作前后移动，譬如刚开机，车轮不动的，shaft_angle和shaft_velocity 都是零
|
/*
|
||||||
// 所以，不操作前后移动，LQR_distance 和 LQR_speed趋近于0，只有角度控制和角速度控制
|
LQR_u 越大，电机输出的转矩越大，电机向后转，默认0-8V
|
||||||
|
LQR_u = angle_control + gyro_control + distance_control + speed_control
|
||||||
|
|
||||||
|
编码器反馈shaft_angle角度，shaft_velocity速度
|
||||||
|
不操作前后移动，譬如刚开机，车轮不动的，shaft_angle和shaft_velocity 都是零
|
||||||
|
所以，不操作前后移动，LQR_distance 和 LQR_speed趋近于0，只有角度控制和角速度控制
|
||||||
|
|
||||||
|
angle_control = pid_angle(LQR_angle - angle_zeropoint) //LQR_u与实际LQR_angle正相关
|
||||||
|
= pid_angle( mpu6050.getAngleY() - angle_zeropoint) //LQR_u与实际pitch正相关
|
||||||
|
gyro_control = pid_gyro(LQR_gyro) //LQR_u与实际LQR_gyro正相关
|
||||||
|
= pid_gyro( mpu6050.getGyroY() ) //LQR_u与实际pitch 角速度 正相关
|
||||||
|
|
||||||
|
所以这里LQR_distance和LQR_speed的计算乘以-0.5,不能单看负号
|
||||||
|
distance_control = pid_distance(LQR_distance - distance_zeropoint) //LQR_u与电LQR_distance正相关
|
||||||
|
= pid_distance( (-0.5)*(motor1.shaft_angle + motor2.shaft_angle) - distance_zeropoint)
|
||||||
|
//在平衡车中，通常设定电机向后转（使车前进）为负，所以LQR_u与电机实际shaft_angle角度正相关，和负号没关系
|
||||||
|
|
||||||
|
speed_control = pid_speed(LQR_speed - 0.1 * lpf_joyy(wrobot.joyy)) // 实际速度-目标速度
|
||||||
|
//LQR_speed代表实际速度，LQR_u与实际LQR_speed速度正相关
|
||||||
|
= pid_speed( (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity) - 0.1 * lpf_joyy(wrobot.joyy))
|
||||||
|
//LQR_u与实际shaft_velocity速度正相关
|
||||||
|
*/
|
||||||
|
|
||||||
// 在平衡车中，通常设定电机向后转（使车前进）为负，所以这里LQR_distance和LQR_speed的计算乘以-0.5
|
// 在平衡车中，通常设定电机向后转（使车前进）为负，所以这里LQR_distance和LQR_speed的计算乘以-0.5
|
||||||
LQR_distance = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle); // 两个电机的旋转角度（shaft_angle）（是位置量）的平均值
|
LQR_distance = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle); // 两个电机的旋转角度（shaft_angle）（是位置量）的平均值
|
||||||
LQR_speed = (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity); // 两个电机角速度（shaft_velocity）的平均值，并取反
|
LQR_speed = (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity); // 两个电机角速度（shaft_velocity）的平均值，并取反
|
||||||
@@ -490,8 +515,6 @@ void leg_loop()
|
|||||||
return; // 跳过后续控制
|
return; // 跳过后续控制
|
||||||
}
|
}
|
||||||
|
|
||||||
jump_loop();
|
|
||||||
|
|
||||||
if (jump_flag == 0) // 不处于跳跃状态
|
if (jump_flag == 0) // 不处于跳跃状态
|
||||||
{
|
{
|
||||||
// 舵机加速度
|
// 舵机加速度
|
||||||
@@ -566,16 +589,8 @@ void leg_loop()
|
|||||||
void jump_loop()
|
void jump_loop()
|
||||||
{ // dir: 4 = STOP, 5 = jump 上次动作是跳跃5，本次是停止4，才开始跳，因为可能连续发送了很多5跳跃指令，只执行最后一次
|
{ // dir: 4 = STOP, 5 = jump 上次动作是跳跃5，本次是停止4，才开始跳，因为可能连续发送了很多5跳跃指令，只执行最后一次
|
||||||
// 跳跃，腿部以最快速度伸到最长
|
// 跳跃，腿部以最快速度伸到最长
|
||||||
if ((wrobot.dir_last == 5) && (wrobot.dir == 4) && (jump_flag == 0))
|
if ((wrobot.dir == 5) && (jump_flag == 0))
|
||||||
{
|
{
|
||||||
// 设定最大正向电压（快速加速）
|
|
||||||
// motor1.target = -8.0f;
|
|
||||||
// motor2.target = -8.0f;
|
|
||||||
// motor1.loopFOC();
|
|
||||||
// motor2.loopFOC();
|
|
||||||
// motor1.move();
|
|
||||||
// motor2.move();
|
|
||||||
// delay(50); // 缩短加速时间（50ms足够获得初速度）
|
|
||||||
|
|
||||||
// 跳跃开始
|
// 跳跃开始
|
||||||
ACC[0] = 0;
|
ACC[0] = 0;
|
||||||
@@ -593,19 +608,16 @@ void jump_loop()
|
|||||||
{
|
{
|
||||||
jump_flag++;
|
jump_flag++;
|
||||||
|
|
||||||
// if (jump_flag == 25) { // 跳跃上升阶段停止轮子转动
|
|
||||||
// motor1.target = 0;
|
|
||||||
// motor2.target = 0;
|
|
||||||
// }
|
|
||||||
|
|
||||||
if ((jump_flag > 30) && (jump_flag < 35))
|
if ((jump_flag > 30) && (jump_flag < 35))
|
||||||
{
|
{
|
||||||
ACC[0] = 0;
|
ACC[0] = 0;
|
||||||
ACC[1] = 0;
|
ACC[1] = 0;
|
||||||
Speed[0] = 0;
|
Speed[0] = 0;
|
||||||
Speed[1] = 0;
|
Speed[1] = 0;
|
||||||
Position[0] = 2048 + 12 + 8.4 * (35 - 32); // 降低腿部高度，即车重心
|
// Position[0] = 2048 + 12 + 8.4 * (35 - 32); // 降低腿部高度，即车重心
|
||||||
Position[1] = 2048 - 12 - 8.4 * (35 - 32);
|
// Position[1] = 2048 - 12 - 8.4 * (35 - 32);
|
||||||
|
Position[0] = SERVO1_MIN + 150; // 收缩腿
|
||||||
|
Position[1] = SERVO2_MIN - 150;
|
||||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||||
|
|
||||||
jump_flag = 40;
|
jump_flag = 40;
|
||||||
|
|||||||
@@ -1,760 +0,0 @@
|
|||||||
// -----------------------------------------------------------------------------
|
|
||||||
// Copyright (c) 2024 Mu Shibo
|
|
||||||
// Modify by Tommy 2025.8.11
|
|
||||||
// -----------------------------------------------------------------------------
|
|
||||||
|
|
||||||
#include <esp_now.h>
|
|
||||||
#include <WiFi.h>
|
|
||||||
#include "main.h"
|
|
||||||
|
|
||||||
/************实例定义*************/
|
|
||||||
|
|
||||||
// 电机实例
|
|
||||||
BLDCMotor motor1 = BLDCMotor(7);
|
|
||||||
BLDCMotor motor2 = BLDCMotor(7);
|
|
||||||
BLDCDriver3PWM driver1 = BLDCDriver3PWM(32, 33, 25, 22);
|
|
||||||
BLDCDriver3PWM driver2 = BLDCDriver3PWM(26, 27, 14, 12);
|
|
||||||
|
|
||||||
// 编码器实例
|
|
||||||
TwoWire I2Cone = TwoWire(0);
|
|
||||||
TwoWire I2Ctwo = TwoWire(1);
|
|
||||||
MagneticSensorI2C sensor1 = MagneticSensorI2C(AS5600_I2C);
|
|
||||||
MagneticSensorI2C sensor2 = MagneticSensorI2C(AS5600_I2C);
|
|
||||||
|
|
||||||
// PID控制器实例
|
|
||||||
PIDController pid_angle(1, 0, 0, 100000, 8); // pitch角度PID使其趋于零点
|
|
||||||
PIDController pid_gyro(0.06, 0, 0, 100000, 8); // 角速度PID使其趋于零点
|
|
||||||
PIDController pid_distance(0.5, 0, 0, 100000, 8);// 位移PID使其趋于零点
|
|
||||||
PIDController pid_speed(0.7, 0, 0, 100000, 8);// 速度PID使其趋于目标速度
|
|
||||||
PIDController pid_yaw_angle(1.0, 0, 0, 100000, 8);
|
|
||||||
PIDController pid_yaw_gyro(0.04, 0, 0, 100000, 8);
|
|
||||||
PIDController pid_lqr_u(1, 15, 0, 100000, 8);
|
|
||||||
PIDController pid_zeropoint(0.002, 0, 0, 100000, 4);
|
|
||||||
PIDController pid_roll_angle(8, 0, 0, 100000, 450);
|
|
||||||
|
|
||||||
// 低通滤波器实例
|
|
||||||
LowPassFilter lpf_joyy(0.2);
|
|
||||||
LowPassFilter lpf_zeropoint(0.1);
|
|
||||||
LowPassFilter lpf_roll(0.3);
|
|
||||||
|
|
||||||
// WebServer实例
|
|
||||||
WebServer webserver; // server服务器
|
|
||||||
WebSocketsServer websocket = WebSocketsServer(81); // 定义一个webSocket服务器来处理客户发送的消息
|
|
||||||
RobotProtocol rp(20);
|
|
||||||
|
|
||||||
// STS舵机实例
|
|
||||||
SMS_STS sms_sts;
|
|
||||||
|
|
||||||
// MPU6050实例
|
|
||||||
MPU6050 mpu6050(I2Ctwo);
|
|
||||||
|
|
||||||
/************参数定义*************/
|
|
||||||
#define pi 3.1415927
|
|
||||||
|
|
||||||
// 舵机位置限制参数
|
|
||||||
#define SERVO1_MIN 1900 // 舵机1最低 从小车正面看右手
|
|
||||||
#define SERVO1_MAX 2400 // 舵机1最高
|
|
||||||
#define SERVO2_MIN 2300 // 舵机2最低 从小车正面看左手(注意大小值与舵机1是相反的)
|
|
||||||
#define SERVO2_MAX 1800 // 舵机2最高
|
|
||||||
|
|
||||||
#define LEG_HEIGHT_BASE 32 // 腿部默认高度基准值
|
|
||||||
|
|
||||||
// LQR自平衡控制器参数
|
|
||||||
float LQR_angle = 0;
|
|
||||||
float LQR_gyro = 0;
|
|
||||||
float LQR_speed = 0;
|
|
||||||
float LQR_distance = 0;
|
|
||||||
float angle_control = 0;
|
|
||||||
float gyro_control = 0;
|
|
||||||
float speed_control = 0;
|
|
||||||
float distance_control = 0;
|
|
||||||
float LQR_u = 0;
|
|
||||||
float angle_zeropoint = 5; // 设定默认俯仰角度，向前到正，向后倒负
|
|
||||||
float distance_zeropoint = 1; // 轮部位移零点偏置
|
|
||||||
|
|
||||||
// YAW轴控制数据
|
|
||||||
float YAW_gyro = 0;
|
|
||||||
float YAW_angle = 0;
|
|
||||||
float YAW_angle_last = 0;
|
|
||||||
float YAW_angle_total = 0;
|
|
||||||
float YAW_angle_zero_point = -10;
|
|
||||||
float YAW_output = 0;
|
|
||||||
|
|
||||||
|
|
||||||
// 腿部舵机控制数据
|
|
||||||
byte ID[2];
|
|
||||||
s16 Position[2];
|
|
||||||
u16 Speed[2];
|
|
||||||
byte ACC[2];
|
|
||||||
|
|
||||||
// 逻辑处理标志位
|
|
||||||
float robot_speed = 0; // 记录当前轮部转速
|
|
||||||
float robot_speed_last = 0; // 记录上一时刻的轮部转速
|
|
||||||
int wrobot_move_stop_flag = 0; // 记录摇杆停止的标识
|
|
||||||
int jump_flag = 0; // 跳跃时段标识
|
|
||||||
float leg_position_add = 0; // roll轴平衡控制量
|
|
||||||
|
|
||||||
// 电压检测
|
|
||||||
uint16_t bat_check_num = 0;
|
|
||||||
int BAT_PIN = 35; // select the input pin for the ADC
|
|
||||||
static esp_adc_cal_characteristics_t adc_chars;
|
|
||||||
static const adc1_channel_t channel = ADC1_CHANNEL_7;
|
|
||||||
static const adc_bits_width_t width = ADC_WIDTH_BIT_12;
|
|
||||||
static const adc_atten_t atten = ADC_ATTEN_DB_11;
|
|
||||||
static const adc_unit_t unit = ADC_UNIT_1;
|
|
||||||
|
|
||||||
// 电量显示LED
|
|
||||||
#define LED_BAT 13
|
|
||||||
|
|
||||||
// 定义摇杆数值死区参数
|
|
||||||
const int deadZone = 15;
|
|
||||||
|
|
||||||
typedef struct {
|
|
||||||
byte j1PotX; // 左摇杆X (0-255)
|
|
||||||
byte j1PotY; // 左摇杆Y (0-255)
|
|
||||||
byte j2PotX; // 右摇杆X (0-255)
|
|
||||||
byte j2PotY; // 右摇杆Y (0-255)
|
|
||||||
bool buttonR1; //底部右按键 1：true （默认），0：false
|
|
||||||
bool buttonR2; //底部左按键 1：true （默认），0：false
|
|
||||||
bool j1Button; //左摇杆按键 1：true （默认），0：false
|
|
||||||
bool j2Button; //右摇杆按键 1：true （默认），0：false
|
|
||||||
bool buttonLB; //顶部左按键 1：true （默认），0：false
|
|
||||||
bool buttonRB; //顶部右按键 1：true （默认），0：false
|
|
||||||
bool tSwitch1; //拨动开关 1：true （默认），0：false
|
|
||||||
byte roll; //用于M5StackCore2或未来增加的陀螺仪功能
|
|
||||||
byte pitch;
|
|
||||||
bool buttonR3;
|
|
||||||
} JoystickData;
|
|
||||||
|
|
||||||
JoystickData remoteData;
|
|
||||||
|
|
||||||
// 蓝灯爆闪
|
|
||||||
bool lastbuttonLB = false;
|
|
||||||
|
|
||||||
// ESP-NOW接收回调函数
|
|
||||||
void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
|
|
||||||
memcpy(&remoteData, incomingData, sizeof(remoteData));
|
|
||||||
|
|
||||||
// 摇杆输出范围0-255，转换到 -100 到 100
|
|
||||||
// roll 左右摇摆
|
|
||||||
int convertedJ1PotX = (remoteData.j1PotX - 127.5) * (30.0 / 127.5); // 转到 -30 到 30
|
|
||||||
// 上下高度
|
|
||||||
int convertedJ1PotY = static_cast<int>((1 - remoteData.j1PotY / 255.0) * (60 - 0) + 0); // 转到 60 到 0 y轴取反，这样可以使机身下降
|
|
||||||
// 左右旋转
|
|
||||||
int convertedJ2PotX = (remoteData.j2PotX - 127.5) * (100.0 / 127.5); // 转到 -100 到 100
|
|
||||||
// 前后速度
|
|
||||||
int convertedJ2PotY = (remoteData.j2PotY - 127.5) * (100.0 / 127.5) * -1 * 1.4; // 转到 -100 到 100 y轴取反,并且放大洗漱
|
|
||||||
|
|
||||||
// 取反按钮值，使得默认显示 0 false，按下显示 1 true
|
|
||||||
int convertedbuttonLB = !remoteData.buttonLB; // 顶部左
|
|
||||||
int convertedbuttonRB = !remoteData.buttonRB; // 顶部右
|
|
||||||
|
|
||||||
// 跳跃
|
|
||||||
if (convertedbuttonRB) { // 1=true，0=false
|
|
||||||
wrobot.dir = 5; // 跳跃
|
|
||||||
}else{
|
|
||||||
wrobot.dir = 4; // 不跳跃
|
|
||||||
}
|
|
||||||
|
|
||||||
// led蓝灯爆闪
|
|
||||||
if (lastbuttonLB && !convertedbuttonLB) {
|
|
||||||
blinkLed(); // 不能写在loop中，会阻断legloop，小车会倒下
|
|
||||||
}
|
|
||||||
// 更新上次 lastbuttonRB 的值
|
|
||||||
lastbuttonLB = convertedbuttonLB;
|
|
||||||
|
|
||||||
if(abs(convertedJ1PotY) > deadZone) { // 上下高度 15 ~ 75
|
|
||||||
wrobot.height = convertedJ1PotY;
|
|
||||||
}else{
|
|
||||||
wrobot.height = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
if(abs(convertedJ1PotX) > deadZone) { // 左右摇摆 -30 ~ 30
|
|
||||||
wrobot.roll = convertedJ1PotX;
|
|
||||||
}else{
|
|
||||||
wrobot.roll = 0;
|
|
||||||
}
|
|
||||||
if(abs(convertedJ2PotY) > deadZone) { // 前后运动 -100 到 100
|
|
||||||
wrobot.joyy = convertedJ2PotY;
|
|
||||||
}else{
|
|
||||||
wrobot.joyy = 0;
|
|
||||||
}
|
|
||||||
if(abs(convertedJ2PotX) > deadZone) { // 左右运动 -100 到 100
|
|
||||||
wrobot.joyx = convertedJ2PotX;
|
|
||||||
} else {
|
|
||||||
wrobot.joyx = 0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void setup()
|
|
||||||
{
|
|
||||||
Serial.begin(115200); // 通讯串口
|
|
||||||
Serial2.begin(1000000); // 腿部sts舵机
|
|
||||||
|
|
||||||
// Wifi初始化
|
|
||||||
WiFi_SetAP();
|
|
||||||
webserver.begin();
|
|
||||||
webserver.on("/", HTTP_GET, basicWebCallback);
|
|
||||||
websocket.begin();
|
|
||||||
websocket.onEvent(webSocketEventCallback);
|
|
||||||
|
|
||||||
// 初始化ESP-NOW
|
|
||||||
WiFi.mode(WIFI_STA); // 打开这行注释就能开启sta模式，同时默认关闭ap热点模式，省电+稳定，而且不会影响espnow通信和整体代码框架
|
|
||||||
if (esp_now_init() != ESP_OK) {
|
|
||||||
Serial.println("Error initializing ESP-NOW");
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
esp_now_register_recv_cb(OnDataRecv);
|
|
||||||
|
|
||||||
// 舵机初始化
|
|
||||||
sms_sts.pSerial = &Serial2;
|
|
||||||
ID[0] = 1;
|
|
||||||
ID[1] = 2;
|
|
||||||
ACC[0] = 30;
|
|
||||||
ACC[1] = 30;
|
|
||||||
Speed[0] = 400;
|
|
||||||
Speed[1] = 400;
|
|
||||||
Position[0] = 2048;
|
|
||||||
Position[1] = 2048;
|
|
||||||
// 舵机(ID1/ID2)以最高速度V=2400步/秒，加速度A=50(50*100步/秒^2)，运行至各自的Position位置
|
|
||||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
|
||||||
|
|
||||||
// 电压检测
|
|
||||||
adc_calibration_init();
|
|
||||||
adc1_config_width(width);
|
|
||||||
adc1_config_channel_atten(channel, atten);
|
|
||||||
esp_adc_cal_characterize(unit, atten, width, 0, &adc_chars);
|
|
||||||
|
|
||||||
// 电量显示LED
|
|
||||||
pinMode(LED_BAT, OUTPUT);
|
|
||||||
|
|
||||||
// 编码器设置
|
|
||||||
I2Cone.begin(19, 18, 400000UL);
|
|
||||||
I2Ctwo.begin(23, 5, 400000UL);
|
|
||||||
sensor1.init(&I2Cone);
|
|
||||||
sensor2.init(&I2Ctwo);
|
|
||||||
|
|
||||||
// mpu6050设置
|
|
||||||
mpu6050.begin();
|
|
||||||
mpu6050.calcGyroOffsets(true);
|
|
||||||
|
|
||||||
// 连接motor对象与编码器对象
|
|
||||||
motor1.linkSensor(&sensor1);
|
|
||||||
motor2.linkSensor(&sensor2);
|
|
||||||
|
|
||||||
// 速度环PID参数
|
|
||||||
motor1.PID_velocity.P = 0.05;
|
|
||||||
motor1.PID_velocity.I = 1;
|
|
||||||
motor1.PID_velocity.D = 0;
|
|
||||||
|
|
||||||
motor2.PID_velocity.P = 0.05;
|
|
||||||
motor2.PID_velocity.I = 1;
|
|
||||||
motor2.PID_velocity.D = 0;
|
|
||||||
|
|
||||||
// 驱动器设置
|
|
||||||
motor1.voltage_sensor_align = 6;
|
|
||||||
motor2.voltage_sensor_align = 6;
|
|
||||||
driver1.voltage_power_supply = 8;
|
|
||||||
driver2.voltage_power_supply = 8;
|
|
||||||
driver1.init();
|
|
||||||
driver2.init();
|
|
||||||
|
|
||||||
// 连接motor对象与驱动器对象
|
|
||||||
motor1.linkDriver(&driver1);
|
|
||||||
motor2.linkDriver(&driver2);
|
|
||||||
|
|
||||||
motor1.torque_controller = TorqueControlType::voltage; // 扭矩控制器类型为 "电压模式"
|
|
||||||
motor2.torque_controller = TorqueControlType::voltage;
|
|
||||||
motor1.controller = MotionControlType::torque; // 运动控制器类型为 "扭矩模式"
|
|
||||||
motor2.controller = MotionControlType::torque;
|
|
||||||
|
|
||||||
|
|
||||||
// monitor相关设置
|
|
||||||
motor1.useMonitoring(Serial);
|
|
||||||
motor2.useMonitoring(Serial);
|
|
||||||
// 电机初始化
|
|
||||||
motor1.init();
|
|
||||||
motor1.initFOC();
|
|
||||||
motor2.init();
|
|
||||||
motor2.initFOC();
|
|
||||||
|
|
||||||
resetToDefault(); // 重置到默认参数状态
|
|
||||||
delay(500);
|
|
||||||
}
|
|
||||||
|
|
||||||
void loop()
|
|
||||||
{
|
|
||||||
|
|
||||||
bat_check(); // 电压检测
|
|
||||||
web_loop(); // Web数据更新
|
|
||||||
mpu6050.update(); // IMU数据更新
|
|
||||||
lqr_balance_loop(); // lqr自平衡控制，更新LQR_u
|
|
||||||
yaw_loop(); // yaw轴转向控制，更新YAW_output
|
|
||||||
leg_loop(); // 腿部动作控制，控制舵机SyncWritePosEx，包括判断jump_loop
|
|
||||||
|
|
||||||
// 记录上一次的遥控数据数据
|
|
||||||
wrobot.dir_last = wrobot.dir;
|
|
||||||
wrobot.joyx_last = wrobot.joyx;
|
|
||||||
wrobot.joyy_last = wrobot.joyy;
|
|
||||||
|
|
||||||
/* 将自平衡计算输出转矩赋给电机
|
|
||||||
motor.target 负数小车向前转，正数小车向后转，
|
|
||||||
LQR_u 越大，电机输出的转矩越大，电机向后转，默认0-8V
|
|
||||||
LQR_u = angle_control + gyro_control + distance_control + speed_control
|
|
||||||
|
|
||||||
angle_control = pid_angle(LQR_angle - angle_zeropoint) //LQR_u与实际LQR_angle正相关
|
|
||||||
= pid_angle( mpu6050.getAngleY() - angle_zeropoint) //LQR_u与实际pitch正相关
|
|
||||||
gyro_control = pid_gyro(LQR_gyro) //LQR_u与实际LQR_gyro正相关
|
|
||||||
= pid_gyro( mpu6050.getGyroY() ) //LQR_u与实际pitch 角速度 正相关
|
|
||||||
|
|
||||||
所以这里LQR_distance和LQR_speed的计算乘以-0.5,不能单看负号
|
|
||||||
distance_control = pid_distance(LQR_distance - distance_zeropoint) //LQR_u与电LQR_distance正相关
|
|
||||||
= pid_distance( (-0.5)*(motor1.shaft_angle + motor2.shaft_angle) - distance_zeropoint)
|
|
||||||
//在平衡车中，通常设定电机向后转（使车前进）为负，所以LQR_u与电机实际shaft_angle角度正相关，和负号没关系
|
|
||||||
|
|
||||||
speed_control = pid_speed(LQR_speed - 0.1 * lpf_joyy(wrobot.joyy)) // 实际速度-目标速度
|
|
||||||
//LQR_speed代表实际速度，LQR_u与实际LQR_speed速度正相关
|
|
||||||
= pid_speed( (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity) - 0.1 * lpf_joyy(wrobot.joyy))
|
|
||||||
//LQR_u与实际shaft_velocity速度正相关
|
|
||||||
*/
|
|
||||||
motor1.target = (-0.5) * (LQR_u + YAW_output); // 负数小车向前，正数小车向后，取
|
|
||||||
motor2.target = (-0.5) * (LQR_u - YAW_output);
|
|
||||||
|
|
||||||
// 查看角度信息, 屁股向后倒 -36度，头向前到底 45度
|
|
||||||
// Serial.println(LQR_angle)
|
|
||||||
|
|
||||||
// 向后倒地，角度小于-36度，向前倒地，角度大于倒地 45度，
|
|
||||||
// 检查小车是否倒地，若是则重置参数，让小车自平衡重新站起来
|
|
||||||
if (LQR_angle < -30.0f || LQR_angle > 40.0f) {
|
|
||||||
resetToDefault(); // 非常重要！！！
|
|
||||||
motor1.target = 0;
|
|
||||||
motor2.target = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
// target目标电压 0~8V, 0为不转, 8为最大转速
|
|
||||||
// Serial.print("target电压:");
|
|
||||||
// Serial.print(motor1.target);
|
|
||||||
// Serial.print(",motor2.target:");
|
|
||||||
// Serial.println(motor2.target);
|
|
||||||
|
|
||||||
// 迭代计算FOC相电压
|
|
||||||
motor1.loopFOC();
|
|
||||||
motor2.loopFOC();
|
|
||||||
|
|
||||||
// 设置轮部电机输出
|
|
||||||
motor1.move();
|
|
||||||
motor2.move();
|
|
||||||
} // loop()
|
|
||||||
|
|
||||||
// lqr自平衡控制
|
|
||||||
void lqr_balance_loop()
|
|
||||||
{
|
|
||||||
|
|
||||||
// LQR_distance,LQR_speed,LQR_angle,LQR_gyro 通过编码器或者mpu实际测得
|
|
||||||
// angle_zeropoint，distance_zeropoint 角度零点和位置零点（在理想平衡状态下俯仰角度和位移目标值），用于和实际值做差，得到控制
|
|
||||||
// lqr 控制参数： angle_control , gyro_control , distance_control , speed_control
|
|
||||||
|
|
||||||
// LQR平衡算式，实际使用中为便于调参，讲算式分解为4个P控制，采用PIDController方法在commander中实时调试
|
|
||||||
// LQR_u = LQR_k1*(LQR_angle - angle_zeropoint)
|
|
||||||
// + LQR_k2*LQR_gyro
|
|
||||||
// + LQR_k3*(LQR_distance - distance_zeropoint)
|
|
||||||
// + LQR_k4*LQR_speed;
|
|
||||||
|
|
||||||
// 给负值是因为按照当前的电机接线，正转矩驱动小车电机向后转
|
|
||||||
// 编码器反馈shaft_angle角度，shaft_velocity速度
|
|
||||||
// 不操作前后移动，譬如刚开机，车轮不动的，shaft_angle和shaft_velocity 都是零
|
|
||||||
// 所以，不操作前后移动，LQR_distance 和 LQR_speed趋近于0，只有角度控制和角速度控制
|
|
||||||
// 在平衡车中，通常设定电机向后转（使车前进）为负，所以这里LQR_distance和LQR_speed的计算乘以-0.5
|
|
||||||
LQR_distance = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle); // 两个电机的旋转角度（shaft_angle）（是位置量）的平均值
|
|
||||||
LQR_speed = (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity); // 两个电机角速度（shaft_velocity）的平均值，并取反
|
|
||||||
LQR_angle = (float)mpu6050.getAngleY(); // mpu6050 pitch 角度，向前倒正数，向后倒负数
|
|
||||||
LQR_gyro = (float)mpu6050.getGyroY(); // pitch Y轴角速度
|
|
||||||
|
|
||||||
// 计算pitch的角度和角速度的pid控制值
|
|
||||||
// 实际pitch角度与零点的差值，目标值是零
|
|
||||||
// 计算角度误差，并通过 PID 控制器得到角度控制量
|
|
||||||
angle_control = pid_angle(LQR_angle - angle_zeropoint);
|
|
||||||
// 实际pitch 角速度与目标值的差值，目标值是零
|
|
||||||
// 通过 PID 控制器对角速度进行处理，得到角速度控制量
|
|
||||||
gyro_control = pid_gyro(LQR_gyro);
|
|
||||||
|
|
||||||
// 运动细节优化处理
|
|
||||||
if (wrobot.joyy != 0) // 有前后方向运动指令时的处理
|
|
||||||
{
|
|
||||||
distance_zeropoint = LQR_distance; // 位移零点重置
|
|
||||||
pid_lqr_u.error_prev = 0; // 输出积分清零
|
|
||||||
}
|
|
||||||
// joyy=0，没有输出前后运动，但是小车可能被推动，运动指令复零时的原地停车处理
|
|
||||||
if ((wrobot.joyx_last != 0 && wrobot.joyx == 0) || (wrobot.joyy_last != 0 && wrobot.joyy == 0))
|
|
||||||
{
|
|
||||||
wrobot_move_stop_flag = 1;
|
|
||||||
}
|
|
||||||
if ((wrobot_move_stop_flag == 1) && (abs(LQR_speed) < 0.5))
|
|
||||||
{
|
|
||||||
distance_zeropoint = LQR_distance; // 位移零点重置
|
|
||||||
wrobot_move_stop_flag = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (abs(LQR_speed) > 15) // 被快速推动时的原地停车处理
|
|
||||||
{
|
|
||||||
distance_zeropoint = LQR_distance; // 位移零点重置
|
|
||||||
}
|
|
||||||
|
|
||||||
// 计算pitch的角度和角速度的pid控制值
|
|
||||||
// 电机转轴角度平均数(上面计算)-位移零点=实际位移与零点的差值, 目标值是零
|
|
||||||
// 计算位移误差，并通过位移 PID 控制器得到位移控制量
|
|
||||||
distance_control = pid_distance(LQR_distance - distance_zeropoint);
|
|
||||||
// 电机转速平均数(上面计算)-速度目标值=实际转速与目标值的差值, 目标值是零
|
|
||||||
// 计算速度误差（实际速度减去经过低通滤波和缩放后的遥控器期望速度），并通过速度 PID 控制器得到速度控制量
|
|
||||||
speed_control = pid_speed(LQR_speed - 0.1 * lpf_joyy(wrobot.joyy));
|
|
||||||
|
|
||||||
// 轮部离地检测, 连续记录两次的轮部转速
|
|
||||||
robot_speed_last = robot_speed; // 记录连续两次的轮部转速
|
|
||||||
robot_speed = LQR_speed;
|
|
||||||
// 若轮部角速度、角加速度"过大"（ > 10或> 50）,或处于跳跃后的恢复时期（jump_flag != 0），认为出现轮部离地现象，需要特殊处理
|
|
||||||
// 轮部离地情况下 LQR_u计算：2个参数，角度控制量+角速度控制量，对轮位移(distance)和速度(speed)控制分量不输出；反之，正常状态下完整输出平衡转矩
|
|
||||||
if (abs(robot_speed - robot_speed_last) > 10 || abs(robot_speed) > 50 || (jump_flag != 0))
|
|
||||||
{
|
|
||||||
distance_zeropoint = LQR_distance; // 位移零点重置
|
|
||||||
LQR_u = angle_control + gyro_control; // 位移和速度控制分量被忽略
|
|
||||||
pid_lqr_u.error_prev = 0; // 输出积分清零
|
|
||||||
}
|
|
||||||
else // 当轮部未离地时，LQR_u：4个参数
|
|
||||||
{
|
|
||||||
// 当轮部未离地时，LQR_u 包含:角度控制量、角速度控制量、位移控制量和速度控制量，以实现完整的自平衡控制
|
|
||||||
LQR_u = angle_control + gyro_control + distance_control + speed_control;
|
|
||||||
// 计算得到LQR_u在loop()函数中在几个子loop执行完，被motor1和motor2执行
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
// 小车没有控制的时候自稳定状态
|
|
||||||
// 控制量lqr_u<5，控制量小， 遥控器无信号输入joyy=0，轮部位移控制正常介入distance_control<4，不处于跳跃后的恢复时期jump_flag=0
|
|
||||||
if (abs(LQR_u) < 5 && wrobot.joyy == 0 && abs(distance_control) < 4 && (jump_flag == 0))
|
|
||||||
{
|
|
||||||
|
|
||||||
LQR_u = pid_lqr_u(LQR_u); // 小转矩非线性补偿
|
|
||||||
// Serial.println(LQR_u);
|
|
||||||
|
|
||||||
// lpf_zeropoint 是一个低通滤波器，对 distance_control 进行滤波处理，减少高频噪声的影响。
|
|
||||||
// 然后，将滤波后的 distance_control 作为输入，通过 pid_zeropoint 控制器进行进一步处理。
|
|
||||||
// angle_zeropoint -= ...：将计算得到的调整量从 angle_zeropoint 中减去，实现重心自适应调整。
|
|
||||||
// angle_zeropoint 是预设的角度零点，调整它可以让小车更好地适应不同的重心位置。
|
|
||||||
angle_zeropoint -= pid_zeropoint(lpf_zeropoint(distance_control)); // 重心自适应
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
pid_lqr_u.error_prev = 0; // 输出积分清零
|
|
||||||
}
|
|
||||||
|
|
||||||
// 平衡控制参数自适应
|
|
||||||
if (wrobot.height < 50)
|
|
||||||
{
|
|
||||||
pid_speed.P = 0.7;
|
|
||||||
}
|
|
||||||
else if (wrobot.height < 64)
|
|
||||||
{
|
|
||||||
pid_speed.P = 0.6;
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
pid_speed.P = 0.5;
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
// 腿部动作控制（舵机）
|
|
||||||
void leg_loop()
|
|
||||||
{
|
|
||||||
|
|
||||||
// 倒地时强制腿部收缩
|
|
||||||
if (LQR_angle < -36.0f || LQR_angle > 45.0f)
|
|
||||||
{
|
|
||||||
Position[0] = SERVO1_MIN;
|
|
||||||
Position[1] = SERVO2_MIN;
|
|
||||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
|
||||||
return; // 跳过后续控制
|
|
||||||
}
|
|
||||||
|
|
||||||
jump_loop();
|
|
||||||
|
|
||||||
if (jump_flag == 0) // 不处于跳跃状态
|
|
||||||
{
|
|
||||||
// 舵机加速度
|
|
||||||
ACC[0] = 8;
|
|
||||||
ACC[1] = 8;
|
|
||||||
|
|
||||||
// 舵机速度
|
|
||||||
Speed[0] = 300;
|
|
||||||
Speed[1] = 300;
|
|
||||||
float roll_angle = (float)mpu6050.getAngleX() + 2.0;
|
|
||||||
leg_position_add = pid_roll_angle(lpf_roll(roll_angle));
|
|
||||||
|
|
||||||
int roll_offset = wrobot.roll;
|
|
||||||
int left_height, right_height;
|
|
||||||
|
|
||||||
// Serial.print("roll_offset: ");
|
|
||||||
// Serial.println(roll_offset);
|
|
||||||
|
|
||||||
// 如果控制了 roll 角度，则调整左右脚的高度
|
|
||||||
// 计算三角函数值
|
|
||||||
float angleRad = abs(roll_offset) * (pi / 180.0); // 将角度转换为弧度
|
|
||||||
float heightAdjust = 30 * sin(angleRad); // 小车宽度的一半是30毫米，抬高的高度是30毫米*sin(弧度)
|
|
||||||
float RollOffset = 18; // 所以水平高度需要加上调整高度，否则向下倾斜无法向下了
|
|
||||||
if (roll_offset > 10) {
|
|
||||||
// 正数，左脚高度 60，右脚高度 40
|
|
||||||
// 因为车身高度最小32，所以水平高度需要加上调整高度15，否则线下倾斜无法向下了
|
|
||||||
left_height = wrobot.height + RollOffset + heightAdjust;
|
|
||||||
right_height = wrobot.height + RollOffset - heightAdjust;
|
|
||||||
leg_position_add = 0; // 不使用roll角度 pid计算
|
|
||||||
} else if (roll_offset < -10) {
|
|
||||||
// 负数，左脚高度 40，右脚高度 60
|
|
||||||
left_height = wrobot.height + RollOffset - heightAdjust;
|
|
||||||
right_height = wrobot.height + RollOffset + heightAdjust;
|
|
||||||
leg_position_add = 0;
|
|
||||||
} else { // 未控制 roll 角度
|
|
||||||
// roll 为 0，左右恢复到同一高度
|
|
||||||
left_height = wrobot.height;
|
|
||||||
right_height = wrobot.height;
|
|
||||||
// 没有人为控制roll角度，leg_position_add根据mpu6050 roll值 进行pid计算得到，以保持小车机身平稳
|
|
||||||
// 但如果小车倒地，则不需要keep roll角度,否则小车倒在地上，1个脚长，1个脚短，很难看
|
|
||||||
if (abs(LQR_angle) > 25.0f)
|
|
||||||
{
|
|
||||||
leg_position_add = 0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// 根据新的高度计算舵机位置
|
|
||||||
// id 1 [0] 正面看右手，id 2 正面看左手 [1]
|
|
||||||
// 舵机id 1：1900（机身最矮） 2400（机身最高）2400-1900=500
|
|
||||||
// 舵机id 2：2300（机身最矮） 1800（机身最高）2300-1800=500
|
|
||||||
// 2048是舵机中间位置，12是微调偏移量，8.4是系数，32是默认高度，最高80，最低32
|
|
||||||
// const int LEG_HEIGHT_BASE = 32; // 这值越小，机身越高，实测范围 0（最高）-52（最低）
|
|
||||||
Position[0] = 2048 + 12 + 8.4 * (left_height - LEG_HEIGHT_BASE) - leg_position_add;
|
|
||||||
Position[1] = 2048 - 12 - 8.4 * (right_height - LEG_HEIGHT_BASE) - leg_position_add;
|
|
||||||
|
|
||||||
// 限制舵机位置范围
|
|
||||||
if (Position[0] < SERVO1_MIN) // 舵机1最低
|
|
||||||
Position[0] = SERVO1_MIN;
|
|
||||||
else if (Position[0] > SERVO1_MAX) // 舵机1最高
|
|
||||||
Position[0] = SERVO1_MAX;
|
|
||||||
|
|
||||||
if (Position[1] > SERVO2_MIN) // 舵机2最低（和舵机1方向是反的）
|
|
||||||
Position[1] = SERVO2_MIN;
|
|
||||||
else if (Position[1] < SERVO2_MAX) // 舵机2最高
|
|
||||||
Position[1] = SERVO2_MAX;
|
|
||||||
|
|
||||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// 跳跃控制
|
|
||||||
void jump_loop()
|
|
||||||
{ // dir: 4 = STOP, 5 = jump 上次动作是跳跃5，本次是停止4，才开始跳，因为可能连续发送了很多5跳跃指令，只执行最后一次
|
|
||||||
// 跳跃，腿部以最快速度伸到最长
|
|
||||||
if ((wrobot.dir_last == 5) && (wrobot.dir == 4) && (jump_flag == 0))
|
|
||||||
{
|
|
||||||
// 跳跃开始
|
|
||||||
ACC[0] = 0;
|
|
||||||
ACC[1] = 0;
|
|
||||||
Speed[0] = 0;
|
|
||||||
Speed[1] = 0;
|
|
||||||
Position[0] = SERVO1_MAX;
|
|
||||||
Position[1] = SERVO2_MAX;
|
|
||||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
|
||||||
|
|
||||||
jump_flag = 1;
|
|
||||||
|
|
||||||
}
|
|
||||||
if (jump_flag > 0) // 跳跃中，腿部缩起来，等待30帧，对应上面设置 jump_flag = 1;
|
|
||||||
{
|
|
||||||
jump_flag++;
|
|
||||||
|
|
||||||
if ((jump_flag > 30) && (jump_flag < 35))
|
|
||||||
{
|
|
||||||
ACC[0] = 0;
|
|
||||||
ACC[1] = 0;
|
|
||||||
Speed[0] = 0;
|
|
||||||
Speed[1] = 0;
|
|
||||||
Position[0] = 2048 + 12 + 8.4 * (35 - 32); // 降低腿部高度，即车重心
|
|
||||||
Position[1] = 2048 - 12 - 8.4 * (35 - 32);
|
|
||||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
|
||||||
|
|
||||||
jump_flag = 40;
|
|
||||||
}
|
|
||||||
if (jump_flag > 200)
|
|
||||||
{
|
|
||||||
jump_flag = 0; // 跳跃过程结束
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// yaw轴转向控制 （转向控制）
|
|
||||||
void yaw_loop()
|
|
||||||
{
|
|
||||||
// YAW_output = 0.03*(YAW_Kp*YAW_angle_total + YAW_Kd*YAW_gyro);
|
|
||||||
yaw_angle_addup();
|
|
||||||
|
|
||||||
YAW_angle_total += wrobot.joyx * 0.01; // 0.002
|
|
||||||
float yaw_angle_control = pid_yaw_angle(YAW_angle_total);
|
|
||||||
float yaw_gyro_control = pid_yaw_gyro(YAW_gyro);
|
|
||||||
// yaw 轴的角度和角速度数据进行了 PID 控制，得到 yaw_angle_control 和 yaw_gyro_control。
|
|
||||||
// 然后，将这两个控制量相加，得到最终的 YAW_output。
|
|
||||||
YAW_output = yaw_angle_control + yaw_gyro_control;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Web数据更新
|
|
||||||
void web_loop()
|
|
||||||
{
|
|
||||||
webserver.handleClient();
|
|
||||||
websocket.loop();
|
|
||||||
rp.spinOnce(); // 更新web端回传的控制信息
|
|
||||||
}
|
|
||||||
|
|
||||||
// yaw轴角度累加函数
|
|
||||||
void yaw_angle_addup()
|
|
||||||
{
|
|
||||||
YAW_angle = (float)mpu6050.getAngleZ();
|
|
||||||
YAW_gyro = (float)mpu6050.getGyroZ();
|
|
||||||
|
|
||||||
if (YAW_angle_zero_point == (-10))
|
|
||||||
{
|
|
||||||
YAW_angle_zero_point = YAW_angle;
|
|
||||||
}
|
|
||||||
|
|
||||||
float yaw_angle_1, yaw_angle_2, yaw_addup_angle;
|
|
||||||
if (YAW_angle > YAW_angle_last)
|
|
||||||
{
|
|
||||||
yaw_angle_1 = YAW_angle - YAW_angle_last;
|
|
||||||
yaw_angle_2 = YAW_angle - YAW_angle_last - 2 * PI;
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
yaw_angle_1 = YAW_angle - YAW_angle_last;
|
|
||||||
yaw_angle_2 = YAW_angle - YAW_angle_last + 2 * PI;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (abs(yaw_angle_1) > abs(yaw_angle_2))
|
|
||||||
{
|
|
||||||
yaw_addup_angle = yaw_angle_2;
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
yaw_addup_angle = yaw_angle_1;
|
|
||||||
}
|
|
||||||
|
|
||||||
YAW_angle_total = YAW_angle_total + yaw_addup_angle;
|
|
||||||
YAW_angle_last = YAW_angle;
|
|
||||||
}
|
|
||||||
|
|
||||||
void basicWebCallback(void)
|
|
||||||
{
|
|
||||||
webserver.send(300, "text/html", basic_web);
|
|
||||||
}
|
|
||||||
|
|
||||||
void webSocketEventCallback(uint8_t num, WStype_t type, uint8_t *payload, size_t length)
|
|
||||||
{
|
|
||||||
if (type == WStype_TEXT)
|
|
||||||
{
|
|
||||||
String payload_str = String((char *)payload);
|
|
||||||
StaticJsonDocument<300> doc;
|
|
||||||
DeserializationError error = deserializeJson(doc, payload_str);
|
|
||||||
|
|
||||||
String mode_str = doc["mode"];
|
|
||||||
if (mode_str == "basic")
|
|
||||||
{
|
|
||||||
rp.parseBasic(doc);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// 电压检测初始化
|
|
||||||
void adc_calibration_init()
|
|
||||||
{
|
|
||||||
if (esp_adc_cal_check_efuse(ESP_ADC_CAL_VAL_EFUSE_TP) == ESP_OK)
|
|
||||||
{
|
|
||||||
printf("eFuse Two Point: Supported\n");
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
printf("eFuse Two Point: NOT supported\n");
|
|
||||||
}
|
|
||||||
// Check Vref is burned into eFuse
|
|
||||||
if (esp_adc_cal_check_efuse(ESP_ADC_CAL_VAL_EFUSE_VREF) == ESP_OK)
|
|
||||||
{
|
|
||||||
printf("eFuse Vref: Supported\n");
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
printf("eFuse Vref: NOT supported\n");
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// 电压检测
|
|
||||||
void bat_check()
|
|
||||||
{
|
|
||||||
if (bat_check_num > 1000)
|
|
||||||
{
|
|
||||||
// 电压读取
|
|
||||||
uint32_t sum = 0;
|
|
||||||
sum = analogRead(BAT_PIN);
|
|
||||||
uint32_t voltage = esp_adc_cal_raw_to_voltage(sum, &adc_chars);
|
|
||||||
double battery = (voltage * 3.97) / 1000.0;
|
|
||||||
|
|
||||||
// Serial.println(battery);
|
|
||||||
// 电量显示
|
|
||||||
if (battery > 7.8)
|
|
||||||
digitalWrite(LED_BAT, HIGH);
|
|
||||||
else
|
|
||||||
digitalWrite(LED_BAT, LOW);
|
|
||||||
|
|
||||||
bat_check_num = 0;
|
|
||||||
}
|
|
||||||
else
|
|
||||||
bat_check_num++;
|
|
||||||
}
|
|
||||||
|
|
||||||
// LED闪烁函数(不要用for，会阻塞其他程序)
|
|
||||||
void blinkLed() {
|
|
||||||
digitalWrite(LED_BAT, HIGH);
|
|
||||||
delay(200);
|
|
||||||
digitalWrite(LED_BAT, LOW);
|
|
||||||
delay(200);
|
|
||||||
digitalWrite(LED_BAT, HIGH);
|
|
||||||
delay(200);
|
|
||||||
digitalWrite(LED_BAT, LOW);
|
|
||||||
delay(200);
|
|
||||||
}
|
|
||||||
|
|
||||||
// 重置到默认参数状态，用于自平衡起立
|
|
||||||
void resetToDefault() {
|
|
||||||
// LQR自平衡控制器参数
|
|
||||||
LQR_angle = 0;
|
|
||||||
LQR_gyro = 0;
|
|
||||||
LQR_speed = 0;
|
|
||||||
LQR_distance = 0;
|
|
||||||
angle_control = 0;
|
|
||||||
gyro_control = 0;
|
|
||||||
speed_control = 0;
|
|
||||||
distance_control = 0;
|
|
||||||
LQR_u = 0;
|
|
||||||
angle_zeropoint = 5;
|
|
||||||
distance_zeropoint = 1;
|
|
||||||
|
|
||||||
// YAW轴控制数据
|
|
||||||
YAW_gyro = 0;
|
|
||||||
YAW_angle = 0;
|
|
||||||
YAW_angle_last = 0;
|
|
||||||
YAW_angle_total = 0;
|
|
||||||
YAW_angle_zero_point = -10;
|
|
||||||
YAW_output = 0;
|
|
||||||
|
|
||||||
// 逻辑处理标志位
|
|
||||||
robot_speed = 0;
|
|
||||||
robot_speed_last = 0;
|
|
||||||
wrobot_move_stop_flag = 0;
|
|
||||||
jump_flag = 0;
|
|
||||||
leg_position_add = 0;
|
|
||||||
|
|
||||||
pid_lqr_u.error_prev = 0;
|
|
||||||
|
|
||||||
}
|
|
||||||
@@ -1,792 +0,0 @@
|
|||||||
// -----------------------------------------------------------------------------
|
|
||||||
// Copyright (c) 2024 Mu Shibo
|
|
||||||
// Modify by Tommy 2025.8.11
|
|
||||||
// -----------------------------------------------------------------------------
|
|
||||||
|
|
||||||
#include <esp_now.h>
|
|
||||||
#include <WiFi.h>
|
|
||||||
#include "main.h"
|
|
||||||
|
|
||||||
/************实例定义*************/
|
|
||||||
|
|
||||||
// 电机实例
|
|
||||||
BLDCMotor motor1 = BLDCMotor(7);
|
|
||||||
BLDCMotor motor2 = BLDCMotor(7);
|
|
||||||
BLDCDriver3PWM driver1 = BLDCDriver3PWM(32, 33, 25, 22);
|
|
||||||
BLDCDriver3PWM driver2 = BLDCDriver3PWM(26, 27, 14, 12);
|
|
||||||
|
|
||||||
// 编码器实例
|
|
||||||
TwoWire I2Cone = TwoWire(0);
|
|
||||||
TwoWire I2Ctwo = TwoWire(1);
|
|
||||||
MagneticSensorI2C sensor1 = MagneticSensorI2C(AS5600_I2C);
|
|
||||||
MagneticSensorI2C sensor2 = MagneticSensorI2C(AS5600_I2C);
|
|
||||||
|
|
||||||
// PID控制器实例
|
|
||||||
PIDController pid_angle(1, 0, 0, 100000, 8); // pitch角度PID使其趋于零点
|
|
||||||
PIDController pid_gyro(0.06, 0, 0, 100000, 8); // 角速度PID使其趋于零点
|
|
||||||
PIDController pid_distance(0.5, 0, 0, 100000, 8);// 位移PID使其趋于零点
|
|
||||||
PIDController pid_speed(0.7, 0, 0, 100000, 8);// 速度PID使其趋于目标速度
|
|
||||||
PIDController pid_yaw_angle(1.0, 0, 0, 100000, 8);
|
|
||||||
PIDController pid_yaw_gyro(0.04, 0, 0, 100000, 8);
|
|
||||||
PIDController pid_lqr_u(1, 15, 0, 100000, 8);
|
|
||||||
PIDController pid_zeropoint(0.002, 0, 0, 100000, 4);
|
|
||||||
PIDController pid_roll_angle(8, 0, 0, 100000, 450);
|
|
||||||
|
|
||||||
// 低通滤波器实例
|
|
||||||
LowPassFilter lpf_joyy(0.2);
|
|
||||||
LowPassFilter lpf_zeropoint(0.1);
|
|
||||||
LowPassFilter lpf_roll(0.3);
|
|
||||||
|
|
||||||
// WebServer实例
|
|
||||||
WebServer webserver; // server服务器
|
|
||||||
WebSocketsServer websocket = WebSocketsServer(81); // 定义一个webSocket服务器来处理客户发送的消息
|
|
||||||
RobotProtocol rp(20);
|
|
||||||
|
|
||||||
// STS舵机实例
|
|
||||||
SMS_STS sms_sts;
|
|
||||||
|
|
||||||
// MPU6050实例
|
|
||||||
MPU6050 mpu6050(I2Ctwo);
|
|
||||||
|
|
||||||
/************参数定义*************/
|
|
||||||
#define pi 3.1415927
|
|
||||||
|
|
||||||
// 舵机位置限制参数
|
|
||||||
#define SERVO1_MIN 1900 // 舵机1最低 从小车正面看右手
|
|
||||||
#define SERVO1_MAX 2400 // 舵机1最高
|
|
||||||
#define SERVO2_MIN 2300 // 舵机2最低 从小车正面看左手(注意大小值与舵机1是相反的)
|
|
||||||
#define SERVO2_MAX 1800 // 舵机2最高
|
|
||||||
|
|
||||||
// LQR自平衡控制器参数
|
|
||||||
float LQR_angle = 0;
|
|
||||||
float LQR_gyro = 0;
|
|
||||||
float LQR_speed = 0;
|
|
||||||
float LQR_distance = 0;
|
|
||||||
float angle_control = 0;
|
|
||||||
float gyro_control = 0;
|
|
||||||
float speed_control = 0;
|
|
||||||
float distance_control = 0;
|
|
||||||
float LQR_u = 0;
|
|
||||||
float angle_zeropoint = 5; // 俯仰角度，向前到正，向后倒负
|
|
||||||
float distance_zeropoint = 1; // 轮部位移零点偏置
|
|
||||||
|
|
||||||
// YAW轴控制数据
|
|
||||||
float YAW_gyro = 0;
|
|
||||||
float YAW_angle = 0;
|
|
||||||
float YAW_angle_last = 0;
|
|
||||||
float YAW_angle_total = 0;
|
|
||||||
float YAW_angle_zero_point = -10;
|
|
||||||
float YAW_output = 0;
|
|
||||||
|
|
||||||
// 腿部舵机控制数据
|
|
||||||
byte ID[2];
|
|
||||||
s16 Position[2];
|
|
||||||
u16 Speed[2];
|
|
||||||
byte ACC[2];
|
|
||||||
|
|
||||||
// 逻辑处理标志位
|
|
||||||
float robot_speed = 0; // 记录当前轮部转速
|
|
||||||
float robot_speed_last = 0; // 记录上一时刻的轮部转速
|
|
||||||
int wrobot_move_stop_flag = 0; // 记录摇杆停止的标识
|
|
||||||
int jump_flag = 0; // 跳跃时段标识
|
|
||||||
float leg_position_add = 0; // roll轴平衡控制量
|
|
||||||
|
|
||||||
// 电压检测
|
|
||||||
uint16_t bat_check_num = 0;
|
|
||||||
int BAT_PIN = 35; // select the input pin for the ADC
|
|
||||||
static esp_adc_cal_characteristics_t adc_chars;
|
|
||||||
static const adc1_channel_t channel = ADC1_CHANNEL_7;
|
|
||||||
static const adc_bits_width_t width = ADC_WIDTH_BIT_12;
|
|
||||||
static const adc_atten_t atten = ADC_ATTEN_DB_11;
|
|
||||||
static const adc_unit_t unit = ADC_UNIT_1;
|
|
||||||
|
|
||||||
// 电量显示LED
|
|
||||||
#define LED_BAT 13
|
|
||||||
|
|
||||||
// LED闪烁控制变量
|
|
||||||
bool blinking = false;
|
|
||||||
unsigned long lastBlinkTime = 0;
|
|
||||||
int blinkCount = 0;
|
|
||||||
const int BLINK_INTERVAL = 200; // 闪烁间隔(ms)
|
|
||||||
const int BLINK_TIMES = 5; // 闪烁次数
|
|
||||||
// 在参数定义部分添加障碍物检测相关变量
|
|
||||||
bool obstacleDetected = false; // 障碍物检测状态
|
|
||||||
unsigned long obstacleStartTime = 0; // 障碍物开始时间
|
|
||||||
const unsigned long OBSTACLE_DELAY = 500; // 判定为障碍物的延迟时间(毫秒)
|
|
||||||
// 在参数定义部分新增扭矩补偿相关变量
|
|
||||||
const unsigned long BOOST_DURATION = 1500; // 最大补偿时长（3秒，避免持续高负载）
|
|
||||||
unsigned long boostStartTime = 0; // 补偿开始时间
|
|
||||||
|
|
||||||
|
|
||||||
// 摇杆数据死区参数
|
|
||||||
const int deadZone = 15;
|
|
||||||
typedef struct {
|
|
||||||
byte j1PotX; // 左摇杆X (0-255)
|
|
||||||
byte j1PotY; // 左摇杆Y (0-255)
|
|
||||||
byte j2PotX; // 右摇杆X (0-255)
|
|
||||||
byte j2PotY; // 右摇杆Y (0-255)
|
|
||||||
bool buttonR1; //底部右按键 1：true （默认），0：false
|
|
||||||
bool buttonR2; //底部左按键 1：true （默认），0：false
|
|
||||||
bool j1Button; //左摇杆按键 1：true （默认），0：false
|
|
||||||
bool j2Button; //右摇杆按键 1：true （默认），0：false
|
|
||||||
bool buttonLB; //顶部左按键 1：true （默认），0：false
|
|
||||||
bool buttonRB; //顶部右按键 1：true （默认），0：false
|
|
||||||
bool tSwitch1; //拨动开关 1：true （默认），0：false
|
|
||||||
byte roll; //用于M5StackCore2或未来增加的陀螺仪功能
|
|
||||||
byte pitch;
|
|
||||||
bool buttonR3;
|
|
||||||
} JoystickData;
|
|
||||||
|
|
||||||
JoystickData remoteData;
|
|
||||||
|
|
||||||
// 蓝灯爆闪
|
|
||||||
bool lastbuttonLB = false;
|
|
||||||
|
|
||||||
// ESP-NOW接收回调函数
|
|
||||||
void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
|
|
||||||
memcpy(&remoteData, incomingData, sizeof(remoteData));
|
|
||||||
|
|
||||||
// 摇杆输出范围0-255，转换到 -100 到 100
|
|
||||||
// roll 左右摇摆
|
|
||||||
int convertedJ1PotX = (remoteData.j1PotX - 127.5) * (30.0 / 127.5); // 转到 -30 到 30
|
|
||||||
// 上下高度
|
|
||||||
int convertedJ1PotY = static_cast<int>((1 - remoteData.j1PotY / 255.0) * (60 - 0) + 0); // 转到 60 到 0 y轴取反，这样可以使机身下降
|
|
||||||
// 左右旋转
|
|
||||||
int convertedJ2PotX = (remoteData.j2PotX - 127.5) * (100.0 / 127.5); // 转到 -100 到 100
|
|
||||||
// 速度
|
|
||||||
int convertedJ2PotY = (remoteData.j2PotY - 127.5) * (100.0 / 127.5) * -1 * 1.3; // 转到 -100 到 100 y轴取反,并且放大洗漱
|
|
||||||
|
|
||||||
// 取反按钮值，使得默认显示 0 false，按下显示 1 true
|
|
||||||
int convertedbuttonLB = !remoteData.buttonLB; // 顶部左
|
|
||||||
int convertedbuttonRB = !remoteData.buttonRB; // 顶部右
|
|
||||||
|
|
||||||
// 跳跃
|
|
||||||
if (convertedbuttonRB) { // 1=true，0=false
|
|
||||||
wrobot.dir = 5; // 跳跃
|
|
||||||
}else{
|
|
||||||
wrobot.dir = 4; // 不跳跃
|
|
||||||
}
|
|
||||||
|
|
||||||
// led蓝灯爆闪
|
|
||||||
if (lastbuttonLB && !convertedbuttonLB) {
|
|
||||||
blinkLed(); // 不能写在loop中，会阻断legloop，小车会倒下
|
|
||||||
}
|
|
||||||
// 更新上次 lastbuttonRB 的值
|
|
||||||
lastbuttonLB = convertedbuttonLB;
|
|
||||||
|
|
||||||
if(abs(convertedJ1PotY) > deadZone) { // 上下高度 15 ~ 75
|
|
||||||
wrobot.height = convertedJ1PotY;
|
|
||||||
}else{
|
|
||||||
wrobot.height = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
if(abs(convertedJ1PotX) > deadZone) { // 左右摇摆 -30 ~ 30
|
|
||||||
wrobot.roll = convertedJ1PotX;
|
|
||||||
}else{
|
|
||||||
wrobot.roll = 0;
|
|
||||||
}
|
|
||||||
if(abs(convertedJ2PotY) > deadZone) { // 前后运动 -100 到 100
|
|
||||||
wrobot.joyy = convertedJ2PotY;
|
|
||||||
}else{
|
|
||||||
wrobot.joyy = 0;
|
|
||||||
}
|
|
||||||
if(abs(convertedJ2PotX) > deadZone) { // 左右运动 -100 到 100
|
|
||||||
wrobot.joyx = convertedJ2PotX;
|
|
||||||
} else {
|
|
||||||
wrobot.joyx = 0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void setup()
|
|
||||||
{
|
|
||||||
Serial.begin(115200); // 通讯串口
|
|
||||||
Serial2.begin(1000000); // 腿部sts舵机
|
|
||||||
|
|
||||||
// Wifi初始化
|
|
||||||
WiFi_SetAP();
|
|
||||||
webserver.begin();
|
|
||||||
webserver.on("/", HTTP_GET, basicWebCallback);
|
|
||||||
websocket.begin();
|
|
||||||
websocket.onEvent(webSocketEventCallback);
|
|
||||||
|
|
||||||
// 初始化ESP-NOW
|
|
||||||
WiFi.mode(WIFI_STA); // 打开这行注释就能开启sta模式，同时默认关闭ap热点模式，省电+稳定，而且不会影响espnow通信和整体代码框架
|
|
||||||
if (esp_now_init() != ESP_OK) {
|
|
||||||
Serial.println("Error initializing ESP-NOW");
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
esp_now_register_recv_cb(OnDataRecv);
|
|
||||||
|
|
||||||
// 舵机初始化
|
|
||||||
sms_sts.pSerial = &Serial2;
|
|
||||||
ID[0] = 1;
|
|
||||||
ID[1] = 2;
|
|
||||||
ACC[0] = 30;
|
|
||||||
ACC[1] = 30;
|
|
||||||
Speed[0] = 400;
|
|
||||||
Speed[1] = 400;
|
|
||||||
Position[0] = 2048;
|
|
||||||
Position[1] = 2048;
|
|
||||||
// 舵机(ID1/ID2)以最高速度V=2400步/秒，加速度A=50(50*100步/秒^2)，运行至各自的Position位置
|
|
||||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
|
||||||
|
|
||||||
// 电压检测
|
|
||||||
adc_calibration_init();
|
|
||||||
adc1_config_width(width);
|
|
||||||
adc1_config_channel_atten(channel, atten);
|
|
||||||
esp_adc_cal_characterize(unit, atten, width, 0, &adc_chars);
|
|
||||||
|
|
||||||
// 电量显示LED
|
|
||||||
pinMode(LED_BAT, OUTPUT);
|
|
||||||
|
|
||||||
// 编码器设置
|
|
||||||
I2Cone.begin(19, 18, 400000UL);
|
|
||||||
I2Ctwo.begin(23, 5, 400000UL);
|
|
||||||
sensor1.init(&I2Cone);
|
|
||||||
sensor2.init(&I2Ctwo);
|
|
||||||
|
|
||||||
// mpu6050设置
|
|
||||||
mpu6050.begin();
|
|
||||||
mpu6050.calcGyroOffsets(true);
|
|
||||||
|
|
||||||
// 连接motor对象与编码器对象
|
|
||||||
motor1.linkSensor(&sensor1);
|
|
||||||
motor2.linkSensor(&sensor2);
|
|
||||||
|
|
||||||
// 速度环PID参数
|
|
||||||
motor1.PID_velocity.P = 0.05;
|
|
||||||
motor1.PID_velocity.I = 1;
|
|
||||||
motor1.PID_velocity.D = 0;
|
|
||||||
|
|
||||||
motor2.PID_velocity.P = 0.05;
|
|
||||||
motor2.PID_velocity.I = 1;
|
|
||||||
motor2.PID_velocity.D = 0;
|
|
||||||
|
|
||||||
// 驱动器设置
|
|
||||||
motor1.voltage_sensor_align = 6;
|
|
||||||
motor2.voltage_sensor_align = 6;
|
|
||||||
driver1.voltage_power_supply = 8;
|
|
||||||
driver2.voltage_power_supply = 8;
|
|
||||||
driver1.init();
|
|
||||||
driver2.init();
|
|
||||||
|
|
||||||
// 连接motor对象与驱动器对象
|
|
||||||
motor1.linkDriver(&driver1);
|
|
||||||
motor2.linkDriver(&driver2);
|
|
||||||
|
|
||||||
motor1.torque_controller = TorqueControlType::voltage;// 扭矩控制类型 :电压作为扭矩控制的方式
|
|
||||||
motor2.torque_controller = TorqueControlType::voltage;
|
|
||||||
motor1.controller = MotionControlType::torque; // 运动控制类型:扭矩控制模式
|
|
||||||
motor2.controller = MotionControlType::torque; // 扭矩控制模式
|
|
||||||
|
|
||||||
// monitor相关设置
|
|
||||||
motor1.useMonitoring(Serial);
|
|
||||||
motor2.useMonitoring(Serial);
|
|
||||||
// 电机初始化
|
|
||||||
motor1.init();
|
|
||||||
motor1.initFOC();
|
|
||||||
motor2.init();
|
|
||||||
motor2.initFOC();
|
|
||||||
|
|
||||||
delay(500);
|
|
||||||
}
|
|
||||||
|
|
||||||
void loop()
|
|
||||||
{
|
|
||||||
|
|
||||||
bat_check(); // 电压检测
|
|
||||||
web_loop(); // Web数据更新
|
|
||||||
mpu6050.update(); // IMU数据更新
|
|
||||||
lqr_balance_loop(); // lqr自平衡控制，更新LQR_u
|
|
||||||
yaw_loop(); // yaw轴转向控制，更新YAW_output
|
|
||||||
leg_loop(); // 腿部动作控制，控制舵机SyncWritePosEx，包括判断jump_loop
|
|
||||||
detectObstacle(); // 障碍物检测
|
|
||||||
|
|
||||||
|
|
||||||
// 记录上一次的遥控数据数据
|
|
||||||
wrobot.dir_last = wrobot.dir;
|
|
||||||
wrobot.joyx_last = wrobot.joyx;
|
|
||||||
wrobot.joyy_last = wrobot.joyy;
|
|
||||||
|
|
||||||
// 将自平衡计算输出转矩赋给电机
|
|
||||||
motor1.target = (-0.5) * (LQR_u + YAW_output);
|
|
||||||
motor2.target = (-0.5) * (LQR_u - YAW_output);
|
|
||||||
|
|
||||||
// 查看角度信息, 屁股向后倒 -36度，头向前到底 45度
|
|
||||||
// Serial.println(LQR_angle)
|
|
||||||
|
|
||||||
// 向后倒地 -36度，向前倒地 45度，超出这个区间，或者跳跃中，马达就不要转动了
|
|
||||||
if(LQR_angle < -40.0f || LQR_angle > 55.0f ) {
|
|
||||||
motor1.target = 0;
|
|
||||||
motor2.target = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
// 计算基础扭矩（原有逻辑）
|
|
||||||
float baseTorque1 = (-0.5) * (LQR_u + YAW_output);
|
|
||||||
float baseTorque2 = (-0.5) * (LQR_u - YAW_output);
|
|
||||||
|
|
||||||
// 障碍物扭矩补偿逻辑
|
|
||||||
if (obstacleDetected) {
|
|
||||||
// 首次进入障碍状态时记录时间
|
|
||||||
if (boostStartTime == 0) {
|
|
||||||
boostStartTime = millis();
|
|
||||||
}
|
|
||||||
|
|
||||||
// 补偿时长未超时且机器人未严重倾斜时，应用扭矩补偿
|
|
||||||
if (millis() - boostStartTime < BOOST_DURATION) {
|
|
||||||
// blinkLed();
|
|
||||||
// 叠加补偿扭矩（仅正向，因为是前进障碍）
|
|
||||||
motor1.target = motor1.voltage_limit;
|
|
||||||
motor2.target = motor2.voltage_limit;
|
|
||||||
} else {
|
|
||||||
// 超时或姿态异常时使用基础扭矩
|
|
||||||
motor1.target = baseTorque1;
|
|
||||||
motor2.target = baseTorque2;
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
// 无障碍物时重置补偿计时器并使用基础扭矩
|
|
||||||
boostStartTime = 0;
|
|
||||||
motor1.target = baseTorque1;
|
|
||||||
motor2.target = baseTorque2;
|
|
||||||
}
|
|
||||||
|
|
||||||
// 迭代计算FOC相电压
|
|
||||||
motor1.loopFOC();
|
|
||||||
motor2.loopFOC();
|
|
||||||
|
|
||||||
// 设置轮部电机输出
|
|
||||||
motor1.move();
|
|
||||||
motor2.move();
|
|
||||||
} // loop()
|
|
||||||
|
|
||||||
|
|
||||||
// lqr自平衡控制
|
|
||||||
void lqr_balance_loop()
|
|
||||||
{
|
|
||||||
|
|
||||||
// LQR_distance,LQR_speed,LQR_angle,LQR_gyro 通过编码器或者mpu实际测得
|
|
||||||
// angle_zeropoint，distance_zeropoint 角度零点和位置零点（在理想平衡状态下俯仰角度和位移目标值），用于和实际值做差，得到控制
|
|
||||||
// angle_control , gyro_control , distance_control , speed_control : lqr 控制参数
|
|
||||||
|
|
||||||
// LQR平衡算式，实际使用中为便于调参，讲算式分解为4个P控制，采用PIDController方法在commander中实时调试
|
|
||||||
// LQR_u = LQR_k1*(LQR_angle - angle_zeropoint)
|
|
||||||
// + LQR_k2*LQR_gyro
|
|
||||||
// + LQR_k3*(LQR_distance - distance_zeropoint)
|
|
||||||
// + LQR_k4*LQR_speed;
|
|
||||||
|
|
||||||
// 给负值是因为按照当前的电机接线，正转矩会向后转
|
|
||||||
// 编码器反馈shaft_angle角度，shaft_velocity速度
|
|
||||||
LQR_distance = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle); // 两个电机的旋转角度（shaft_angle）（是位置量）的平均值
|
|
||||||
LQR_speed = (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity); // 两个电机角速度（shaft_velocity）的平均值，并取反
|
|
||||||
LQR_angle = (float)mpu6050.getAngleY(); // mpu6050 pitch 角度，向前倒正数，向后倒负数
|
|
||||||
LQR_gyro = (float)mpu6050.getGyroY(); // pitch Y轴角速度
|
|
||||||
|
|
||||||
// 计算pitch的角度和角速度的pid控制值
|
|
||||||
// 实际pitch角度与零点的差值，目标值是零
|
|
||||||
// 计算角度误差，并通过 PID 控制器得到角度控制量
|
|
||||||
angle_control = pid_angle(LQR_angle - angle_zeropoint);
|
|
||||||
// 实际pitch 角速度与目标值的差值，目标值是零
|
|
||||||
// 通过 PID 控制器对角速度进行处理，得到角速度控制量
|
|
||||||
gyro_control = pid_gyro(LQR_gyro);
|
|
||||||
|
|
||||||
// 运动细节优化处理
|
|
||||||
if (wrobot.joyy != 0) // 有前后方向运动指令时的处理
|
|
||||||
{
|
|
||||||
distance_zeropoint = LQR_distance; // 位移零点重置
|
|
||||||
pid_lqr_u.error_prev = 0; // 输出积分清零
|
|
||||||
}
|
|
||||||
// joyy=0，没有输出前后运动，但是小车可能被推动，运动指令复零时的原地停车处理
|
|
||||||
if ((wrobot.joyx_last != 0 && wrobot.joyx == 0) || (wrobot.joyy_last != 0 && wrobot.joyy == 0))
|
|
||||||
{
|
|
||||||
wrobot_move_stop_flag = 1;
|
|
||||||
}
|
|
||||||
if ((wrobot_move_stop_flag == 1) && (abs(LQR_speed) < 0.5))
|
|
||||||
{
|
|
||||||
distance_zeropoint = LQR_distance; // 位移零点重置
|
|
||||||
wrobot_move_stop_flag = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (abs(LQR_speed) > 15) // 被快速推动时的原地停车处理
|
|
||||||
{
|
|
||||||
distance_zeropoint = LQR_distance; // 位移零点重置
|
|
||||||
}
|
|
||||||
|
|
||||||
// 计算pitch的角度和角速度的pid控制值
|
|
||||||
// 电机转轴角度平均数(上面计算)-位移零点=实际位移与零点的差值, 目标值是零
|
|
||||||
// 计算位移误差，并通过位移 PID 控制器得到位移控制量
|
|
||||||
distance_control = pid_distance(LQR_distance - distance_zeropoint);
|
|
||||||
// 电机转速平均数(上面计算)-速度目标值=实际转速与目标值的差值, 目标值是零
|
|
||||||
// 计算速度误差（实际速度减去经过低通滤波和缩放后的遥控器期望速度），并通过速度 PID 控制器得到速度控制量
|
|
||||||
speed_control = pid_speed(LQR_speed - 0.1 * lpf_joyy(wrobot.joyy));
|
|
||||||
|
|
||||||
// 轮部离地检测, 连续记录两次的轮部转速
|
|
||||||
robot_speed_last = robot_speed;
|
|
||||||
robot_speed = LQR_speed;
|
|
||||||
// 若轮部角速度、角加速度"过大"（ > 10或> 50）,或处于跳跃后的恢复时期（jump_flag != 0），认为出现轮部离地现象，需要特殊处理
|
|
||||||
//轮部离地情况下 LQR_u计算：2个参数，角度控制量+角速度控制量，对轮位移(distance)和速度(speed)控制分量不输出；反之，正常状态下完整输出平衡转矩
|
|
||||||
if (abs(robot_speed - robot_speed_last) > 10 || abs(robot_speed) > 50 || (jump_flag != 0)) // 轮部离地时
|
|
||||||
{
|
|
||||||
distance_zeropoint = LQR_distance; // 位移零点重置
|
|
||||||
// 位移和速度控制分量被忽略
|
|
||||||
LQR_u = angle_control + gyro_control; // 轮部离地情况下，对轮部分量不输出；反之，正常状态下完整输出平衡转矩
|
|
||||||
pid_lqr_u.error_prev = 0; // 输出积分清零
|
|
||||||
}
|
|
||||||
else // 当轮部未离地时，LQR_u：4个参数
|
|
||||||
{
|
|
||||||
// 当轮部未离地时，LQR_u 包含:角度控制量、角速度控制量、位移控制量和速度控制量，以实现完整的自平衡控制
|
|
||||||
LQR_u = angle_control + gyro_control + distance_control + speed_control;
|
|
||||||
// 计算得到LQR_u在loop()函数中在几个子loop执行完，被motor1和motor2执行
|
|
||||||
|
|
||||||
}
|
|
||||||
// 小车没有控制的时候自稳定状态
|
|
||||||
// 控制量lqr_u<5，控制量小， 遥控器无信号输入joyy=0，轮部位移控制正常介入distance_control<4，不处于跳跃后的恢复时期jump_flag=0
|
|
||||||
if (abs(LQR_u) < 5 && wrobot.joyy == 0 && abs(distance_control) < 4 && (jump_flag == 0))
|
|
||||||
{
|
|
||||||
|
|
||||||
LQR_u = pid_lqr_u(LQR_u); // 小转矩非线性补偿
|
|
||||||
// Serial.println(LQR_u);
|
|
||||||
|
|
||||||
// lpf_zeropoint 是一个低通滤波器，对 distance_control 进行滤波处理，减少高频噪声的影响。
|
|
||||||
// 然后，将滤波后的 distance_control 作为输入，通过 pid_zeropoint 控制器进行进一步处理。
|
|
||||||
// angle_zeropoint -= ...：将计算得到的调整量从 angle_zeropoint 中减去，实现重心自适应调整。
|
|
||||||
// angle_zeropoint 是预设的角度零点，调整它可以让小车更好地适应不同的重心位置。
|
|
||||||
angle_zeropoint -= pid_zeropoint(lpf_zeropoint(distance_control)); // 重心自适应
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
pid_lqr_u.error_prev = 0; // 输出积分清零
|
|
||||||
}
|
|
||||||
|
|
||||||
if (obstacleDetected) {
|
|
||||||
// 首次进入障碍状态时记录时间
|
|
||||||
if (boostStartTime == 0) {
|
|
||||||
boostStartTime = millis();
|
|
||||||
}
|
|
||||||
|
|
||||||
// 补偿时长未超时且机器人未严重倾斜时，应用扭矩补偿
|
|
||||||
if (millis() - boostStartTime < BOOST_DURATION) {
|
|
||||||
// blinkLed();
|
|
||||||
// 叠加补偿扭矩（仅正向，因为是前进障碍）
|
|
||||||
LQR_u = LQR_u * 10.5f;
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
// 无障碍物时重置补偿计时器并使用基础扭矩
|
|
||||||
boostStartTime = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
// 平衡控制参数自适应
|
|
||||||
if (wrobot.height < 50)
|
|
||||||
{
|
|
||||||
pid_speed.P = 0.7;
|
|
||||||
}
|
|
||||||
else if (wrobot.height < 64)
|
|
||||||
{
|
|
||||||
pid_speed.P = 0.6;
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
pid_speed.P = 0.5;
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
// 腿部动作控制（舵机）
|
|
||||||
void leg_loop()
|
|
||||||
{
|
|
||||||
|
|
||||||
// 倒地时强制腿部收缩
|
|
||||||
if (LQR_angle < -36.0f || LQR_angle > 45.0f)
|
|
||||||
{
|
|
||||||
Position[0] = SERVO1_MIN;
|
|
||||||
Position[1] = SERVO2_MIN;
|
|
||||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
|
||||||
return; // 跳过后续控制
|
|
||||||
}
|
|
||||||
|
|
||||||
jump_loop();
|
|
||||||
|
|
||||||
if (jump_flag == 0) // 不处于跳跃状态
|
|
||||||
{
|
|
||||||
// 舵机加速度
|
|
||||||
ACC[0] = 8;
|
|
||||||
ACC[1] = 8;
|
|
||||||
|
|
||||||
// 舵机速度
|
|
||||||
Speed[0] = 300;
|
|
||||||
Speed[1] = 300;
|
|
||||||
float roll_angle = (float)mpu6050.getAngleX() + 2.0;
|
|
||||||
leg_position_add = pid_roll_angle(lpf_roll(roll_angle));
|
|
||||||
|
|
||||||
int roll_offset = wrobot.roll;
|
|
||||||
int left_height, right_height;
|
|
||||||
|
|
||||||
// Serial.print("roll_offset: ");
|
|
||||||
// Serial.println(roll_offset);
|
|
||||||
|
|
||||||
// 如果控制了 roll 角度，则调整左右脚的高度
|
|
||||||
// 计算三角函数值
|
|
||||||
float angleRad = abs(roll_offset) * (pi / 180.0); // 将角度转换为弧度
|
|
||||||
float heightAdjust = 30 * sin(angleRad); // 小车宽度的一半是30毫米，抬高的高度是30毫米*sin(弧度)
|
|
||||||
float RollOffset = 18; // 所以水平高度需要加上调整高度，否则向下倾斜无法向下了
|
|
||||||
if (roll_offset > 10) {
|
|
||||||
// 正数，左脚高度 60，右脚高度 40
|
|
||||||
// 因为车身高度最小32，所以水平高度需要加上调整高度15，否则线下倾斜无法向下了
|
|
||||||
left_height = wrobot.height + RollOffset + heightAdjust;
|
|
||||||
right_height = wrobot.height + RollOffset - heightAdjust;
|
|
||||||
leg_position_add = 0; // 不使用roll角度 pid计算
|
|
||||||
} else if (roll_offset < -10) {
|
|
||||||
// 负数，左脚高度 40，右脚高度 60
|
|
||||||
left_height = wrobot.height + RollOffset - heightAdjust;
|
|
||||||
right_height = wrobot.height + RollOffset + heightAdjust;
|
|
||||||
leg_position_add = 0;
|
|
||||||
} else { // 未控制 roll 角度
|
|
||||||
// roll 为 0，左右恢复到同一高度
|
|
||||||
left_height = wrobot.height;
|
|
||||||
right_height = wrobot.height;
|
|
||||||
// 没有人为控制roll角度，leg_position_add根据mpu6050 roll值 进行pid计算得到，以保持小车机身平稳
|
|
||||||
// 但如果小车倒地，则不需要keep roll角度,否则小车倒在地上，1个脚长，1个脚短，很难看
|
|
||||||
if (abs(LQR_angle) > 25.0f)
|
|
||||||
{
|
|
||||||
leg_position_add = 0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
// 根据新的高度计算舵机位置
|
|
||||||
// id 1 [0] 正面看右手，id 2 正面看左手 [1]
|
|
||||||
// 舵机id 1：1900（机身最矮） 2400（机身最高）2400-1900=500
|
|
||||||
// 舵机id 2：2300（机身最矮） 1800（机身最高）2300-1800=500
|
|
||||||
// 2048是舵机中间位置，12是微调偏移量，8.4是系数，32是默认高度，最高80，最低32
|
|
||||||
Position[0] = 2048 + 12 + 8.4 * (left_height - 32) - leg_position_add;
|
|
||||||
Position[1] = 2048 - 12 - 8.4 * (right_height - 32) - leg_position_add;
|
|
||||||
// Position[0] = 2048 + 12 + 8.4 * (left_height - 52) - leg_position_add; // -52 机身下降20
|
|
||||||
// Position[1] = 2048 - 12 - 8.4 * (right_height - 52) - leg_position_add;
|
|
||||||
// Position[0] = 2048 + 12 + 8.4 * (left_height - 0) - leg_position_add; // 不减32 机身高度默认抬高32
|
|
||||||
// Position[1] = 2048 - 12 - 8.4 * (right_height - 0) - leg_position_add;
|
|
||||||
|
|
||||||
// Serial.print("left_height: ");
|
|
||||||
// Serial.print(left_height);
|
|
||||||
// Serial.print(", right_height: ");
|
|
||||||
// Serial.print(right_height);
|
|
||||||
// Serial.print(", leg_position_add: ");
|
|
||||||
// Serial.println(leg_position_add);
|
|
||||||
|
|
||||||
|
|
||||||
// 限制舵机位置范围
|
|
||||||
if (Position[0] < SERVO1_MIN) // 舵机1最低
|
|
||||||
Position[0] = SERVO1_MIN;
|
|
||||||
else if (Position[0] > SERVO1_MAX) // 舵机1最高
|
|
||||||
Position[0] = SERVO1_MAX;
|
|
||||||
|
|
||||||
if (Position[1] > SERVO2_MIN) // 舵机2最低（和舵机1方向是反的）
|
|
||||||
Position[1] = SERVO2_MIN;
|
|
||||||
else if (Position[1] < SERVO2_MAX) // 舵机2最高
|
|
||||||
Position[1] = SERVO2_MAX;
|
|
||||||
|
|
||||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// 跳跃控制
|
|
||||||
void jump_loop()
|
|
||||||
{ // dir: 4 = STOP, 5 = jump 上次动作是跳跃5，本次是停止4，才开始跳，因为可能连续发送了很多5跳跃指令，只执行最后一次
|
|
||||||
// 跳跃，腿部以最快速度伸到最长
|
|
||||||
if ((wrobot.dir_last == 5) && (wrobot.dir == 4) && (jump_flag == 0))
|
|
||||||
{ // 跳跃开始
|
|
||||||
ACC[0] = 0;
|
|
||||||
ACC[1] = 0;
|
|
||||||
Speed[0] = 0;
|
|
||||||
Speed[1] = 0;
|
|
||||||
Position[0] = SERVO1_MAX;
|
|
||||||
Position[1] = SERVO2_MAX;
|
|
||||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
|
||||||
|
|
||||||
jump_flag = 1;
|
|
||||||
}
|
|
||||||
if (jump_flag > 0) // 跳跃中，腿部缩起来，等待30帧，对应上面设置 jump_flag = 1;
|
|
||||||
{
|
|
||||||
jump_flag++;
|
|
||||||
if ((jump_flag > 30) && (jump_flag < 35))
|
|
||||||
{
|
|
||||||
ACC[0] = 0;
|
|
||||||
ACC[1] = 0;
|
|
||||||
Speed[0] = 0;
|
|
||||||
Speed[1] = 0;
|
|
||||||
Position[0] = 2048 + 12 + 8.4 * (35 - 32); // 降低腿部高度，即车重心
|
|
||||||
Position[1] = 2048 - 12 - 8.4 * (35 - 32);
|
|
||||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
|
||||||
|
|
||||||
jump_flag = 40;
|
|
||||||
}
|
|
||||||
if (jump_flag > 200)
|
|
||||||
{
|
|
||||||
jump_flag = 0; // 跳跃过程结束
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// yaw轴转向控制 （转向控制）
|
|
||||||
void yaw_loop()
|
|
||||||
{
|
|
||||||
// YAW_output = 0.03*(YAW_Kp*YAW_angle_total + YAW_Kd*YAW_gyro);
|
|
||||||
yaw_angle_addup();
|
|
||||||
|
|
||||||
YAW_angle_total += wrobot.joyx * 0.008; // 0.002
|
|
||||||
float yaw_angle_control = pid_yaw_angle(YAW_angle_total);
|
|
||||||
float yaw_gyro_control = pid_yaw_gyro(YAW_gyro);
|
|
||||||
// yaw 轴的角度和角速度数据进行了 PID 控制，得到 yaw_angle_control 和 yaw_gyro_control。
|
|
||||||
// 然后，将这两个控制量相加，得到最终的 YAW_output。
|
|
||||||
YAW_output = yaw_angle_control + yaw_gyro_control;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Web数据更新
|
|
||||||
void web_loop()
|
|
||||||
{
|
|
||||||
webserver.handleClient();
|
|
||||||
websocket.loop();
|
|
||||||
rp.spinOnce(); // 更新web端回传的控制信息
|
|
||||||
}
|
|
||||||
|
|
||||||
// yaw轴角度累加函数
|
|
||||||
void yaw_angle_addup()
|
|
||||||
{
|
|
||||||
YAW_angle = (float)mpu6050.getAngleZ();
|
|
||||||
YAW_gyro = (float)mpu6050.getGyroZ();
|
|
||||||
|
|
||||||
if (YAW_angle_zero_point == (-10))
|
|
||||||
{
|
|
||||||
YAW_angle_zero_point = YAW_angle;
|
|
||||||
}
|
|
||||||
|
|
||||||
float yaw_angle_1, yaw_angle_2, yaw_addup_angle;
|
|
||||||
if (YAW_angle > YAW_angle_last)
|
|
||||||
{
|
|
||||||
yaw_angle_1 = YAW_angle - YAW_angle_last;
|
|
||||||
yaw_angle_2 = YAW_angle - YAW_angle_last - 2 * PI;
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
yaw_angle_1 = YAW_angle - YAW_angle_last;
|
|
||||||
yaw_angle_2 = YAW_angle - YAW_angle_last + 2 * PI;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (abs(yaw_angle_1) > abs(yaw_angle_2))
|
|
||||||
{
|
|
||||||
yaw_addup_angle = yaw_angle_2;
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
yaw_addup_angle = yaw_angle_1;
|
|
||||||
}
|
|
||||||
|
|
||||||
YAW_angle_total = YAW_angle_total + yaw_addup_angle;
|
|
||||||
YAW_angle_last = YAW_angle;
|
|
||||||
}
|
|
||||||
|
|
||||||
void basicWebCallback(void)
|
|
||||||
{
|
|
||||||
webserver.send(300, "text/html", basic_web);
|
|
||||||
}
|
|
||||||
|
|
||||||
void webSocketEventCallback(uint8_t num, WStype_t type, uint8_t *payload, size_t length)
|
|
||||||
{
|
|
||||||
if (type == WStype_TEXT)
|
|
||||||
{
|
|
||||||
String payload_str = String((char *)payload);
|
|
||||||
StaticJsonDocument<300> doc;
|
|
||||||
DeserializationError error = deserializeJson(doc, payload_str);
|
|
||||||
|
|
||||||
String mode_str = doc["mode"];
|
|
||||||
if (mode_str == "basic")
|
|
||||||
{
|
|
||||||
rp.parseBasic(doc);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// 电压检测初始化
|
|
||||||
void adc_calibration_init()
|
|
||||||
{
|
|
||||||
if (esp_adc_cal_check_efuse(ESP_ADC_CAL_VAL_EFUSE_TP) == ESP_OK)
|
|
||||||
{
|
|
||||||
printf("eFuse Two Point: Supported\n");
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
printf("eFuse Two Point: NOT supported\n");
|
|
||||||
}
|
|
||||||
// Check Vref is burned into eFuse
|
|
||||||
if (esp_adc_cal_check_efuse(ESP_ADC_CAL_VAL_EFUSE_VREF) == ESP_OK)
|
|
||||||
{
|
|
||||||
printf("eFuse Vref: Supported\n");
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
printf("eFuse Vref: NOT supported\n");
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// 电压检测
|
|
||||||
void bat_check()
|
|
||||||
{
|
|
||||||
if (bat_check_num > 1000)
|
|
||||||
{
|
|
||||||
// 电压读取
|
|
||||||
uint32_t sum = 0;
|
|
||||||
sum = analogRead(BAT_PIN);
|
|
||||||
uint32_t voltage = esp_adc_cal_raw_to_voltage(sum, &adc_chars);
|
|
||||||
double battery = (voltage * 3.97) / 1000.0;
|
|
||||||
|
|
||||||
// Serial.println(battery);
|
|
||||||
// 电量显示
|
|
||||||
if (battery > 7.8)
|
|
||||||
digitalWrite(LED_BAT, HIGH);
|
|
||||||
else
|
|
||||||
digitalWrite(LED_BAT, LOW);
|
|
||||||
|
|
||||||
bat_check_num = 0;
|
|
||||||
}
|
|
||||||
else
|
|
||||||
bat_check_num++;
|
|
||||||
}
|
|
||||||
|
|
||||||
// LED闪烁函数(不适用for语气，非阻塞)
|
|
||||||
void blinkLed() {
|
|
||||||
unsigned long currentTime = millis();
|
|
||||||
if (currentTime - lastBlinkTime > BLINK_INTERVAL) {
|
|
||||||
lastBlinkTime = currentTime;
|
|
||||||
digitalWrite(LED_BAT, !digitalRead(LED_BAT)); // 切换LED状态
|
|
||||||
|
|
||||||
if (++blinkCount >= BLINK_TIMES * 2) { // 每次闪烁包含开和关
|
|
||||||
blinking = false;
|
|
||||||
digitalWrite(LED_BAT, LOW); // 结束闪烁后保持熄灭
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// 障碍物检测函数
|
|
||||||
void detectObstacle() {
|
|
||||||
// 条件1: 有前进指令(joyy > 0)且超过死区
|
|
||||||
// 条件2: 实际速度很低(接近零)
|
|
||||||
// 条件3: 不在跳跃状态
|
|
||||||
bool hasForwardCommand = (wrobot.joyy > deadZone);
|
|
||||||
bool isAlmostStopped = (abs(LQR_speed) < 1.0);
|
|
||||||
bool notJumping = (jump_flag == 0);
|
|
||||||
bool fallAngle = (LQR_angle > 5.0f || LQR_angle < 40.0f);
|
|
||||||
|
|
||||||
if (hasForwardCommand && isAlmostStopped && notJumping && fallAngle) {
|
|
||||||
// 如果符合条件且尚未计时，记录开始时间
|
|
||||||
if (obstacleStartTime == 0) {
|
|
||||||
obstacleStartTime = millis();
|
|
||||||
}
|
|
||||||
// 如果持续时间超过阈值，判定为遇到障碍物
|
|
||||||
else if (millis() - obstacleStartTime > OBSTACLE_DELAY) {
|
|
||||||
obstacleDetected = true;
|
|
||||||
// blinkLed();
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
// 不符合条件，重置状态
|
|
||||||
obstacleDetected = false;
|
|
||||||
obstacleStartTime = 0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
+106
-35
@@ -103,6 +103,9 @@ static const adc_bits_width_t width = ADC_WIDTH_BIT_12;
|
|||||||
static const adc_atten_t atten = ADC_ATTEN_DB_11;
|
static const adc_atten_t atten = ADC_ATTEN_DB_11;
|
||||||
static const adc_unit_t unit = ADC_UNIT_1;
|
static const adc_unit_t unit = ADC_UNIT_1;
|
||||||
|
|
||||||
|
int loopCounter = 0; // loop函数执行计数
|
||||||
|
int resetToDefaultCounter = 0; // resetToDefault执行计数
|
||||||
|
|
||||||
// 电量显示LED
|
// 电量显示LED
|
||||||
#define LED_BAT 13
|
#define LED_BAT 13
|
||||||
|
|
||||||
@@ -148,6 +151,16 @@ void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
|
|||||||
// 取反按钮值，使得默认显示 0 false，按下显示 1 true
|
// 取反按钮值，使得默认显示 0 false，按下显示 1 true
|
||||||
int convertedbuttonLB = !remoteData.buttonLB; // 顶部左
|
int convertedbuttonLB = !remoteData.buttonLB; // 顶部左
|
||||||
int convertedbuttonRB = !remoteData.buttonRB; // 顶部右
|
int convertedbuttonRB = !remoteData.buttonRB; // 顶部右
|
||||||
|
int convertedbuttonR1 = !remoteData.buttonR1; // 底部右
|
||||||
|
int convertedbuttonR2 = !remoteData.buttonR2; // 底部左
|
||||||
|
|
||||||
|
// 改变倾角
|
||||||
|
// if(convertedbuttonR1){
|
||||||
|
// angle_zeropoint++;
|
||||||
|
// }
|
||||||
|
// if(convertedbuttonR2){
|
||||||
|
// angle_zeropoint--;
|
||||||
|
// }
|
||||||
|
|
||||||
// 跳跃
|
// 跳跃
|
||||||
if (convertedbuttonRB) { // 1=true，0=false
|
if (convertedbuttonRB) { // 1=true，0=false
|
||||||
@@ -214,8 +227,10 @@ void setup()
|
|||||||
ACC[1] = 30;
|
ACC[1] = 30;
|
||||||
Speed[0] = 400;
|
Speed[0] = 400;
|
||||||
Speed[1] = 400;
|
Speed[1] = 400;
|
||||||
Position[0] = 2048;
|
// Position[0] = 2048;
|
||||||
Position[1] = 2048;
|
// Position[1] = 2048;
|
||||||
|
Position[0] = SERVO1_MIN;
|
||||||
|
Position[1] = SERVO2_MIN;
|
||||||
// 舵机(ID1/ID2)以最高速度V=2400步/秒，加速度A=50(50*100步/秒^2)，运行至各自的Position位置
|
// 舵机(ID1/ID2)以最高速度V=2400步/秒，加速度A=50(50*100步/秒^2)，运行至各自的Position位置
|
||||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||||
|
|
||||||
@@ -278,6 +293,7 @@ void setup()
|
|||||||
motor2.init();
|
motor2.init();
|
||||||
motor2.initFOC();
|
motor2.initFOC();
|
||||||
|
|
||||||
|
resetToDefault(); // 重置到默认参数状态
|
||||||
delay(500);
|
delay(500);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -290,47 +306,41 @@ void loop()
|
|||||||
lqr_balance_loop(); // lqr自平衡控制，更新LQR_u
|
lqr_balance_loop(); // lqr自平衡控制，更新LQR_u
|
||||||
yaw_loop(); // yaw轴转向控制，更新YAW_output
|
yaw_loop(); // yaw轴转向控制，更新YAW_output
|
||||||
leg_loop(); // 腿部动作控制，控制舵机SyncWritePosEx，包括判断jump_loop
|
leg_loop(); // 腿部动作控制，控制舵机SyncWritePosEx，包括判断jump_loop
|
||||||
|
|
||||||
|
|
||||||
// 记录上一次的遥控数据数据
|
// 记录上一次的遥控数据数据
|
||||||
wrobot.dir_last = wrobot.dir;
|
wrobot.dir_last = wrobot.dir;
|
||||||
wrobot.joyx_last = wrobot.joyx;
|
wrobot.joyx_last = wrobot.joyx;
|
||||||
wrobot.joyy_last = wrobot.joyy;
|
wrobot.joyy_last = wrobot.joyy;
|
||||||
|
|
||||||
/* 将自平衡计算输出转矩赋给电机
|
// 将自平衡计算输出转矩赋给电机
|
||||||
motor.target 负数小车向前转，正数小车向后转，
|
// motor.target 负数小车向前转，正数小车向后转，
|
||||||
LQR_u 越大，电机输出的转矩越大，电机向后转，默认0-8V
|
|
||||||
LQR_u = angle_control + gyro_control + distance_control + speed_control
|
|
||||||
|
|
||||||
angle_control = pid_angle(LQR_angle - angle_zeropoint) //LQR_u与实际LQR_angle正相关
|
|
||||||
= pid_angle( mpu6050.getAngleY() - angle_zeropoint) //LQR_u与实际pitch正相关
|
|
||||||
gyro_control = pid_gyro(LQR_gyro) //LQR_u与实际LQR_gyro正相关
|
|
||||||
= pid_gyro( mpu6050.getGyroY() ) //LQR_u与实际pitch 角速度 正相关
|
|
||||||
|
|
||||||
所以这里LQR_distance和LQR_speed的计算乘以-0.5,不能单看符负号
|
|
||||||
distance_control = pid_distance(LQR_distance - distance_zeropoint) //LQR_u与电LQR_distance正相关
|
|
||||||
= pid_distance( (-0.5)*(motor1.shaft_angle + motor2.shaft_angle) - distance_zeropoint)
|
|
||||||
//在平衡车中，通常设定电机向后转（使车前进）为负，所以LQR_u与电机实际shaft_angle角度正相关，和负号没关系
|
|
||||||
|
|
||||||
speed_control = pid_speed(LQR_speed - 0.1 * lpf_joyy(wrobot.joyy)) // 实际速度-目标速度
|
|
||||||
//LQR_speed代表实际速度，LQR_u与实际LQR_speed速度正相关
|
|
||||||
= pid_speed( (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity) - 0.1 * lpf_joyy(wrobot.joyy))
|
|
||||||
//LQR_u与实际shaft_velocity速度正相关
|
|
||||||
*/
|
|
||||||
motor1.target = (-0.5) * (LQR_u + YAW_output); // 负数小车向前，正数小车向后，取
|
motor1.target = (-0.5) * (LQR_u + YAW_output); // 负数小车向前，正数小车向后，取
|
||||||
motor2.target = (-0.5) * (LQR_u - YAW_output);
|
motor2.target = (-0.5) * (LQR_u - YAW_output);
|
||||||
|
|
||||||
// 查看角度信息, 屁股向后倒 -36度，头向前到底 45度
|
// 查看角度信息, 屁股向后倒 -33~-34度,舵机2048的时候-42度，头向前到底 75度，完全垂直88度
|
||||||
// Serial.println(LQR_angle)
|
// Serial.println(LQR_angle);
|
||||||
|
Serial.println(angle_zeropoint);
|
||||||
|
|
||||||
// 向后倒地 -36度，向前倒地 45度，超出这个区间，或者跳跃中，马达就不要转动了
|
// 检测小车是否向后马上要倒地了，在中间过程重置所有参数
|
||||||
if(LQR_angle < -50.0f || LQR_angle > 80.0f ) {
|
loopCounter++;
|
||||||
motor1.target = 0;
|
// loopCounter作用：在刚开机的800次loop循环，不能进入resetToDefault()函数
|
||||||
motor2.target = 0;
|
// resetToDefaultCounter作用：防止小车倒地后某个角度，陷入循环，motor.target一直等于0，就躺地上不动了
|
||||||
|
if ((LQR_angle < -35.0f || LQR_angle > 70.0f )&& loopCounter > 800 && resetToDefaultCounter < 130) {
|
||||||
|
resetToDefault();
|
||||||
|
resetToDefaultCounter++;
|
||||||
|
motor1.target = 0;
|
||||||
|
motor2.target = 0;
|
||||||
|
// Serial.println("resetToDefault");
|
||||||
|
}
|
||||||
|
|
||||||
|
if( abs(LQR_angle) < 25.0f){
|
||||||
|
resetToDefaultCounter = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
// target目标电压 0~8V, 0为不转, 8为最大转速
|
// target目标电压 0~8V, 0为不转, 8为最大转速
|
||||||
// Serial.print("target电压:");
|
// Serial.print("LQR_angle:");
|
||||||
|
// Serial.print(LQR_angle);
|
||||||
|
// Serial.print(",motor1.target:");
|
||||||
// Serial.print(motor1.target);
|
// Serial.print(motor1.target);
|
||||||
// Serial.print(",motor2.target:");
|
// Serial.print(",motor2.target:");
|
||||||
// Serial.println(motor2.target);
|
// Serial.println(motor2.target);
|
||||||
@@ -359,9 +369,31 @@ void lqr_balance_loop()
|
|||||||
// + LQR_k4*LQR_speed;
|
// + LQR_k4*LQR_speed;
|
||||||
|
|
||||||
// 给负值是因为按照当前的电机接线，正转矩驱动小车电机向后转
|
// 给负值是因为按照当前的电机接线，正转矩驱动小车电机向后转
|
||||||
// 编码器反馈shaft_angle角度，shaft_velocity速度
|
|
||||||
// 不操作前后移动，譬如刚开机，车轮不动的，shaft_angle和shaft_velocity 都是零
|
/*
|
||||||
// 所以，不操作前后移动，LQR_distance 和 LQR_speed趋近于0，只有角度控制和角速度控制
|
LQR_u 越大，电机输出的转矩越大，电机向后转，默认0-8V
|
||||||
|
LQR_u = angle_control + gyro_control + distance_control + speed_control
|
||||||
|
|
||||||
|
编码器反馈shaft_angle角度，shaft_velocity速度
|
||||||
|
不操作前后移动，譬如刚开机，车轮不动的，shaft_angle和shaft_velocity 都是零
|
||||||
|
所以，不操作前后移动，LQR_distance 和 LQR_speed趋近于0，只有角度控制和角速度控制
|
||||||
|
|
||||||
|
angle_control = pid_angle(LQR_angle - angle_zeropoint) //LQR_u与实际LQR_angle正相关
|
||||||
|
= pid_angle( mpu6050.getAngleY() - angle_zeropoint) //LQR_u与实际pitch正相关
|
||||||
|
gyro_control = pid_gyro(LQR_gyro) //LQR_u与实际LQR_gyro正相关
|
||||||
|
= pid_gyro( mpu6050.getGyroY() ) //LQR_u与实际pitch 角速度 正相关
|
||||||
|
|
||||||
|
所以这里LQR_distance和LQR_speed的计算乘以-0.5,不能单看负号
|
||||||
|
distance_control = pid_distance(LQR_distance - distance_zeropoint) //LQR_u与电LQR_distance正相关
|
||||||
|
= pid_distance( (-0.5)*(motor1.shaft_angle + motor2.shaft_angle) - distance_zeropoint)
|
||||||
|
//在平衡车中，通常设定电机向后转（使车前进）为负，所以LQR_u与电机实际shaft_angle角度正相关，和负号没关系
|
||||||
|
|
||||||
|
speed_control = pid_speed(LQR_speed - 0.1 * lpf_joyy(wrobot.joyy)) // 实际速度-目标速度
|
||||||
|
//LQR_speed代表实际速度，LQR_u与实际LQR_speed速度正相关
|
||||||
|
= pid_speed( (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity) - 0.1 * lpf_joyy(wrobot.joyy))
|
||||||
|
//LQR_u与实际shaft_velocity速度正相关
|
||||||
|
*/
|
||||||
|
|
||||||
// 在平衡车中，通常设定电机向后转（使车前进）为负，所以这里LQR_distance和LQR_speed的计算乘以-0.5
|
// 在平衡车中，通常设定电机向后转（使车前进）为负，所以这里LQR_distance和LQR_speed的计算乘以-0.5
|
||||||
LQR_distance = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle); // 两个电机的旋转角度（shaft_angle）（是位置量）的平均值
|
LQR_distance = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle); // 两个电机的旋转角度（shaft_angle）（是位置量）的平均值
|
||||||
LQR_speed = (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity); // 两个电机角速度（shaft_velocity）的平均值，并取反
|
LQR_speed = (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity); // 两个电机角速度（shaft_velocity）的平均值，并取反
|
||||||
@@ -410,7 +442,7 @@ void lqr_balance_loop()
|
|||||||
robot_speed_last = robot_speed; // 记录连续两次的轮部转速
|
robot_speed_last = robot_speed; // 记录连续两次的轮部转速
|
||||||
robot_speed = LQR_speed;
|
robot_speed = LQR_speed;
|
||||||
// 若轮部角速度、角加速度"过大"（ > 10或> 50）,或处于跳跃后的恢复时期（jump_flag != 0），认为出现轮部离地现象，需要特殊处理
|
// 若轮部角速度、角加速度"过大"（ > 10或> 50）,或处于跳跃后的恢复时期（jump_flag != 0），认为出现轮部离地现象，需要特殊处理
|
||||||
//轮部离地情况下 LQR_u计算：2个参数，角度控制量+角速度控制量，对轮位移(distance)和速度(speed)控制分量不输出；反之，正常状态下完整输出平衡转矩
|
// 轮部离地情况下 LQR_u计算：2个参数，角度控制量+角速度控制量，对轮位移(distance)和速度(speed)控制分量不输出；反之，正常状态下完整输出平衡转矩
|
||||||
if (abs(robot_speed - robot_speed_last) > 10 || abs(robot_speed) > 50 || (jump_flag != 0))
|
if (abs(robot_speed - robot_speed_last) > 10 || abs(robot_speed) > 50 || (jump_flag != 0))
|
||||||
{
|
{
|
||||||
distance_zeropoint = LQR_distance; // 位移零点重置
|
distance_zeropoint = LQR_distance; // 位移零点重置
|
||||||
@@ -465,7 +497,7 @@ void leg_loop()
|
|||||||
{
|
{
|
||||||
|
|
||||||
// 倒地时强制腿部收缩
|
// 倒地时强制腿部收缩
|
||||||
if (LQR_angle < -36.0f || LQR_angle > 45.0f)
|
if (LQR_angle < -30.0f || LQR_angle > 45.0f)
|
||||||
{
|
{
|
||||||
Position[0] = SERVO1_MIN;
|
Position[0] = SERVO1_MIN;
|
||||||
Position[1] = SERVO2_MIN;
|
Position[1] = SERVO2_MIN;
|
||||||
@@ -551,6 +583,7 @@ void jump_loop()
|
|||||||
// 跳跃，腿部以最快速度伸到最长
|
// 跳跃，腿部以最快速度伸到最长
|
||||||
if ((wrobot.dir_last == 5) && (wrobot.dir == 4) && (jump_flag == 0))
|
if ((wrobot.dir_last == 5) && (wrobot.dir == 4) && (jump_flag == 0))
|
||||||
{
|
{
|
||||||
|
|
||||||
// 跳跃开始
|
// 跳跃开始
|
||||||
ACC[0] = 0;
|
ACC[0] = 0;
|
||||||
ACC[1] = 0;
|
ACC[1] = 0;
|
||||||
@@ -567,6 +600,7 @@ void jump_loop()
|
|||||||
{
|
{
|
||||||
jump_flag++;
|
jump_flag++;
|
||||||
|
|
||||||
|
|
||||||
if ((jump_flag > 30) && (jump_flag < 35))
|
if ((jump_flag > 30) && (jump_flag < 35))
|
||||||
{
|
{
|
||||||
ACC[0] = 0;
|
ACC[0] = 0;
|
||||||
@@ -721,4 +755,41 @@ void blinkLed() {
|
|||||||
delay(200);
|
delay(200);
|
||||||
digitalWrite(LED_BAT, LOW);
|
digitalWrite(LED_BAT, LOW);
|
||||||
delay(200);
|
delay(200);
|
||||||
|
}
|
||||||
|
|
||||||
|
// 重置到默认参数状态，用于自平衡起立
|
||||||
|
void resetToDefault() {
|
||||||
|
// LQR自平衡控制器参数
|
||||||
|
LQR_angle = 0;
|
||||||
|
LQR_gyro = 0;
|
||||||
|
angle_control = 0;
|
||||||
|
gyro_control = 0;
|
||||||
|
angle_zeropoint = 5;
|
||||||
|
|
||||||
|
LQR_speed = 0;
|
||||||
|
LQR_distance = 0;
|
||||||
|
speed_control = 0;
|
||||||
|
distance_control = 0;
|
||||||
|
distance_zeropoint = 1;
|
||||||
|
LQR_u = 0;
|
||||||
|
|
||||||
|
// YAW轴控制数据
|
||||||
|
YAW_gyro = 0;
|
||||||
|
YAW_angle = 0;
|
||||||
|
YAW_angle_last = 0;
|
||||||
|
YAW_angle_total = 0;
|
||||||
|
YAW_angle_zero_point = -10;
|
||||||
|
YAW_output = 0;
|
||||||
|
|
||||||
|
// 逻辑处理标志位
|
||||||
|
robot_speed = 0;
|
||||||
|
robot_speed_last = 0;
|
||||||
|
wrobot_move_stop_flag = 0;
|
||||||
|
jump_flag = 0;
|
||||||
|
leg_position_add = 0;
|
||||||
|
|
||||||
|
pid_lqr_u.error_prev = 0;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
Reference in new issue
Block a user