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];

    // iterationCounter
    if (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);

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