deepseek/Micro-Wheeled_leg-Robot
1
0 Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
2408ed027353c13922fcb51d5f7298f88051950f
Micro-Wheeled_leg-Robot/3.Software/wl_pro_robot/wl_pro_robot.ino
T
MuShibo e64e304b9c Open source demo.
2024-07-20 18:36:35 +08:00

569 lines
17 KiB
Arduino
Raw Blame History

// -----------------------------------------------------------------------------
// 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.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(I2Ctwo);
/************参数定义*************/
#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; //机身倾角过大导致失控
//电压检测
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初始化
//WiFi_SetAP();
set_sta_wifi(); // ESP-01S STA模式接入WiFi网络
webserver.begin();
webserver.on("/", HTTP_GET, basicWebCallback);
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();
// 映射电机到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数据更新
lqr_balance_loop(); //lqr自平衡控制
yaw_loop(); //yaw轴转向控制
leg_loop(); //腿部动作控制
//将自平衡计算输出转矩赋给电机
motor1.target = (-0.5)*(LQR_u + YAW_output);
motor2.target = (-0.5)*(LQR_u - YAW_output);
//倒地失控后关闭输出
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));//重心自适应
}
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(){
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++;
}
Reference in new issue View Git Blame Copy Permalink