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

一、遥想星空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控制周期

二、吴方法

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

由于车轮电机给了机体一个扭矩，那么一定要抵消这个扭矩，怎么抵消扭矩？关节电机是不可能的，所以还是**车轮电机重新提供一个反方向的扭矩！**而自己在调试过程中，以为lsk的平衡控制算法会帮我抵消扭矩，其实不会，因为他是根据陀螺仪实时测量俯仰角pitch来进行平衡控制的，并没有检测扭矩的传感器。

在这里插入图片描述

三、自己的控制规划

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

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

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

如果不设为切换状态时的位置会产生急停，力矩还在，车体直接翻倒：
在这里插入图片描述

四、控制代码debug

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

正确处理方式：

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

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

站立过程状态含义：

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