Lab 5

Date: 1. march 2012

Duration of activity: 3 hours

Group members participating: Thomas, Stefan and Jeppe

Goal

• Make a self-balancing Lego robot inspired by Steve Hassenplug.

Plan

We will start by building our self-balancing robot.

The robot has a light sensor mounted on the back that points down, toward the ground.

Our initial idea was to mount tow touch sensors to make the robot learn when it has fallen.
The first task for the robot would be to learn its balance point.
Perhaps this could be done by a binary search of different combinations of power for different periods of time. Either the robot would flip over on the other side of the balance point - and be caught by the other "leg" - thus telling that the power has been set to high. Or the the robot will stay tilted to the same side, thus touching the same touch sensor and the amount of power would have been too less. This could be one way how to make the robot learn to find its own balance.
However we decided to spend our time on making something more simple work frist.

To begin with we copied the two clases PCcarController.java and BTcontrolledCar.java into our Eclipse project. We added the Car class and added the counter() method.
Initially we had some problems running the PCcarController program, we got the error:

"Invalid layout of java.lang.Thread at name
# fatal error: Invalid layout of preloaded class"

This error was resolved in Eclipse by doing the following:

1) Run - Run Configurations

2) Click PCcarController run target

3) Click on the tab "Classpath"

4) Remove the LeJOS Runtime library from the Bootstrap Entries.

After this we got the GUI -PC program working.

Thomas implemented the text fields to enter the error values.

Results

The initial layout of the robot, with its two "support legs" with touch sensors mounted.

We had some problems getting the bluetooth connection working, so we decided to spend our time implementing a simple algorithm to make the robot balance.

The implementation of our segway controller looks as follows:

public static void main(String [] args) throws Exception {
//exit listener
Button.ESCAPE.addButtonListener(new ButtonListener() {
 public void buttonPressed(Button b) {
  System.exit(0);
 }

 public void buttonReleased(Button b) {

 }
});
LightSensor light = new LightSensor(SensorPort.S2);
while (true) {
 LCD.clear();
 LCD.drawString("Press ENTER when ready",0,0);
 LCD.refresh();
 //wait for enter press after which calibration starts
 while ( ! Button.ENTER.isDown() ) { }
 int number_of_readings = 0;
 int light_thres = 0;
 Thread.sleep(500);
 //wait for enter press to stop calibration
 while ( ! Button.ENTER.isDown() ) {
  light_thres += light.getNormalizedLightValue();
  number_of_readings++;
  LCD.clear();
  LCD.drawString("Light Thres: " + light_thres / number_of_readings, 0, 1);
  LCD.drawString("Curr: " + light.getNormalizedLightValue(),0, 2);
  Thread.sleep(10);
 }
 //calibration is averages of all measurements
 light_thres = light_thres / number_of_readings;

 //begin to balance
 int power = 0;
 float error = 0;
 int crr_light = 0;
 int dif = 0;
 int error_point = 0;
 int integral = 0;
 float error_point_scale = 8;
 float dif_scale = 4;
 while (true) {
  crr_light = light.getNormalizedLightValue();
  dif = (crr_light - light_thres) - error_point;           //d term in pid
  error_point = crr_light - light_thres;                   //p term in pid
  error = error_point_scale*error_point + dif_scale*dif;
  LCD.clear();
  LCD.drawString("Error: " + error, 0, 1);
  if (error < 0) {
   power = 50 - (int)error;
   MotorPort.A.controlMotor(power, 1);
   MotorPort.B.controlMotor(power, 1);
  } else {
   power = 50 + (int)error;
   MotorPort.A.controlMotor(power, 2);
   MotorPort.B.controlMotor(power, 2);   
   }
  Thread.sleep(5);
 }
  }
} 

We were experimenting a bit to find some good constants. It was not straight forward at all, but for the constants we ended up with:

  float error_point_scale = 8;
 float dif_scale = 4;

the robot could balance for just a few seconds.

Conclusion

Making the robot stand straight for more than a few seconds was quite difficult. If we calibrated the robot to be a bit forward bend, then it would actually stay up for just a bit longer, since it would start moving forward.

References

Lab Session 5,
http://legolab.cs.au.dk/DigitalControl.dir/NXT/Lesson5.dir/Lesson.html