优化跳跃功能，可以支持连续跳跃，不倒地。
This commit is contained in:
myperworld committed
1 parent
9f73afb56b
commit
5a0bbef43b
5 files changed
+165
-1631
No files matched your search
+106
-35
@@ -103,6 +103,9 @@ 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
|
||||
|
||||
@@ -148,6 +151,16 @@ void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
|
||||
// 取反按钮值，使得默认显示 0 false，按下显示 1 true
|
||||
int convertedbuttonLB = !remoteData.buttonLB; // 顶部左
|
||||
int convertedbuttonRB = !remoteData.buttonRB; // 顶部右
|
||||
int convertedbuttonR1 = !remoteData.buttonR1; // 底部右
|
||||
int convertedbuttonR2 = !remoteData.buttonR2; // 底部左
|
||||
|
||||
// 改变倾角
|
||||
// if(convertedbuttonR1){
|
||||
// angle_zeropoint++;
|
||||
// }
|
||||
// if(convertedbuttonR2){
|
||||
// angle_zeropoint--;
|
||||
// }
|
||||
|
||||
// 跳跃
|
||||
if (convertedbuttonRB) { // 1=true，0=false
|
||||
@@ -214,8 +227,10 @@ void setup()
|
||||
ACC[1] = 30;
|
||||
Speed[0] = 400;
|
||||
Speed[1] = 400;
|
||||
Position[0] = 2048;
|
||||
Position[1] = 2048;
|
||||
// Position[0] = 2048;
|
||||
// Position[1] = 2048;
|
||||
Position[0] = SERVO1_MIN;
|
||||
Position[1] = SERVO2_MIN;
|
||||
// 舵机(ID1/ID2)以最高速度V=2400步/秒，加速度A=50(50*100步/秒^2)，运行至各自的Position位置
|
||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||
|
||||
@@ -278,6 +293,7 @@ void setup()
|
||||
motor2.init();
|
||||
motor2.initFOC();
|
||||
|
||||
resetToDefault(); // 重置到默认参数状态
|
||||
delay(500);
|
||||
}
|
||||
|
||||
@@ -290,47 +306,41 @@ void loop()
|
||||
lqr_balance_loop(); // lqr自平衡控制，更新LQR_u
|
||||
yaw_loop(); // yaw轴转向控制，更新YAW_output
|
||||
leg_loop(); // 腿部动作控制，控制舵机SyncWritePosEx，包括判断jump_loop
|
||||
|
||||
|
||||
// 记录上一次的遥控数据数据
|
||||
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速度正相关
|
||||
*/
|
||||
// 将自平衡计算输出转矩赋给电机
|
||||
// motor.target 负数小车向前转，正数小车向后转，
|
||||
motor1.target = (-0.5) * (LQR_u + YAW_output); // 负数小车向前，正数小车向后，取
|
||||
motor2.target = (-0.5) * (LQR_u - YAW_output);
|
||||
|
||||
// 查看角度信息, 屁股向后倒 -36度，头向前到底 45度
|
||||
// Serial.println(LQR_angle)
|
||||
// 查看角度信息, 屁股向后倒 -33~-34度,舵机2048的时候-42度，头向前到底 75度，完全垂直88度
|
||||
// Serial.println(LQR_angle);
|
||||
Serial.println(angle_zeropoint);
|
||||
|
||||
// 向后倒地 -36度，向前倒地 45度，超出这个区间，或者跳跃中，马达就不要转动了
|
||||
if(LQR_angle < -50.0f || LQR_angle > 80.0f ) {
|
||||
motor1.target = 0;
|
||||
motor2.target = 0;
|
||||
// 检测小车是否向后马上要倒地了，在中间过程重置所有参数
|
||||
loopCounter++;
|
||||
// loopCounter作用：在刚开机的800次loop循环，不能进入resetToDefault()函数
|
||||
// resetToDefaultCounter作用：防止小车倒地后某个角度，陷入循环，motor.target一直等于0，就躺地上不动了
|
||||
if ((LQR_angle < -35.0f || LQR_angle > 70.0f )&& loopCounter > 800 && resetToDefaultCounter < 130) {
|
||||
resetToDefault();
|
||||
resetToDefaultCounter++;
|
||||
motor1.target = 0;
|
||||
motor2.target = 0;
|
||||
// Serial.println("resetToDefault");
|
||||
}
|
||||
|
||||
if( abs(LQR_angle) < 25.0f){
|
||||
resetToDefaultCounter = 0;
|
||||
}
|
||||
|
||||
// target目标电压 0~8V, 0为不转, 8为最大转速
|
||||
// Serial.print("target电压:");
|
||||
// Serial.print("LQR_angle:");
|
||||
// Serial.print(LQR_angle);
|
||||
// Serial.print(",motor1.target:");
|
||||
// Serial.print(motor1.target);
|
||||
// Serial.print(",motor2.target:");
|
||||
// Serial.println(motor2.target);
|
||||
@@ -359,9 +369,31 @@ void lqr_balance_loop()
|
||||
// + LQR_k4*LQR_speed;
|
||||
|
||||
// 给负值是因为按照当前的电机接线，正转矩驱动小车电机向后转
|
||||
// 编码器反馈shaft_angle角度，shaft_velocity速度
|
||||
// 不操作前后移动，譬如刚开机，车轮不动的，shaft_angle和shaft_velocity 都是零
|
||||
// 所以，不操作前后移动，LQR_distance 和 LQR_speed趋近于0，只有角度控制和角速度控制
|
||||
|
||||
/*
|
||||
LQR_u 越大，电机输出的转矩越大，电机向后转，默认0-8V
|
||||
LQR_u = angle_control + gyro_control + distance_control + speed_control
|
||||
|
||||
编码器反馈shaft_angle角度，shaft_velocity速度
|
||||
不操作前后移动，譬如刚开机，车轮不动的，shaft_angle和shaft_velocity 都是零
|
||||
所以，不操作前后移动，LQR_distance 和 LQR_speed趋近于0，只有角度控制和角速度控制
|
||||
|
||||
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速度正相关
|
||||
*/
|
||||
|
||||
// 在平衡车中，通常设定电机向后转（使车前进）为负，所以这里LQR_distance和LQR_speed的计算乘以-0.5
|
||||
LQR_distance = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle); // 两个电机的旋转角度（shaft_angle）（是位置量）的平均值
|
||||
LQR_speed = (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity); // 两个电机角速度（shaft_velocity）的平均值，并取反
|
||||
@@ -410,7 +442,7 @@ void lqr_balance_loop()
|
||||
robot_speed_last = robot_speed; // 记录连续两次的轮部转速
|
||||
robot_speed = LQR_speed;
|
||||
// 若轮部角速度、角加速度"过大"（ > 10或> 50）,或处于跳跃后的恢复时期（jump_flag != 0），认为出现轮部离地现象，需要特殊处理
|
||||
//轮部离地情况下 LQR_u计算：2个参数，角度控制量+角速度控制量，对轮位移(distance)和速度(speed)控制分量不输出；反之，正常状态下完整输出平衡转矩
|
||||
// 轮部离地情况下 LQR_u计算：2个参数，角度控制量+角速度控制量，对轮位移(distance)和速度(speed)控制分量不输出；反之，正常状态下完整输出平衡转矩
|
||||
if (abs(robot_speed - robot_speed_last) > 10 || abs(robot_speed) > 50 || (jump_flag != 0))
|
||||
{
|
||||
distance_zeropoint = LQR_distance; // 位移零点重置
|
||||
@@ -465,7 +497,7 @@ void leg_loop()
|
||||
{
|
||||
|
||||
// 倒地时强制腿部收缩
|
||||
if (LQR_angle < -36.0f || LQR_angle > 45.0f)
|
||||
if (LQR_angle < -30.0f || LQR_angle > 45.0f)
|
||||
{
|
||||
Position[0] = SERVO1_MIN;
|
||||
Position[1] = SERVO2_MIN;
|
||||
@@ -551,6 +583,7 @@ void jump_loop()
|
||||
// 跳跃，腿部以最快速度伸到最长
|
||||
if ((wrobot.dir_last == 5) && (wrobot.dir == 4) && (jump_flag == 0))
|
||||
{
|
||||
|
||||
// 跳跃开始
|
||||
ACC[0] = 0;
|
||||
ACC[1] = 0;
|
||||
@@ -567,6 +600,7 @@ void jump_loop()
|
||||
{
|
||||
jump_flag++;
|
||||
|
||||
|
||||
if ((jump_flag > 30) && (jump_flag < 35))
|
||||
{
|
||||
ACC[0] = 0;
|
||||
@@ -721,4 +755,41 @@ void blinkLed() {
|
||||
delay(200);
|
||||
digitalWrite(LED_BAT, LOW);
|
||||
delay(200);
|
||||
}
|
||||
|
||||
// 重置到默认参数状态，用于自平衡起立
|
||||
void resetToDefault() {
|
||||
// LQR自平衡控制器参数
|
||||
LQR_angle = 0;
|
||||
LQR_gyro = 0;
|
||||
angle_control = 0;
|
||||
gyro_control = 0;
|
||||
angle_zeropoint = 5;
|
||||
|
||||
LQR_speed = 0;
|
||||
LQR_distance = 0;
|
||||
speed_control = 0;
|
||||
distance_control = 0;
|
||||
distance_zeropoint = 1;
|
||||
LQR_u = 0;
|
||||
|
||||
// YAW轴控制数据
|
||||
YAW_gyro = 0;
|
||||
YAW_angle = 0;
|
||||
YAW_angle_last = 0;
|
||||
YAW_angle_total = 0;
|
||||
YAW_angle_zero_point = -10;
|
||||
YAW_output = 0;
|
||||
|
||||
// 逻辑处理标志位
|
||||
robot_speed = 0;
|
||||
robot_speed_last = 0;
|
||||
wrobot_move_stop_flag = 0;
|
||||
jump_flag = 0;
|
||||
leg_position_add = 0;
|
||||
|
||||
pid_lqr_u.error_prev = 0;
|
||||
|
||||
|
||||
|
||||
}
|
||||
Reference in new issue
Block a user