【兔子机器人】实现从初始状态到站立

一、遥想星空up主的方法

由于我有卡位结构，无法做到劈腿，而且底盘也不一样，无法使用此方法
但是其代码思想是可以借鉴的。

参考视频：
【【开源啦！】无刷轮腿平衡机器人】【精准空降到 01:16】 https://www.bilibili.com/video/BV1bP411k75b/?share_source=copy_web&vd_source=77e36f24add8dc77c362748ffb980148&t=76

在这里插入图片描述

Motor_SetTorque(&leftJoint[0], 0.2f);  //将0.2Nm扭矩添加到左边0号关节电机

在这里插入图片描述

这部分代码更新腿部姿态

vTaskDelay(5); // 延时，直到腿部角度转到一定位置

在这里插入图片描述
设置standupState初始值为StandupState_None

StandupState_None的引用：

StandupState_Standup的引用：

lsk兔子的站立代码：

电机力矩求解函数：
在这里插入图片描述

每次循环都解算关节电机

#define PROTECT_CONDITION (leftTheta < -M_PI_4 || leftTheta > M_PI_4 || \rightTheta < -M_PI_4 || rightTheta > M_PI_4 || \imuData.pitch > M_PI_4 || imuData.pitch < -M_PI_4) //腿部角度超限保护条件
if(PROTECT_CONDITION) //当前达到保护条件
{if(standupState == StandupState_None) //未处于起立过程中{//关闭所有电机Motor_SetTorque(&leftWheel, 0);Motor_SetTorque(&rightWheel, 0);Motor_SetTorque(&leftJoint[0], 0);Motor_SetTorque(&leftJoint[1], 0);Motor_SetTorque(&rightJoint[0], 0);Motor_SetTorque(&rightJoint[1], 0);//阻塞等待腿部角度回到安全范围，再等待4s后恢复控制(若中途触发了起立则在起立准备完成后直接跳出)while(PROTECT_CONDITION && standupState == StandupState_None){leftTheta = leftLegPos.angle - imuData.pitch - M_PI_2;rightTheta = rightLegPos.angle - imuData.pitch - M_PI_2;vTaskDelay(100);}if(standupState == StandupState_None)vTaskDelay(4000);//退出保护后设定目标位置和yaw角度为当前值target.position = (leftWheel.angle + rightWheel.angle) / 2 * wheelRadius;target.yawAngle = imuData.yaw;continue;}if(standupState == StandupState_Standup && (leftTheta < -M_PI_4 || rightTheta > M_PI_4))standupState = StandupState_None;
}
else
{if(standupState == StandupState_Standup) //未达到保护条件且处于起立过程中，说明起立完成，退出起立过程standupState = StandupState_None;
}//设定关节电机输出扭矩
Motor_SetTorque(&leftJoint[0], -leftJointTorque[0]);
Motor_SetTorque(&leftJoint[1], -leftJointTorque[1]);
Motor_SetTorque(&rightJoint[0], -rightJointTorque[0]);
Motor_SetTorque(&rightJoint[1], -rightJointTorque[1]);vTaskDelayUntil(&xLastWakeTime, 4); //4ms控制周期

二、吴方法

核心思想：
在这里插入图片描述
实操：

三、自己的控制规划

四腿贴紧卡位，然后解算出陀螺仪的pitch俯仰角，来判断是前倒还是后倒。然后车轮瞬间给一个大力距，将兔子立起来。

后仰角度：
在这里插入图片描述
前倾角度：

要将站立写在一个线程里面，或者main.c里面，因为线程在来回切换，会有可能影响条件语句判断导致出错，而且代码看起来很乱。目前的错是，轮毂电机的力不受控制，一直都大。

四、控制代码debug

直接将以下代码放在control.h中，会出现下面的报错，因为.h只能声明，而不能初始化赋值。
在这里插入图片描述

正确处理方式：

在.h里面声明，在.c里面初始化赋值：
在这里插入图片描述

外部文件想使用其变量（变成外部变量），在自己的.h文件利用extern声明变量，但不要赋值，这样子就可以在.c文件直接引用了。

站立过程状态含义：

//站立过程状态枚举量
enum StandupState
{StandupState_None_Front,StandupState_None_Back,//当pitch角度在一定倾斜角度时，赋值StandupState_None，//如果是前倾--Front,如果是后倾--BackStandupState_Prepare,//虚拟腿收缩最短，则赋值StandupState_PrepareStandupState_Standup//准备工作完成，则赋值StandupState_Standup，开始站立
};