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
Function name | Purpose | Note |
mGaitManager->start () | Start | |
mGaitManager->stop () | Stop | |
mGaitManager->step (int t) | Stop after running for some time | T in milliseconds |
mGaitManager->setXAmplitude (double X) | Forward / backward | X affects the forward length of a step, and can take any value between - 1 and 1 |
mGaitManager->setYAmplitude (double Y) | Move left / right | Y affects the length of the foot in the side direction, it can take any value between - 1 and 1 |
mGaitManager->setAAmplitude (double A) | Turn left / right | A affects the angle of gait and allows the robot to rotate during walking. It can take any value between 0 and 1 |
wait (int t) | Wait | T in milliseconds |
mMotionManager->playPage(9) | Ready | playPage is a series of enca-psulated robot action files |
mMotionManager->playPage(10) | Stand up | The initial state is that the robot faces down and stands up |
mMotionManager->playPage(11) | Stand up | The initial state is that the back of the robot is facing down, and the robot is allowed to stand up |