Naive Control Test

Now familiar with BricxCC and the HTWay, it's time check the new platform's operation. I'm trying to get as close to a balancing code as possible with an accelerometer, but not worrying about it too much until I acquire the latest Matlab. In the meantime, this should bring to light a lot of other considerations that will need to be evaluated and handled in the final control. To get this naive control to work adequately, I'll try using Ziegler-Nichols tuning on a basic control that maps angle to motor speeds. First, a word on the tuning method.

Ziegler-Nichols Tuning Method

J.G. Ziegler and N.B. Nichols in 1942 published "Optimum Settings for Automatic Controllers" as part of "Transactions of the ASME". In it, Ziegler and Nichols developed time domain and frequency domain methods of determining appropriate tuning values. These values are based off of step response and frequency response characterizations of the system's dynamics. Consisting of relatively simple rules, its easy to apply the method to many different problems. For a complete discussion of Ziegler-Nichols tuning, I suggest looking into the wikipedia article, and the original ASME article republished with permission at http://www.driedger.ca/.

For my process, I decided to follow the frequency response method. I attempted to identify the critical oscillation gain, the gain at which the robot oscillated around the upright balanced position, and the period at which it oscillated. To determine this value, I ran a simplified version of the code below with only the proportional factor controlling the output. I varied the values of the proportional gain until the platform began to oscillate nearly around the point of balancing. I found this value to be 1.25. The period at which it oscillated was estimated to be 200 milliseconds.

Naive Control

After some trial and error I was able to put together the code at the end of this post, a simple implementation of a PID controller in NXC. Let's go through stanza by stanza and I'll explain what's going on, the intent behind the code, and considerations for future implementations.

Lines 1 through 5 defines the bound function. This function returns the value of x bounded by an upper and lower value. Since the motors should only be set with values between -100 and 100, we'll use this to limit its actuation.

Line 7 begins the main task. This is the first task that is run on the NXT. Tasks are similar to threads or parallel processes. We could have multiple tasks for sensing and actuation for potentially better performance. For more complex code, I'll revisit this idea.

Lines 8 through 23 declare the variables needed:

• Integers
• angle - The angle is represented by acceleration expressed in the forward and backward horizontal plane of the robot in gs divided by 200.
• deltaTime - The approximate time between each iteration of the main loop in milliseconds. This is set to be 50. A known delta value is used to control the loop speed so further analysis can take place down the road.
• error - The difference between the idealAngle and the sensed angle in gs divided by 200.
• garbage - Not used for anything constructive. This is necessary when acquiring data from the accelerometer.
• idealAngle - The sensed balanced acceleration value in gs divided by 200. Due to the weight distribution of the hardware, the robot needs to lean a little forward to be balanced. Thus, this is set to 9.
• lastError - The last error value from the last loop iteration in gs divided 200.
• overheadTime - The time in milliseconds the sensor acquisition, calculations, and motor settings need to complete. Using the commented lines, I was able to determine this to be about 16 milliseconds.
• Floats
• derivative and integral - The derivative and integral values from the PID calculations.
• Kc and Pc - The critical oscillation gain and period. Specifically, Kc is the minimum proportional gain necessary to oscillate the system as described in the last second. These are set to 1.25 and 200 respectively.
• Kp, Ki, and Kd - The proportional, integral, and derivative gain values. Note that the derivative gain is set to zero. Derivative terms can add a lot of noise to a system, so this is left out for now.
• output - The output from the PID control.

Lines 25 through 27 initialize the proportional and integral gains using the Ziegler-Nichols tuning method. The critical oscillation gain is multiplied by 0.45 to determine the proportional gain. The proportional gain is multiplied by 1.2 and divided by the period to get the integral gain.

Lines 29 and 30 initialize the accelerometer for reading. This could take some time, so a brief wait is called after the initialization.

Lines 33 through 36 find an initial last error value by reading the accelerometer and finding its difference from the ideal value.

Lines 38 and on are the main loop of the program. All lines that have NumOut(...) are debugging output that gets printed to the NXT's LCD screen.

Line 40 forces the loop to wait an adjusted value to match an actual delay nearly the set delta time.

Lines 49 through 60 find the sensor value and calculate the PID controller's output.

Line 63 sets the motor speed. Note that the output is multiplied by -1.0 because the motors are mounted backwards.

Running the Code

Running the code on the platform has made it apparent the accelerometer alone is not sufficient for balancing. The accelerometer is sensing the acceleration the robot is under. Our axis of interest is the forward-back horizontal direction. In the presence of gravity and no other forces, the acceleration could be directly mapped to an angle accurately. But because the robot is actually under many forces, most importantly the motor driven wheels keeping it balanced, the acceleration in our axis of interest will be the result of all these forces. Another sensor is necessary.

A piezoelectric gyroscope is a sound alternative. Mounted in the appropriate axis, it can be used to detect the speed at which the angle of the robot is changing. This could be used to accurately determine the robot's angle. With more effort, this could be used in conjunction with the accelerometer for even more accurate sensing. I've placed an order for the HiTechnic gyro sensor I discussed in my last post. It should be arriving next week. I'll adjust the controller and try again with the gyroscope.

Next week will be a short week for work on the project. I'll be mentoring the FIRST Robotics Competition Hartford Regional, but I'll try to get as much done as possible.

Source Code

1   int bound(int lower, int x, int upper) {
2     if (x < lower) {return lower;}
3     if (x > upper) {return upper;}
4     return x;
5   }
6

7   task main() {
8     unsigned long tick;
9     int angle,
10        deltaTime = 50, // must be >16
11        error,
12        garbage,
13        idealAngle = 9,
14        lastError,
15        overheadTime = 16;
16    float derivative, 

17          integral = 0,
18          Kc = 1.25,
19          Kp = 0,
20          Ki = 0,
21          Kd = 0,
22          output,
23          Pc = 200.0;
24     
25    Kp = 0.45 * Kc;
26    NumOut(10, LCD_LINE1, Kp);
27    Ki = 1.2 * Kp * deltaTime / Pc;
28     
29    SetSensorLowspeed(S4);
30    Wait(50);
31
32    // read sensor (15 ticks)
33    ReadSensorHTAccel(S4, angle, garbage, garbage);
34  

35    // set an initial last error
36    lastError = idealAngle + angle;
37
38    while (true) {
39      // wait
40      Wait(deltaTime - overheadTime);
41     
42      NumOut(10, LCD_LINE1, Kp, DRAW_OPT_CLEAR_WHOLE_SCREEN);
43      NumOut(10, LCD_LINE2, Ki);
44      NumOut(10, LCD_LINE3, Kd);
45     
46      //tick = CurrentTick();
47 
48      // read sensor (15 ticks)
49      ReadSensorHTAccel(S4, angle, garbage, garbage);

50 
51      // pid calculations (0 ticks)
52      error = idealAngle - angle;
53      NumOut(10, LCD_LINE4, error); 

54      integral = integral + (error * deltaTime);
55      NumOut(10, LCD_LINE5, integral);
56      derivative = (error - lastError) / deltaTime;
57      NumOut(10, LCD_LINE6, derivative); 

58      output = (Kp * error) + (Ki * integral) + (Kd * derivative);
59      NumOut(10, LCD_LINE7, output);
60      lastError = error;
61   
62      // set motor speed (0-1 tick)
63      OnFwdSync(OUT_AC, bound(-100, output * -1.0, 100), 0);
64   
65      //NumOut(10, LCD_LINE1, CurrentTick()-tick);
66    }
67  }