16-299: Lab Assignment Part 1.

The assignment focuses on modeling and control of the isolated motor-wheel system. Later we will focus on balancing your robot.

We are going to wipe your robot's brain, so you might want to make a video of it balancing where you push it a couple of times, so you can compare the current performance with what it can do with the controller you create.

Put your robot upside down on your desk.

Make sure your battery is fully charged.
Connect the USB cable between the Arduino Nano and your computer.
Using the Arduino IDE (Integrated Development Environment), download the example "blink" program into the Arduino to verify you are able to download programs. (This will remove the program the Arduino came with). Is the LED blinking?

Now modify the blink program to make the LED blink faster (Nothing to turn in, just a useful exercise).

More information on how to use the Arduino can be found on our Elegoo web page as well as by googling "setting started with arduinos". This year we are using unmodified Arduino Nanos, rather than Arduino Nano Everys.

Installing the example servo program

I wrote an example controller, servo, to help you get started. It includes a way to estimate which way the wheel is turning so the encoder is read correctly. Download the program from here. The top folder (also called a directory on Linux) is called Arduino-16299. When you set up and ran the Arduino IDE, it set up a folder called Arduino, which is the default location where the Arduino IDE looks for programs and libraries. In what follows, I mention your "Arduino folder". On Windows this is in your Documents folder. On Linux it is ~/Arduino. Not sure where it is on the Mac. You can set it to be somewhere else, but those are the defaults.

You should copy the contents of Arduino-16299/libraries/ to your Arduino/libraries folder (create the libraries folder in Arduino/ if it isn't already there).

You should end up with the following in your Arduino/libraries folder:

Adafruit_NeoPixel  I2Cdev  MPU6050  PinChangeInterrupt  readme.txt

Copy Arduino-16299/16299 to your Arduino folder. When you run your Arduino IDE, you should see 16299 as an option when you click on "Open":

Clicking or double clicking on "16299" should get you to:

Clicking or double clicking on "servo" should get you to:

And clicking or double clicking on "servo.ino" (.ino is the file type of Arduino programs) should get you a new Arduino IDE window:

I then kill the previous Arduino IDE window to avoid getting confused.

Take a look at the servo.ino program. You can ignore the .h file tabs in the Arduino IDE and the .h file includes (#include "xxx.h") in the servo.ino program.

An experiment is made up of trials where the run_trial() subroutine enables the motors, does something for a while, and then disables the motors. The subroutine loop() calls run_trial() several times to run trials under diffferent conditions.

The part of run_trial() with the comment "Servo part" is where motor commands are generated using feedback.

Let's run the servo program.

Turn the battery on. Although some USB ports are powerful enough to survive the robot motor turning on, some aren't. Turning on the battery makes the battery the source of power for the motors, rather than your USB port. Your USB cable should remain connected, powering the Arduino and providing communication between your computer and the Arduino.

To download the servo program to the Arduino, and run it, click on the right arrow in the Arduino interface. You should have set your board ("Tools,Board,Arduino AVR Board,Arduino Nano") and port ("Port,/dev/ttyACM0" for me on Linux, COM something for Windows) correctly to download and run the blink program. The servo program prints stuff out, so after seeing the message "Done uploading", start the serial monitor (click on "Tools,Serial Monitor"). Set the line ending to be "New Line" and the baud rate to be 115200. Then hit right arrow again, and click on the Serial Monitor tab. This will reset the Arduino Nano, and it should print a message out:

Type 'g' into the box marked "Message" and hit return. This tells the servo program to run an experiment. Wheels should eventually turn sinusoidally. If you get bored watching the wheels turn, try typing 's' newline (or anything other than 'g') to the message window. This should stop the robot eventually. The Arduino only checks for commands in between trials, so be patient. However, if that doesn't work, unplug the USB cable where it is plugged in to your computer and then turn the battery off, which kills power to the robot, and it should stop. If that doesn't stop the robot, the robot has become super-intelligent and probably wants to kill you. The robot revolution has begun. Run!

Note that when you plug the USB cable back in, the port used may change, and you may have to update the port information. Other things that can go wrong:

If the Arduino interface (IDE) gets into a state where it can't download into
the Arduino (for whatever reason), or Putty doesn't print stuff out at all or
like a normal startup.
1) Turn the battery off.
2) Exit the Arduino interface (IDE) or Putty.
3) Unplug the Arduino USB cable from your computer (unplug the big connector
   (male A), not the micro USB connector on the Arduino. There is a chance
   the micro USB connector fails or breaks.).
4) Count to 10 (1 Mississippi, 2 Mississippi, ..., 10 Mississippi will take
about 10 seconds).
5) Plug the Arduino USB cable into your computer.
6) Restart the Arduino interface (IDE) or Putty.
7) Turn the battery on.

Common Errors:
1) avrdude: ser_open(): can't open device "/dev/ttyUSB0": No such file or
directory
-> USB cable to arduino not plugged in. -> plug it in
-or-
-> system switched to different port, switch ports in Arduino IDE

2) This error message
avrdude: stk500_recv(): programmer is not responding
avrdude: stk500_getsync() attempt 1 of 10: not in sync: resp=0x00
...
avrdude: stk500_recv(): programmer is not responding
avrdude: stk500_getsync() attempt 10 of 10: not in sync: resp=0x00
-> Do the first procedure above (kill, unplug, count, plug, restart).

Collect some data

$ putty 

$ ls
Makefile  parse-data  parse-data.c  putty.log

$ mv putty.log servo40.log

$ ./parse-data servo40
Writing servo40/f000
Voltage 8.33 1012
Gains 2000 0 0
Goal 3.14
Frequency 1
2002 points read.
Writing servo40/f001
Voltage 8.32 1010
Gains 2000 0 0
Goal 0
Frequency -1
402 points read.
Writing servo40/f002

$ ls -l servo40
total 104K
-rw-rw-r-- 1 cga cga 82K Mar 20 21:08 f000
-rw-rw-r-- 1 cga cga 14K Mar 20 21:08 f001
-rw-rw-r-- 1 cga cga  91 Mar 20 21:08 files.txt

$ cd servo40
$ cat files.txt
../servo40/f000 8.33 1012 2000 0 0 3.14 1 2002
../servo40/f001 8.32 1010 2000 0 0 0 -1 402

          Serial.print( current_micros );
          Serial.print( " " );
          Serial.print( raw_left_encoder_count );
          Serial.print( " " );
          Serial.print( left_command );
          Serial.print( " " );
          Serial.print( (int) (1000.0*left_wheel_angle_radians) );
          Serial.print( " " );
          Serial.print( (int) (1000.0*left_wheel_angular_velocity_radians) );
          Serial.print( " " );
          Serial.print( (int) (1000.0*angle_desired) );
          Serial.print( " " );
          Serial.print( (int) (1000.0*angle_error_integral_left) );
          Serial.print( " " );
          Serial.println( my_debug );

>> load f000
>> plot(f000(:,3)) % plotting command
>> plot(f000(:,4)) % plotting 1000*angle
>> plot(f000(:,5)) % plotting 1000*angular velocity
>> plot(f000(:,6)) % plotting 1000*angle desired
>> plot(f000(:,7)) % plotting 1000*integrated error

Try to make the control system go unstable.

run_trial( 2000.0, // P (position) gain
           0.0, // D (velocity) gain
           0.0, // I (integral) gain
           0.0, // Start angle
           100, // Delay after data collection starts to set goal
           PI, // Goal angle
          -1.0, // Frequency (negative means no sinusoid)
           2000, // Run time limit in milliseconds
           1 ); // Should data be collected?

delay( 1000 ); // take a 1s break

run_trial( 2000.0, // P (position) gain
           0.0, // D (velocity) gain
           0.0, // I (integral) gain
           PI, // Start angle
           100, // Delay after data collection starts to set goal
           0.0, // Goal angle
          -1.0, // Frequency (negative means no sinusoid)
           2000, // Run time limit in milliseconds
           1 ); // Should data be collected?

delay( 1000 ); // take a 1s break

Now gradually increase the P gain from 2000 (first argument) and watch (and listen) to what happens. (Gradually increasing is almost doubling each time: 2000, 5000, 10000, 20000, ...) Change the value in the code in the IDE servo.ino tab. Don't forget to click on the right arrow button to recompile and download the new version of the program. At the end of each movement the wheel vibrates more and more. At some point the wheel will vibrate continuously, probably around a gain of 100000. At the end of 2 seconds it stops because the servo is turned off. Turn in a plot of the command for the unstable trial, with the gains you used indicated.

Set the position gain back to 2000, and gradually increase the D (Damping or velocity) gain (10, 20, 50, 100, ...). When does the movement become critically damped? When does the contol system go unstable due to the velocity gain being too high? Turn in a plot of the command for the unstable trial, with the gains you used indicated.

Here is what "instability" looks like, a continuous oscillation:

The oscillation frequency is about 15Hz. For a system with no saturation, this would tell us tells us that the frequencies around 15Hz are most important to model accurately for feedback controller design. Lower frequencies won't have much phase lag, and large gains can be applied at low frequencies even if the model is uncertain or wrong at those frequencies. Higher frequencies have too much phase lag to do much with in terms of increasing gains to improve performance, so we won't even try to get good performance at those frequencies. The crossover region is where model-based feedback control design can improve performance the most, which is around the frequency where the system oscillates as the gain is increased too much. The gain and phase margins have been reduced to zero or become negative. Since this controller is saturating the command, it is likely that the true crossover region is at a different frequency. The most accurate model I have oscillates at 8Hz when not saturating the command. It is less damped than the actual robot, probably because it does not include friction that is not linear with respect to velocity.

See how fast you can make the wheel go half a revolution. This will involve saturating the command for most of the movement. The feedback controller takes over when the movement slows down and stops. Below are two examples of what I was able to do. Turn in similar plots with a description of the control system that generated them.

Make a model relating motor command to wheel angle based on the data from your robot.
The Arduino code to collect the sinusoidal data that was used to figure out the direction the wheel is spinning in is here.
The code to make that first model is here.
This model only tries to predict the next velocity on the basis of the current velocity and command:
velocity[k+1] = a*velocity[k] + b*command[k]
Look at readme.txt in sinusoids3/ and sinusoids3/model/ for more information.

Compare the data you took to simulations of the model with the same command. An example simulator is here. See readme.txt for more information. Turn in the comparisons of the model predictions with what the robot actually did.

Design a controller by hand and test it on your simulation. Turn in a description of the controller and why you designed it the way you did. Document its performance in simulation.

Try the controller out on the real robot. How well does the real robot match your simulation? Adjust the controller until it works well again. Turn in a description of the controller and why you designed it the way you did. Document its performance in reality, and how well the simulation matches reality.

Design an LQR controller, first in simulation, and then on the real robot. Turn in your LQR controllers, and how and why you designed them the way you did.

Bonus Part A

Bonus Part B

Bonus Part C

Can you "power-assist" the wheel so that when you try to manually turn it, it "pushes" you to turn even more? The wheel should not spin when you let go.