1. Analysis and description of controller code

Open RunningRobot Env_ V1.0.wbt file, select the robot, right-click, select "Edit controller", open in the right window Walk.cpp.

In Walk.cpp, firstly, some key constants are defined to represent some joints of robot motion:

In the robot function, set the initial state of the robot, such as the color of the LED on the head and eyes, and initialize the keyboard input and the motion manager of the motion manager

1.  Walk::Walk() : Robot() {  

2.    mTimeStep = getBasicTimeStep();  

3.    

4.    getLED("HeadLed")->set(0xFF0000);  

5.    getLED("EyeLed")->set(0x00FF00);  

6.    mAccelerometer = getAccelerometer("Accelerometer");  

7.    mAccelerometer->enable(mTimeStep);  

8.    

9.    getGyro("Gyro")->enable(mTimeStep);  

10.   

11.   for (int i = 0; i < NMOTORS; i++) {  

12.     mMotors[i] = getMotor(motorNames[i]);  

13.     string sensorName = motorNames[i];  

14.     sensorName.push_back('S');  

15.     mPositionSensors[i] = getPositionSensor(sensorName);  

16.     mPositionSensors[i]->enable(mTimeStep);  

17.   }  

18.   

19.   mKeyboard = getKeyboard();  

20.   mKeyboard->enable(mTimeStep);  

21.   

22.   mMotionManager = new RobotisOp2MotionManager(this);  

23.   mGaitManager = new RobotisOp2GaitManager(this, "config.ini");  

24. }  

myStep function enables the robot to move in one step:

1.  void Walk::myStep() {  

2.    int ret = step(mTimeStep);  

3.    if (ret == -1)  

4.      exit(EXIT_SUCCESS);  

5.  }  

wait function causes the robot to wait for a period of time:

1.  void Walk::wait(int ms) {  

2.    double startTime = getTime();  

3.    double s = (double)ms / 1000.0;  

4.    while (s + startTime >= getTime())  

5.      myStep();  

6.  }  

run function is used to control the continuous motion of the robot:

1.  // function containing the main feedback loop  

2.  void Walk::run() {  

3.    cout << "The robot will automatically take a few steps" << endl;  

4.    

5.    // First step to update sensors values  

6.    myStep();  

7.    

8.    // play the hello motion  

9.    mMotionManager->playPage(9);  // init position  

10.   wait(200);  

11.   

12.   // main loop  

13.   while (true) {  

14.     checkIfFallen();  

15.   

16.     mGaitManager->setXAmplitude(0.0);  

17.     mGaitManager->setAAmplitude(0.0);  

18.   

19.     mGaitManager->start();  

20.     mGaitManager->setXAmplitude(1.0);  

21.   

22.     mGaitManager->step(mTimeStep);  

23.   

24.     // step  

25.     myStep();  

26.   }  

27. }  

When the emulator starts to run, run the Walk.cpp Files in the same folder main.cpp File, which calls the run function. In the run function, motion_ Manager is used to make the robot stand, and then the controller goes into an infinite while loop. The first thing to do in the loop is to check whether the robot has not fallen, that is, to call the checkiffallon function, which is implemented by using an accelerometer. The above is the modified run function, which makes the robot move directly without keyboard control. Later, we will introduce how to modify it.

checkIfFallen function to detect if the robot falls:

1.  void Walk::checkIfFallen() {  

2.    static int fup = 0;  

3.    static int fdown = 0;  

4.    static const double acc_tolerance = 80.0;  

5.    static const double acc_step = 100;  

6.    

7.    // count how many steps the accelerometer  

8.    // says that the robot is down  

9.    const double *acc = mAccelerometer->getValues();  

10.   if (acc[1] < 512.0 - acc_tolerance)  

11.     fup++;  

12.   else  

13.     fup = 0;  

14.   

15.   if (acc[1] > 512.0 + acc_tolerance)  

16.     fdown++;  

17.   else  

18.     fdown = 0;  

19.   

20.   // the robot face is down  

21.   if (fup > acc_step) {  

22.     mMotionManager->playPage(10);  // f_up  

23.     mMotionManager->playPage(9);   // init position  

24.     fup = 0;  

25.   }  

26.   // the back face is down  

27.   else if (fdown > acc_step) {  

28.     mMotionManager->playPage(11);  // b_up  

29.     mMotionManager->playPage(9);   // init position  

30.     fdown = 0;  

31.   }  

32. }  

There are two kinds of situations: face down and back down. According to the situation, the robot stands up again and initializes its motion state.

2 Modify the code to control the robot's motion

We try to sample the code Walk.cpp Modify it so that the robot can move automatically without keyboard control. Delete the bool iswalking = false; statement in the function void walk:: run(), and modify the contents of the main loop while as follows:

1.  while (true) {  

2.      checkIfFallen();  

3.    

4.      mGaitManager->setXAmplitude(0.0);  

5.      mGaitManager->setAAmplitude(0.0);  

6.    

7.      mGaitManager->start();  

8.      mGaitManager->setXAmplitude(1.0);  

9.    

10.     mGaitManager->step(mTimeStep);  

11.   

12.     // step  

13.     myStep();  

14.   }  

      Then press and hold Ctrl + s to save, and click on the top right corner to compile, as shown below.

Then rerun the simulation, you can see that the robot starts to move forward automatically without keyboard control, as shown below.

In the code, mGaitManager - > start(); let the robot start moving, mGaitManager - > setXAmplitude (1.0); let the robot move forward. Since the while loop runs all the time, the robot moves all the way forward. If you want the robot to stop in place, you can modify the conditions of the while loop.

3 List of common functions