Mini Cheetah 代码分析（七）控制器自动切换运动模式

正常情况下，需要在运行mit_ctrl控制器后，有两种指令下发方式：一种是通过游戏手柄遥控器发送射频信号，机器人上的接收器接收到后再处理；另一种是通过在Qt仿真界面的窗口通过lcm通讯发送指令。如果需要不通过发指令实现自动运动，则需要对控制器状态机部分进行修改。

主要步骤：

（1）手动设置指令下发模式，跳过ESTOP，进入控制器。

（2）去掉指令接收接口，重写控制机逻辑部分。

（3）控制机身姿态。

1. 在/robot/src/RobotRunner.cpp文件中，找到void RobotRunner::run()函数的如下代码段：

    if( (rc_control.mode == 0) && controlParameters->use_rc ) {if(count_ini%1000 ==0)   printf("ESTOP!\n");for (int leg = 0; leg < 4; leg++) {_legController->commands[leg].zero();}_robot_ctrl->Estop();}

代码解释：如果遥控器控制的模式为0，且通过界面读取到的配置文档的use_rc参数为1（即使用遥控器），则停留在Estop()模式中。因此，只要跳过此判断，就能够继续向下执行控制器，跳过ESTOP模式。

在if之前添加变量赋值即可。

controlParameters->use_rc=0;//mark???if( (rc_control.mode == 0) && controlParameters->use_rc ) {if(count_ini%1000 ==0)   printf("ESTOP!\n");for (int leg = 0; leg < 4; leg++) {_legController->commands[leg].zero();}_robot_ctrl->Estop();}

将controlParameters->use_rc=0;将使得仿真界面的通信失效。或者将if里面内容注释掉，只要能跳出_robot_ctrl->Estop();这句的状态，进入到后面的这个地方即可：

// Run Control _robot_ctrl->runController();

2. 在/user/MIT_Controller/FSM_States/ControlFSM.cpp文件中，找到void ControlFSM<T>::runFSM() 函数，将
if(data.controlParameters->use_rc){... ...}此判断删除或注释掉。
并添加如下内容，参数参考control_mode。

 operatingMode = safetyPreCheck();
if (iter<1000)
{data.controlParameters->control_mode = K_PASSIVE;printf("Mark K_PASSIVE=0");
}
else if (iter<2000){data.controlParameters->control_mode = K_RECOVERY_STAND;
}
else if (iter<3000){data.controlParameters->control_mode = K_BALANCE_STAND;printf("Mark control_mode = K_BALANCE_STAND");
}
else if (iter<4000){data.controlParameters->control_mode = K_LOCOMOTION;printf("Mark control_mode = K_LOCOMOTION");
}
else if (iter<5000) {data.controlParameters->control_mode = K_BALANCE_STAND;printf("Mark control_mode = K_BALANCE_STAND");
}
else {data.controlParameters->control_mode = K_BACKFLIP;printf("Mark control_mode = K_BACKFLIP");
}

解释：直接通过iter计时，从掉电状态K_PASSIVE切换到位控的站立状态K_RECOVERY_STAND；切换到力控的站立状态K_BALANCE_STAND；再切换到运动状态K_LOCOMOTION，对应于convexMPC/convexMPCLocomotion.cpp中的控制器，可以编辑控制器实现预设的运动，包括机身六自由度的运动和不同步态模式的设定；最后切换到力控站立后，做了一个后空翻的动作。

运行时，打开仿真环境./sim/sim，再运行控制器mit_ctrl m s，不需要在界面输入指令，等待即可发现机器人在按照预设切换不同的运动模式。

（3）使用上述内容，运动时会切换到ConvexMPCLocomotion.cpp这个控制器中，该控制器自带计数器iterationCounter，周期是0.002 s，可以用来对机身姿态发送指令控制。比如可以将源代码注释掉一部分，使用如下代码，实现自动运动。

else{// _yaw_turn_rate = data._desiredStateCommand->rightAnalogStick[0]*1.5;// x_vel_cmd = data._desiredStateCommand->leftAnalogStick[1]*1.5;//+0.07;// y_vel_cmd = data._desiredStateCommand->leftAnalogStick[0];// iterationCounterif (iterationCounter%500==0){printf("zhhw count = %d", iterationCounter);}int64_t t = iterationCounter*0.002;   if(t<3.14){gaitNumber = 6;_yaw_turn_rate = sin(4*t);}else {gaitNumber = 3;_yaw_turn_rate = 1;x_vel_cmd = 0.3;//+0.07;y_vel_cmd = 0.0;}}

(4)根据以上步骤，实现机械狗的运动流程为：开机启动->站立->周期性前后走->停止的代码如下：

在ControlFSM.cpp中ControlFSM<T>::runFSM()函数添加部分内容如下：

    if (iter<1000){data.controlParameters->control_mode = K_PASSIVE;// printf("Mark K_PASSIVE=0\n");}else if (iter<2000){data.controlParameters->control_mode = K_RECOVERY_STAND;}else if (iter<3000){data.controlParameters->control_mode = K_BALANCE_STAND;// printf("Mark control_mode = K_BALANCE_STAND\n");}else if (iter<18000){data.controlParameters->control_mode = K_LOCOMOTION;// printf("Mark control_mode = K_LOCOMOTION\n");}else if (iter<19000) {data.controlParameters->control_mode = K_BALANCE_STAND;// printf("Mark control_mode = K_BALANCE_STAND\n");}else if (iter<22000){data.controlParameters->control_mode = K_RECOVERY_STAND;// printf("Mark control_mode = K_RECOVERY_STAND\n");}else{printf("done!\n");}

在ConvexMPCLocomotion.cpp中ConvexMPCLocomotion::_SetupCommand(ControlFSMData<float> & data)函数添加部分如下：

double t = sin(0.5*iterationCounter*0.002);if(t>=0)
{gaitNumber = 6;_yaw_turn_rate = 0.0;x_vel_cmd = 0.2;//+0.07;y_vel_cmd = 0;printf("t>=0,step forward\n");
}
else if(t<0)
{gaitNumber = 1;_yaw_turn_rate = 0;x_vel_cmd = -0.2;//+0.07;y_vel_cmd = 0.0;printf("t<0,back\n");
}printf("out!, t=%f\n",t);

修改完成后便可实现机械狗的来回踱步运动

Mini Cheetah 代码分析（七）控制器自动切换运动模式相关推荐

QSPI Flash驱动代码分析 (QSPI控制器初始化)
QSPI Flash驱动代码分析 (QSPI控制器初始化) 1. 函数cqspi_controller_enable() 该函数主要使能和去能QSPI控制器.QSPI配置寄存器(偏移量0x00)的bi ...
UG/NX 二次开发(C#) 程序自动切换制图模式
作为C#语言的铁粉,在做二次开发的时候,发现西门子在C#语言下的支持还是不够完善. 就拿自动切换制图模式来说,C++可以直接调用内部函数 UI_APP_change_application,还在网上找 ...
android定时切换活动,安卓手机如何定时自动切换情景模式 (全文)
相信很多人都知道手机情景模式这个功能.它可以根据不同场景环境而将手机调为静音或者震动模式.这个功能确确实实方便了许多人,因而也经常被使用.但是很多人希望手机能定时自动切换情景模式.安卓系统本身目前并不 ...
笔记本电脑拔掉电源自动切换性能模式
[任务计划程序]实现自动切换性能模式我的笔记本电脑是小米air13.3指纹版,插上电源的时候性能还可以,勉强能满足我日常开发需求,但是一旦拔掉电源,电脑就会因为供电不足而导致CPU频率降到1GHz以 ...
WIN10 自动切换夜间模式
WIN10 自动切换夜间模式介绍系统版本步骤 1. 更改power shell执行策略,以便能执行切换脚本 2.创建切换脚本 3. 添加脚本到任务计划程序中介绍 Win10 现在还没有自动切换 ...
2021SC@SDUSC BRPC代码分析(七) —— bthread综述、Butex及mutex详解
2021SC@SDUSC 文章目录一.bthread的背景知识学习二.代码分析总结一.bthread的背景知识学习经过前面6篇代码分析,我将BRPC一个极其实用的工具--bvar做了全面系统 ...
Win10《芒果TV》更新v3.8.70周年版：升级高级配色、自动切换夜间模式
官宣!光阴似箭,已过三年时光,我们一起从最初的青涩变成更加成熟起来,同呼吸共命运,匠心依旧,感恩有你,Win10版<芒果TV>全平台同步更新周年版v3.8.70,升级高级配色,新增自动切换 ...
kdb代码分析(七)
在大家都进入了kdb之后,in control的那个cpu就开始执行1566行的kdb_local()函数了.依然定义于kdb/kdbmain.c: 1134 /* 1135 * kdb_local ...
人工智能研究中心快递柜——代码分析七
2021SC@SDUSC 目录 index.wxml index.js 本次分析项目中柜子的定位功能的实现. index.wxml 在微信官方开发文档中,提供了map组件,通过对wx.createMa ...

Mini Cheetah 代码分析（七）控制器自动切换运动模式

Mini Cheetah 代码分析（七）控制器自动切换运动模式相关推荐

最新文章

热门文章