508 lines
18 KiB
C++
508 lines
18 KiB
C++
#include <Arduino.h>
|
||
|
||
// ESP-NOW/WiFi相关头文件
|
||
// #include <esp_now.h>
|
||
// #include <WiFi.h>
|
||
// #include "esp_wifi.h"
|
||
|
||
#include "main.h"
|
||
|
||
// 蓝牙相关头文件
|
||
#include <BLEDevice.h>
|
||
#include <BLEUtils.h>
|
||
#include <BLEServer.h>
|
||
|
||
// 蓝牙服务与特征UUID
|
||
#define SERVICE_UUID "6e400001-b5a3-f393-e0a9-e50e24dcca9e"
|
||
#define CHARACTERISTIC_UUID "6e400002-b5a3-f393-e0a9-e50e24dcca9e"
|
||
|
||
|
||
/************实例定义*************/
|
||
|
||
// 电机实例
|
||
// 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 LEG_HEIGHT_BASE 20 // 腿部默认高度基准值，根据公式值越小，机身越高， 实测范围 0（最高），52（最低）
|
||
// #define SERVO1_MIN (2048 + 12 + 8.4 * (35 - 52)) // 舵机1最低 从小车正面看右手
|
||
// #define SERVO2_MIN (2048 - 12 - 8.4 * (35 - 52)) // 舵机2最低 从小车正面看左手
|
||
// #define SERVO1_MAX (2048 + 12 + 8.4 * (35 - 0)) // 舵机1最高
|
||
// #define SERVO2_MAX (2048 - 12 - 8.4 * (35 - 0)) // 舵机2最高
|
||
|
||
// // 腿部舵机全局变量
|
||
// int lastPosition0 = 2048;
|
||
// int lastPosition1 = 2048;
|
||
// #define SERVO_DEAD_ZONE 5 // 设置死区阈值 最小更新间隔(ms)
|
||
|
||
// LQR自平衡控制器参数
|
||
// float LQR_angle = 0;
|
||
// float LQR_gyro = 0;
|
||
// float LQR_gyroZ = 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; // 5 设定默认俯仰角度，向前到正，向后倒负
|
||
// float original_angle_zeropoint = 5; // 保存原始的角度零点
|
||
// float distance_zeropoint = 0.5f; // 轮部位移零点偏置
|
||
|
||
// // 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;
|
||
|
||
// int loopCounter = 0; // loop函数执行计数
|
||
// int resetToDefaultCounter = 0; // resetToDefault执行计数
|
||
|
||
// // 电量显示LED
|
||
// #define LED_BAT 13
|
||
|
||
// // 定义摇杆数值死区参数
|
||
// const int deadZone = 15;
|
||
|
||
// // 小车状态控制
|
||
// bool robot_enabled = true; // 小车使能状态，true为起立状态，false为坐下状态
|
||
// bool last_robot_enabled = true; // 记录上一次的小车状态
|
||
// bool sitting_down = false; // 标记是否正在执行坐下动作
|
||
|
||
// // 向前跳跃控制变量
|
||
// int forward_jump_flag = 0; // 向前跳跃状态标志
|
||
|
||
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; // 默认 127
|
||
byte pitch; // 默认 127
|
||
bool buttonR3; // 默认 1
|
||
} JoystickData;
|
||
|
||
// 蓝灯爆闪
|
||
bool lastbuttonLB = false;
|
||
|
||
// 蓝牙变量
|
||
BLECharacteristic *pCharacteristic; //蓝牙特性
|
||
bool deviceConnected = false; //判断连接
|
||
|
||
// 回调函数：用于监听客户端连接和断开
|
||
class MyServerCallbacks: public BLEServerCallbacks {
|
||
void onConnect(BLEServer* pServer) {
|
||
deviceConnected = true;
|
||
Serial.println("客户端已连接");
|
||
}
|
||
|
||
void onDisconnect(BLEServer* pServer) {
|
||
deviceConnected = false;
|
||
Serial.println("客户端已断开");
|
||
// 重新开始广播，让设备可被再次扫描
|
||
pServer->getAdvertising()->start();
|
||
}
|
||
};
|
||
|
||
std::string receivedValue; //传回数据
|
||
String resStr;//保存读取到的蓝牙信号
|
||
int Checkcomma; //判断是否含有逗号分隔
|
||
int length=0;//数据长度
|
||
float newSteering;
|
||
float newThrottle;
|
||
|
||
// 回调函数：用于监听客户端对特性的读写操作
|
||
class MyCallbacks: public BLECharacteristicCallbacks {
|
||
void onWrite(BLECharacteristic *pCharacteristic) {
|
||
receivedValue = pCharacteristic->getValue();
|
||
|
||
// ---------------------- 新增：打印原始接收信息 ----------------------
|
||
Serial.printf("\n【蓝牙接收数据】原始数据长度：%d 字节\n", receivedValue.length());
|
||
// 打印原始字节流（十六进制，方便排查数据是否完整）
|
||
Serial.print("【蓝牙接收数据】原始字节流（Hex）：");
|
||
for (int i = 0; i < receivedValue.length(); i++) {
|
||
Serial.printf("%02X ", (uint8_t)receivedValue[i]);
|
||
}
|
||
Serial.println();
|
||
// ------------------------------------------------------------------
|
||
|
||
}
|
||
};
|
||
|
||
void bledInit(){
|
||
Serial.println("初始化 BLE...");
|
||
|
||
// 初始化 BLE 设备
|
||
BLEDevice::init("平衡车");
|
||
|
||
// 创建 BLE 服务器
|
||
BLEServer *pServer = BLEDevice::createServer();
|
||
pServer->setCallbacks(new MyServerCallbacks());
|
||
|
||
// 创建 BLE 服务
|
||
BLEService *pService = pServer->createService(SERVICE_UUID);
|
||
|
||
// 创建 BLE 特性 (读/写)
|
||
pCharacteristic = pService->createCharacteristic(
|
||
CHARACTERISTIC_UUID,
|
||
BLECharacteristic::PROPERTY_READ |
|
||
BLECharacteristic::PROPERTY_WRITE
|
||
);
|
||
|
||
// 设置特性的回调监听
|
||
pCharacteristic->setCallbacks(new MyCallbacks());
|
||
|
||
// 设置初始值
|
||
pCharacteristic->setValue("欢迎来到平衡车");
|
||
|
||
// 启动服务
|
||
pService->start();
|
||
|
||
// 开始广播，使设备可以被扫描到
|
||
pServer->getAdvertising()->start();
|
||
Serial.println("等待客户端连接...");
|
||
}
|
||
|
||
void setup()
|
||
{
|
||
Serial.begin(115200); // 通讯串口
|
||
// Serial2.begin(1000000); // 腿部sts舵机
|
||
|
||
// 模式1：适配遥控器模式，设置WiFi通道为6
|
||
// WiFi.mode(WIFI_STA); // 设置为STA模式
|
||
// 设置WiFi通道为6，为了适配遥控器模式
|
||
// esp_wifi_set_channel(6, WIFI_SECOND_CHAN_NONE);
|
||
|
||
// 模式2：适配小车wifi热点模式
|
||
// WiFi_SetAP(); // 打开这行注释就能开启热点模式
|
||
|
||
// ---------------------- 新增蓝牙初始化 ----------------------
|
||
bledInit();
|
||
// ------------------------------------------------------------
|
||
|
||
// webserver.begin();
|
||
// webserver.on("/", HTTP_GET, basicWebCallback);
|
||
// websocket.begin();
|
||
// websocket.onEvent(webSocketEventCallback);
|
||
|
||
// 初始化ESP-NOW
|
||
// if (esp_now_init() != ESP_OK) {
|
||
// Serial.println("Error initializing ESP-NOW");
|
||
// return;
|
||
// }
|
||
// esp_now_register_recv_cb(OnDataRecv);
|
||
|
||
// Serial.print("WIFI channel:");
|
||
// Serial.println(WiFi.channel());
|
||
|
||
// 舵机初始化
|
||
// 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()
|
||
{
|
||
|
||
// // 检测小车状态变化 坐 -> 站
|
||
// if (robot_enabled && !last_robot_enabled) {
|
||
// // 从坐下状态变为起立状态，重置参数
|
||
// resetToDefault();
|
||
// }
|
||
// last_robot_enabled = robot_enabled;
|
||
|
||
// // 如果小车处于坐下状态，只执行必要的功能
|
||
// if (!robot_enabled) {
|
||
// bat_check(); // 电压检测
|
||
// web_loop(); // Web数据更新
|
||
// mpu6050.update(); // IMU数据更新
|
||
|
||
// // 如果正在执行坐下动作，逐步调整角度零点让小车向后倒
|
||
// if (sitting_down) {
|
||
// // 缓慢调整角度零点，使小车向后倒
|
||
// static int sit_counter = 0;
|
||
// sit_counter++;
|
||
|
||
// // 先给个向后退脉冲
|
||
// if (sit_counter < 30) {
|
||
// // 阶段1：设置电机目标为负数，使小车向后运动
|
||
// motor1.target = -1.0; // 负数小车向后，正数向前
|
||
// motor2.target = -1.0;
|
||
// } else {
|
||
// // 阶段2：停止电机，让小车自然向后倒
|
||
// motor1.target = 0;
|
||
// motor2.target = 0;
|
||
// }
|
||
// motor1.loopFOC();
|
||
// motor2.loopFOC();
|
||
// motor1.move();
|
||
// motor2.move();
|
||
|
||
// // 当sit_counter为0时，设置舵机到最低位置
|
||
// if (sit_counter == 0) {
|
||
// Position[0] = SERVO1_MIN;
|
||
// Position[1] = SERVO2_MIN;
|
||
// ACC[0] = 5;
|
||
// ACC[1] = 5;
|
||
// Speed[0] = 100;
|
||
// Speed[1] = 100;
|
||
// sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||
// }
|
||
|
||
// if (sit_counter < 100) { // 分100步完成坐下动作
|
||
// // 逐步将平衡零点变成负数，小车抬头（向后倒）
|
||
// angle_zeropoint -- ;
|
||
// } else {
|
||
// // 坐下动作完成，设置到最低位置和最终角度零点
|
||
// sit_counter = 0;
|
||
// sitting_down = false;
|
||
// angle_zeropoint = -20; // 最终的角度零点，确保小车向后倒
|
||
// }
|
||
// } else {
|
||
// // 已经坐下，保持最低位置和向后倒的角度零点
|
||
// angle_zeropoint = -15; // 保持向后倒的角度零点
|
||
// }
|
||
|
||
// return; // 跳过其他控制
|
||
// }
|
||
|
||
// bat_check(); // 电压检测 <1ms
|
||
// web_loop(); // Web数据更新 <2ms
|
||
// mpu6050.update(); // IMU数据更新 <1ms
|
||
// lqr_balance_loop(); // lqr自平衡控制，更新LQR_u ★ 关键实时控制 2-3ms
|
||
// yaw_loop(); // yaw轴转向控制，更新YAW_output ★ 关键实时控制 1-2ms
|
||
// leg_loop(); // 腿部动作控制，控制舵机SyncWritePosEx，包括判断jump_loop 舵机控制 1ms
|
||
|
||
// // 记录上一次的遥控数据数据
|
||
// 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);
|
||
|
||
// // 查看角度信息, 屁股向后倒 -33~-34度,舵机2048的时候-42度，头向前到底 75度，完全垂直88度
|
||
// // Serial.println(LQR_angle);
|
||
|
||
// // 检测小车是否向后马上要倒地了，在中间过程重置所有参数
|
||
// loopCounter++;
|
||
// // loopCounter作用：在期初的800次循环内视为电源开通后第一次自平衡，不能进入resetToDefault()函数重置参数
|
||
// // resetToDefaultCounterz作用：防止小车倒地后某个角度，陷入循环，motor.target一直=0
|
||
// if ((LQR_angle < -35.0f || LQR_angle > 70.0f ) && loopCounter > 800 && resetToDefaultCounter < 130) {
|
||
// resetToDefault();
|
||
// resetToDefaultCounter++;
|
||
// motor1.target = 0;
|
||
// motor2.target = 0;
|
||
// }
|
||
|
||
// if( abs(LQR_angle) < 25.0f){
|
||
// resetToDefaultCounter = 0;
|
||
// }
|
||
|
||
// // target目标电压 0~8V, 0为不转, 8为最大转速
|
||
// // Serial.print("LQR_angle:");
|
||
// // Serial.println(LQR_angle);
|
||
// // // Serial.print(",loopCounter:");
|
||
// // // Serial.print(loopCounter);
|
||
// // Serial.print(",resetToDefaultCounter:");
|
||
// // Serial.print(resetToDefaultCounter);
|
||
// // // Serial.print(",motor1.target:");
|
||
// // // Serial.print(motor1.target);
|
||
// // Serial.print(",motor2.target:");
|
||
// // Serial.println(motor2.target);
|
||
|
||
// // 监测速度变化
|
||
// // int speed_delta = robot_speed - robot_speed_last;
|
||
// // if(speed_delta > 0){
|
||
// // Serial.print("speed_delta:");
|
||
// // Serial.print(speed_delta);
|
||
// // Serial.print(",robot_speed:");
|
||
// // Serial.println(robot_speed);
|
||
// // }
|
||
|
||
// // 处理向前跳跃
|
||
// if (forward_jump_flag) {
|
||
// forward_jump_flag++;
|
||
// if (forward_jump_flag < 25) {
|
||
// // 保持向前脉冲
|
||
// motor1.target = 1.8;
|
||
// motor2.target = 1.8;
|
||
// } else {
|
||
// // 跳跃结束，重置状态
|
||
// forward_jump_flag = 0;
|
||
// wrobot.dir = 5; // 跳跃
|
||
// }
|
||
// }
|
||
|
||
// // 迭代计算FOC相电压
|
||
// motor1.loopFOC();
|
||
// motor2.loopFOC();
|
||
|
||
// // 设置轮部电机输出
|
||
// motor1.move();
|
||
// motor2.move();
|
||
|
||
} // loop()
|
||
|