// ----------------------------------------------------------------------------- // 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 #include "Servo_STS3032.h" #include #include //wifi控制数据传输头文件 #include #include #include #include #include #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++; }