deepseek Bot
5d9e5f73b0
- Replace wifi.cpp/wifi.h with wifi_manager.cpp/wifi_manager.h: AP and STA run together, external WiFi credentials are stored in NVS and editable at /wifi, with optional static IP, 15s auto-reconnect, disconnect reason reporting, and build/version diagnostics - Fix MPU6050 I2C port to I2C0 to match the board wiring - Initialize distance_zeropoint at startup and on Robot Go! rising edge - Clamp angle_zeropoint to +/-10 degrees to prevent balance-point drift - Add DEBUG_DISABLE_YAW switch and periodic balance debug output - Update WiFi setup instructions in README.md and README_CN.md
598 lines
18 KiB
Arduino
598 lines
18 KiB
Arduino
// -----------------------------------------------------------------------------
|
||
// Copyright (c) 2024 Mu Shibo
|
||
//
|
||
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
||
// of this software and associated documentation files (the "Software"), to deal
|
||
// in the Software without restriction, including without limitation the rights
|
||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||
// copies of the Software, and to permit persons to whom the Software is
|
||
// furnished to do so, subject to the following conditions:
|
||
//
|
||
// The above copyright notice and this permission notice shall be included in
|
||
// all copies or substantial portions of the Software.
|
||
//
|
||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||
// THE SOFTWARE.
|
||
// -----------------------------------------------------------------------------
|
||
|
||
//机器人控制头文件
|
||
#include <MPU6050_tockn.h>
|
||
#include "Servo_STS3032.h"
|
||
#include <SimpleFOC.h>
|
||
#include <Arduino.h>
|
||
|
||
//wifi控制数据传输头文件
|
||
#include <WebSocketsServer.h>
|
||
#include <ArduinoJson.h>
|
||
#include <WebServer.h>
|
||
#include <WiFi.h>
|
||
#include <FS.h>
|
||
#include "basic_web.h"
|
||
#include "robot.h"
|
||
#include "wifi_manager.h"
|
||
#include "esp_adc_cal.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 {.P = 1, .I = 0, .D = 0, .ramp = 100000, .limit = 8};
|
||
PIDController pid_gyro {.P = 0.06, .I = 0, .D = 0, .ramp = 100000, .limit = 8};
|
||
PIDController pid_distance {.P = 0.5, .I = 0, .D = 0, .ramp = 100000, .limit = 8};
|
||
PIDController pid_speed {.P = 0.7, .I = 0, .D = 0, .ramp = 100000, .limit = 8};
|
||
PIDController pid_yaw_angle {.P = 1.0, .I = 0, .D = 0, .ramp = 100000, .limit = 8};
|
||
PIDController pid_yaw_gyro {.P = 0.04, .I = 0, .D = 0, .ramp = 100000, .limit = 8};
|
||
PIDController pid_lqr_u {.P = 1, .I = 15, .D = 0, .ramp = 100000, .limit = 8};
|
||
PIDController pid_zeropoint {.P = 0.002,.I = 0, .D = 0, .ramp = 100000, .limit = 4};
|
||
PIDController pid_roll_angle{.P = 8, .I = 0, .D = 0, .ramp = 100000, .limit = 450};
|
||
|
||
//低通滤波器实例
|
||
LowPassFilter lpf_joyy{.Tf = 0.2};
|
||
LowPassFilter lpf_zeropoint{.Tf = 0.1};
|
||
LowPassFilter lpf_roll{.Tf = 0.3};
|
||
|
||
// commander通信实例
|
||
Commander command = Commander(Serial);
|
||
|
||
void StabAngle(char* cmd) { command.pid(&pid_angle, cmd); }
|
||
void StabGyro(char* cmd) { command.pid(&pid_gyro, cmd); }
|
||
void StabDistance(char* cmd) { command.pid(&pid_distance, cmd); }
|
||
void StabSpeed(char* cmd) { command.pid(&pid_speed, cmd); }
|
||
void StabYawAngle(char* cmd) { command.pid(&pid_yaw_angle, cmd); }
|
||
void StabYawGyro(char* cmd) { command.pid(&pid_yaw_gyro, cmd); }
|
||
void lpfJoyy(char* cmd) { command.lpf(&lpf_joyy, cmd); }
|
||
void StabLqrU(char* cmd) { command.pid(&pid_lqr_u, cmd); }
|
||
void StabZeropoint(char* cmd) { command.pid(&pid_zeropoint, cmd); }
|
||
void lpfZeropoint(char* cmd) { command.lpf(&lpf_zeropoint, cmd); }
|
||
void StabRollAngle(char* cmd) { command.pid(&pid_roll_angle, cmd);}
|
||
void lpfRoll(char* cmd) { command.lpf(&lpf_roll, cmd); }
|
||
|
||
//void Stabtest_zeropoint(char* cmd) { command.pid(&test_zeropoint, cmd); }
|
||
|
||
//WebServer实例
|
||
WebServer webserver; // server服务器
|
||
WebSocketsServer websocket = WebSocketsServer(81); // 定义一个webSocket服务器来处理客户发送的消息
|
||
RobotProtocol rp(20);
|
||
int joystick_value[2];
|
||
|
||
//STS舵机实例
|
||
SMS_STS sms_sts;
|
||
|
||
//MPU6050实例
|
||
MPU6050 mpu6050(I2Cone);
|
||
|
||
/************参数定义*************/
|
||
#define pi 3.1415927
|
||
|
||
//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 = -2.25;
|
||
float distance_zeropoint = -256.0; //轮部位移零点偏置（-256为一个不可能的位移值，将其作为未刷新的标志）
|
||
|
||
//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轴平衡控制量
|
||
int uncontrolable = 0; //机身倾角过大导致失控
|
||
|
||
//【调试】测试平衡方向时置 true，屏蔽 YAW 差速，避免转向干扰轮子转向判断；测试完成改回 false
|
||
const bool DEBUG_DISABLE_YAW = false;
|
||
|
||
//电压检测
|
||
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
|
||
|
||
void setup() {
|
||
Serial.begin(115200);//通讯串口
|
||
Serial2.begin(1000000);//腿部sts舵机
|
||
|
||
//Wifi初始化：AP热点 + 外部WiFi（STA）
|
||
WiFi_Init();
|
||
WiFi_RegisterWebHandlers(webserver);
|
||
webserver.on("/", HTTP_GET, basicWebCallback);
|
||
webserver.begin();
|
||
websocket.begin();
|
||
websocket.onEvent(webSocketEventCallback);
|
||
|
||
//舵机初始化
|
||
//舵机有效行程450
|
||
//左侧舵机[2048+12+50,2048+12+450]
|
||
//左侧舵机[2048-12-50,2048-12-450]
|
||
sms_sts.pSerial = &Serial2;
|
||
ID[0] = 1;
|
||
ID[1] = 2;
|
||
ACC[0] = 30;
|
||
ACC[1] = 30;
|
||
Speed[0] = 300;
|
||
Speed[1] = 300;
|
||
Position[0] = 2148;
|
||
Position[1] = 1948;
|
||
//舵机(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();
|
||
|
||
//【调试】初始化位移零点，避免 distance_zeropoint=-256 造成的启动冲击
|
||
distance_zeropoint = -0.5f * (motor1.shaft_angle + motor2.shaft_angle);
|
||
|
||
// 映射电机到commander
|
||
command.add('A', StabAngle, "pid angle");
|
||
command.add('B', StabGyro, "pid gyro");
|
||
command.add('C', StabDistance, "pid distance");
|
||
command.add('D', StabSpeed, "pid speed");
|
||
command.add('E', StabYawAngle, "pid yaw angle");
|
||
command.add('F', StabYawGyro, "pid yaw gyro");
|
||
command.add('G', lpfJoyy, "lpf joyy");
|
||
command.add('H', StabLqrU, "pid lqr u");
|
||
command.add('I', StabZeropoint, "pid zeropoint");
|
||
command.add('J', lpfZeropoint, "lpf zeropoint");
|
||
command.add('K', StabRollAngle, "pid roll angle");
|
||
command.add('L', lpfRoll, "lpf roll");
|
||
|
||
//command.add('M', Stabtest_zeropoint, "test_zeropoint");
|
||
|
||
|
||
|
||
delay(500);
|
||
}
|
||
|
||
void loop() {
|
||
bat_check(); //电压检测
|
||
web_loop(); //Web数据更新
|
||
mpu6050.update(); //IMU数据更新
|
||
|
||
//勾选 Robot Go! 的上升沿：以当前姿态作为新的航向/位移零点
|
||
static bool go_last = false;
|
||
if (wrobot.go && !go_last) {
|
||
YAW_angle_total = 0;
|
||
YAW_angle_last = mpu6050.getAngleZ();
|
||
distance_zeropoint = -0.5f * (motor1.shaft_angle + motor2.shaft_angle);
|
||
}
|
||
go_last = wrobot.go;
|
||
|
||
lqr_balance_loop(); //lqr自平衡控制
|
||
|
||
//【调试】每100ms输出一次平衡状态
|
||
static uint32_t t_dbg = 0;
|
||
if (millis() - t_dbg > 100) {
|
||
t_dbg = millis();
|
||
Serial.printf("ang=%.2f gyro=%.2f zp=%.2f dist=%.2f u=%.2f spd=%.2f unc=%d\n",
|
||
LQR_angle, LQR_gyro, angle_zeropoint, distance_control, LQR_u, LQR_speed, uncontrolable);
|
||
}
|
||
|
||
yaw_loop(); //yaw轴转向控制
|
||
leg_loop(); //腿部动作控制
|
||
|
||
//将自平衡计算输出转矩赋给电机
|
||
const float yaw_out = DEBUG_DISABLE_YAW ? 0.0f : YAW_output;
|
||
motor1.target = (-0.5)*(LQR_u + yaw_out);
|
||
motor2.target = (-0.5)*(LQR_u - yaw_out);
|
||
|
||
//倒地失控后关闭输出
|
||
if( abs(LQR_angle) > 25.0f )
|
||
{
|
||
uncontrolable = 1;
|
||
}
|
||
if( uncontrolable != 0 )//扶起后延时恢复
|
||
{
|
||
if( abs(LQR_angle) < 10.0f )
|
||
{
|
||
uncontrolable++;
|
||
}
|
||
if( uncontrolable > 200 )//200次程序循环的延时时间
|
||
{
|
||
uncontrolable = 0;
|
||
}
|
||
}
|
||
|
||
//关停输出（遥控停止或角度过大失控）
|
||
if(wrobot.go==0 || uncontrolable!=0)
|
||
{
|
||
motor1.target = 0;
|
||
motor2.target = 0;
|
||
leg_position_add = 0;
|
||
}
|
||
|
||
//记录上一次的遥控数据数据
|
||
wrobot.dir_last = wrobot.dir;
|
||
wrobot.joyx_last = wrobot.joyx;
|
||
wrobot.joyy_last = wrobot.joyy;
|
||
|
||
//迭代计算FOC相电压
|
||
motor1.loopFOC();
|
||
motor2.loopFOC();
|
||
|
||
//设置轮部电机输出
|
||
motor1.move();
|
||
motor2.move();
|
||
|
||
command.run();
|
||
}
|
||
|
||
//lqr自平衡控制
|
||
void lqr_balance_loop(){
|
||
//LQR平衡算式，实际使用中为便于调参，讲算式分解为4个P控制，采用PIDController方法在commander中实时调试
|
||
//QR_u = LQR_k1*(LQR_angle - angle_zeropoint) + LQR_k2*LQR_gyro + LQR_k3*(LQR_distance - distance_zeropoint) + LQR_k4*LQR_speed;
|
||
|
||
//给负值是因为按照当前的电机接线，正转矩会向后转
|
||
LQR_distance = (-0.5) *(motor1.shaft_angle + motor2.shaft_angle);
|
||
LQR_speed = (-0.5) *(motor1.shaft_velocity + motor2.shaft_velocity);
|
||
LQR_angle = (float)mpu6050.getAngleY();
|
||
LQR_gyro = (float)mpu6050.getGyroY();
|
||
//Serial.println(LQR_distance);
|
||
|
||
//计算自平衡输出
|
||
angle_control = pid_angle(LQR_angle - angle_zeropoint);
|
||
gyro_control = pid_gyro(LQR_gyro);
|
||
|
||
//运动细节优化处理
|
||
if(wrobot.joyy != 0)//有前后方向运动指令时的处理
|
||
{
|
||
distance_zeropoint = LQR_distance;//位移零点重置
|
||
pid_lqr_u.error_prev = 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;//位移零点重置
|
||
}
|
||
|
||
//计算位移控制输出
|
||
distance_control = pid_distance(LQR_distance - distance_zeropoint);
|
||
speed_control = pid_speed(LQR_speed- 0.1*lpf_joyy(wrobot.joyy) );
|
||
|
||
//轮部离地检测
|
||
robot_speed_last = robot_speed; //记录连续两次的轮部转速
|
||
robot_speed = LQR_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 = angle_control + gyro_control + distance_control + speed_control;
|
||
}
|
||
|
||
//触发条件：遥控器无信号输入、轮部位移控制正常介入、不处于跳跃后的恢复时期
|
||
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);
|
||
angle_zeropoint -= pid_zeropoint(lpf_zeropoint(distance_control));//重心自适应
|
||
angle_zeropoint = constrain(angle_zeropoint, -10.0f, 10.0f); //限幅，防止平衡点跑飞
|
||
}
|
||
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(){
|
||
jump_loop();
|
||
if(jump_flag == 0)//不处于跳跃状态
|
||
{
|
||
//机身高度自适应控制
|
||
ACC[0] = 8;
|
||
ACC[1] = 8;
|
||
Speed[0] = 200;
|
||
Speed[1] = 200;
|
||
float roll_angle = (float)mpu6050.getAngleX() + 2.0;
|
||
//leg_position_add += pid_roll_angle(roll_angle);
|
||
leg_position_add = pid_roll_angle(lpf_roll(roll_angle));//test
|
||
Position[0] = 2048 + 12 + 8.4*(wrobot.height-32) - leg_position_add;
|
||
Position[1] = 2048 - 12 - 8.4*(wrobot.height-32) - leg_position_add;
|
||
if( Position[0]<2110 )
|
||
Position[0]=2110;
|
||
else if( Position[0]>2510 )
|
||
Position[0]=2510;
|
||
if( Position[1]<1586 )
|
||
Position[1]=1586;
|
||
else if( Position[1]>1986 )
|
||
Position[1]=1986;
|
||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||
}
|
||
}
|
||
|
||
//跳跃控制
|
||
void jump_loop(){
|
||
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] = 2048 + 12 + 8.4*(80-32);
|
||
Position[1] = 2048 - 12 - 8.4*(80-32);
|
||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||
|
||
jump_flag = 1;
|
||
}
|
||
if( jump_flag > 0 )
|
||
{
|
||
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*(40-32);
|
||
Position[1] = 2048 - 12 - 8.4*(40-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.002;
|
||
float yaw_angle_control = pid_yaw_angle(YAW_angle_total);
|
||
float yaw_gyro_control = pid_yaw_gyro(YAW_gyro);
|
||
YAW_output = yaw_angle_control + yaw_gyro_control;
|
||
}
|
||
|
||
//Web数据更新
|
||
void web_loop(){
|
||
WiFi_Loop(); //WiFi状态维护/后台重连
|
||
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++;
|
||
}
|