修复舵机发热问题
This commit is contained in:
myperworld committed
1 parent
2ccbd44b51
commit
3b94aa4d1c
5 files changed
+170
-48
No files matched your search
@@ -114,6 +114,12 @@ int resetToDefaultCounter = 0; // resetToDefault执行计数
|
||||
// 定义摇杆数值死区参数
|
||||
const int deadZone = 15;
|
||||
|
||||
// 小车状态控制
|
||||
bool robot_enabled = true; // 小车使能状态，true为起立状态，false为坐下状态
|
||||
bool last_robot_enabled = true; // 记录上一次的小车状态
|
||||
bool sitting_down = false; // 标记是否正在执行坐下动作
|
||||
float original_angle_zeropoint = 5; // 保存原始的角度零点
|
||||
|
||||
typedef struct {
|
||||
byte j1PotX; // 左摇杆X (0-255)
|
||||
byte j1PotY; // 左摇杆Y (0-255)
|
||||
@@ -156,6 +162,35 @@ void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
|
||||
// Serial.print(", pitch="); Serial.print(remoteData.pitch);
|
||||
// Serial.print(", buttonR3="); Serial.println(remoteData.buttonR3);
|
||||
|
||||
// 取反按钮值，使得默认显示 0 false，按下显示 1 true
|
||||
int convertedbuttonLB = !remoteData.buttonLB; // 顶部左
|
||||
int convertedbuttonRB = !remoteData.buttonRB; // 顶部右
|
||||
int convertedbuttonR1 = !remoteData.buttonR1; // 底部右按键（取反：默认0，按下1）
|
||||
int convertedbuttonR2 = !remoteData.buttonR2; // 底部左按键（取反：默认0，按下1）
|
||||
|
||||
|
||||
// 处理坐下/起立指令
|
||||
if (convertedbuttonR2 && robot_enabled) { // 底部左按键按下，坐下
|
||||
robot_enabled = false;
|
||||
sitting_down = true; // 标记开始坐下动作
|
||||
original_angle_zeropoint = angle_zeropoint; // 保存当前的角度零点
|
||||
}
|
||||
if (convertedbuttonR1 && !robot_enabled) { // 底部右按键按下，起立
|
||||
robot_enabled = true;
|
||||
sitting_down = false; // 清除坐下动作标记
|
||||
resetToDefault(); // 重置参数准备起立
|
||||
}
|
||||
|
||||
// 如果小车处于坐下状态或正在执行坐下动作，不处理其他指令
|
||||
if (!robot_enabled || sitting_down) {
|
||||
wrobot.dir = 4; // 强制设为"不跳跃"
|
||||
wrobot.joyx = 0; // 清空左右转向指令
|
||||
wrobot.joyy = 0; // 清空前后移动指令
|
||||
wrobot.roll = 0; // 清空左右摇摆指令
|
||||
wrobot.height = 0; // 清空高度指令
|
||||
return;
|
||||
}
|
||||
|
||||
// 跳跃状态不接受指令
|
||||
if( jump_flag > 0){
|
||||
wrobot.dir = 4; // 强制设为“不跳跃”
|
||||
@@ -175,11 +210,6 @@ void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
|
||||
// 前后速度
|
||||
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; // 顶部右
|
||||
int convertedbuttonR1 = !remoteData.buttonR1; // 底部右按键（取反：默认0，按下1）
|
||||
int convertedbuttonR2 = !remoteData.buttonR2; // 底部左按键（取反：默认0，按下1）
|
||||
|
||||
// 跳跃
|
||||
if (convertedbuttonRB) { // 1=true，0=false
|
||||
@@ -328,12 +358,75 @@ void setup()
|
||||
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) {
|
||||
// 设置电机目标为正，使小车向后运动
|
||||
motor1.target = -1.0; // 2V电压，向后转（轮子向前转，小车向后运动）
|
||||
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;
|
||||
Speed[0] = 100;
|
||||
Speed[1] = 100;
|
||||
ACC[0] = 5;
|
||||
ACC[1] = 5;
|
||||
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
|
||||
// jump_loop();
|
||||
|
||||
// 记录上一次的遥控数据数据
|
||||
wrobot.dir_last = wrobot.dir;
|
||||
@@ -360,7 +453,7 @@ void loop()
|
||||
= 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);
|
||||
|
||||
// 查看角度信息, 屁股向后倒 -33~-34度,舵机2048的时候-42度，头向前到底 75度，完全垂直88度
|
||||
|
||||
Reference in new issue
Block a user