稳定版
This commit is contained in:
myperworld committed
1 parent
5a0bbef43b
commit
603877cec4
7 files changed
+2241
-123
No files matched your search
@@ -151,16 +151,6 @@ 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
|
||||
@@ -312,20 +302,37 @@ void loop()
|
||||
wrobot.joyx_last = wrobot.joyx;
|
||||
wrobot.joyy_last = wrobot.joyy;
|
||||
|
||||
// 将自平衡计算输出转矩赋给电机
|
||||
// motor.target 负数小车向前转，正数小车向后转，
|
||||
/* 将自平衡计算输出转矩赋给电机
|
||||
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速度正相关
|
||||
*/
|
||||
motor1.target = (-0.5) * (LQR_u + YAW_output); // 负数小车向前，正数小车向后，取
|
||||
motor2.target = (-0.5) * (LQR_u - YAW_output);
|
||||
|
||||
// 查看角度信息, 屁股向后倒 -33~-34度,舵机2048的时候-42度，头向前到底 75度，完全垂直88度
|
||||
// Serial.println(LQR_angle);
|
||||
Serial.println(angle_zeropoint);
|
||||
|
||||
// 检测小车是否向后马上要倒地了，在中间过程重置所有参数
|
||||
loopCounter++;
|
||||
// loopCounter作用：在刚开机的800次loop循环，不能进入resetToDefault()函数
|
||||
// resetToDefaultCounter作用：防止小车倒地后某个角度，陷入循环，motor.target一直等于0，就躺地上不动了
|
||||
if ((LQR_angle < -35.0f || LQR_angle > 70.0f )&& loopCounter > 800 && resetToDefaultCounter < 130) {
|
||||
// loopCounter作用：在期初的800次循环内视为电源开通后第一次自平衡，不能进入resetToDefault()函数重置参数
|
||||
// resetToDefaultCounterz作用：防止小车倒地后某个角度，陷入循环，motor.target一直=0
|
||||
if ((LQR_angle < -35.0f || LQR_angle > 70.0f ) && loopCounter > 800 && resetToDefaultCounter < 130) {
|
||||
resetToDefault();
|
||||
resetToDefaultCounter++;
|
||||
motor1.target = 0;
|
||||
@@ -369,31 +376,9 @@ void lqr_balance_loop()
|
||||
// + LQR_k4*LQR_speed;
|
||||
|
||||
// 给负值是因为按照当前的电机接线，正转矩驱动小车电机向后转
|
||||
|
||||
/*
|
||||
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速度正相关
|
||||
*/
|
||||
|
||||
// 编码器反馈shaft_angle角度，shaft_velocity速度
|
||||
// 不操作前后移动，譬如刚开机，车轮不动的，shaft_angle和shaft_velocity 都是零
|
||||
// 所以，不操作前后移动，LQR_distance 和 LQR_speed趋近于0，只有角度控制和角速度控制
|
||||
// 在平衡车中，通常设定电机向后转（使车前进）为负，所以这里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）的平均值，并取反
|
||||
@@ -583,6 +568,14 @@ void jump_loop()
|
||||
// 跳跃，腿部以最快速度伸到最长
|
||||
if ((wrobot.dir_last == 5) && (wrobot.dir == 4) && (jump_flag == 0))
|
||||
{
|
||||
// 设定最大正向电压（快速加速）
|
||||
// motor1.target = -8.0f;
|
||||
// motor2.target = -8.0f;
|
||||
// motor1.loopFOC();
|
||||
// motor2.loopFOC();
|
||||
// motor1.move();
|
||||
// motor2.move();
|
||||
// delay(50); // 缩短加速时间（50ms足够获得初速度）
|
||||
|
||||
// 跳跃开始
|
||||
ACC[0] = 0;
|
||||
@@ -600,6 +593,10 @@ void jump_loop()
|
||||
{
|
||||
jump_flag++;
|
||||
|
||||
// if (jump_flag == 25) { // 跳跃上升阶段停止轮子转动
|
||||
// motor1.target = 0;
|
||||
// motor2.target = 0;
|
||||
// }
|
||||
|
||||
if ((jump_flag > 30) && (jump_flag < 35))
|
||||
{
|
||||
|
||||
Reference in new issue
Block a user