NXT Programming lesson 5 & lesson 6

Date: 30. September 2010 & 7 october 2010

Duration of activity: 30 sept: (14 - 17) & 7 oct: (9.45 - 18.30)

Group members participating: Nikki & Knud

---

GOAL for exercise 5:
We decided to start the build of robot and partly follow the discussion papers for the exercise, by implementing a line follower that we could use in the robot race for lesson 6

Plan for achieving the goal
Our idea is to use 3 sensors in front of the car to make it follow a black line.The amount of turning is controlled by a PID regulator for the middle sensor's error compared to the calibrated black light value. The 2 flanking sensors are used for deciding which motor the PID should regulate power to, thus changing direction of the robot. We also hoped the extra sensors would help in detecting the splitting black line on the race track and know when to turn,

Example 1:

The robot has strayed of course, and the middle sensor is only halfway on the black line. The left sensor is also halfway on the black line, and the right sensor is not on the black line.

Because the left sensor is sensing more black than the right sensor, we know we need to turn right to get back on track. The left motor will have a predetermined amount of power applied to it, and the right motor will have the predetermined amount of power applied minus the value generated by our PID controller. The bigger the error on the middle sensor, the more power will be subtracted by the PID, making the robot turn sharper.

Example 2:

Middle sensor is exactly on the black line. PID should make zero regulation and no turning would occur

Due to small variances in sensor accuracy, changing ambient light, reflections, and dirt on the black line, we will almost never get a 0% error compared to the calibrated value.

A small, but almost unnoticeable turning will probably occur.

Results

We did as planned, but often the robot got off track. We had hardcoded the constant parts of P, I and D so it was time-consuming to finetune them. In the end we made it possible to adjust the constants directly on the LCD of the robot. We followed directions from wikipedia (http://en.wikipedia.org/wiki/PID_controller#Manual_tuning) on how to fine tune the constants. Finally we got it to consistently follow the line at a pretty good speed. The fintuning part was actually done in exercise 6.

PID Regulator:

  while (! Button.ESCAPE.isPressed())
  {    
   light = centerS.getNormalizedLightValue();
   leftLight = leftS.getNormalizedLightValue();
   rightLight = rightS.getNormalizedLightValue();
   backLight = backS.getNormalizedLightValue();
   difference = leftLight-rightLight;
   
   // Find sensor that is at least almost black
   if(leftLight <= blackWhiteThreshold) {
    lastBlackSensor = LEFT_SENSOR;
   }else if(rightLight <= blackWhiteThreshold) {
    lastBlackSensor = RIGHT_SENSOR;
   }
   
   error = light - centerSetPoint;
   // Proportional term
   Pout = Kp*error;
   // Integral term
   Iout = Ki* ( (prev_error + error)/2 );
   // Derivative term
   Dout = Kd * (error - prev_error);
   prev_error = error;
   power = Math.round(Pout + Iout + Dout); 
   power = Math.abs(power);
   power = Math.min(power, maxPower);
   
   if(iTimer == 0 && timer.elapsed() > 4500) {
    Sound.beep();
    iTimer++;
    LCD.drawString("TIMER: FIRST     ", 0, 7);
    Car.forward(100, 60);
    lastBlackSensor = LEFT_SENSOR;
    Thread.sleep(800);
    timer.reset();
   }
   if(iTimer == 1 && timer.elapsed() > 6300) {
    Sound.beep();
    iTimer++;
    LCD.drawString("TIMER: SECOND     ", 0, 7);
    Car.forward(60, 100);
    lastBlackSensor = RIGHT_SENSOR;
    Thread.sleep(600);
    timer.reset();
   }
   
    if ( difference <>


    { 
     Car.forward(maxPower-power,maxPower);
     LCD.drawString("PID: Left          ", 0, 7);
    }
    else if (0 <>


    {
     Car.forward(maxPower, maxPower-power);
     LCD.drawString("PID: Right         ", 0, 7);     
    }
    break;
   Thread.sleep(10);
  }

GOAL for exercise 6:

Complete the racetrack within a reasonable amount of time.

We wanted to use the linefollower from exercise 5 and try to make the robot detect when to turn and when to stop. A extra sensor on the back of the robot, could be used to detect when the robot entered the goal zone.

Another solution to turning could be to use hard-coded turns and states.

The race track:
Rules: (http://www.legolab.daimi.au.dk/DigitalControl.dir/NXT/Lesson6.dir/Lesson.html)

• The car must start from the start area. No part of the car is allowed to exceed the start area.
• A push on ENTER starts the car and the car should then follow the track to the retrace area, the top platform. From the retrace area the car should drive back until the car drives into the start area again. The car should then stop and time elapsed since start should be shown in the LCD. The car should be inside the start area before it stops.
• When on the top the car should be inside the retrace area before going back.

We are allowed to use any LEGO elements to build the car.

Results
Video of the result 38,49 secs:
http://www.youtube.com/watch?v=slqSYa_75b0

We ended up using the linefollower, but it had a lot of trouble turning on the way up the ramp, so we decided to use states and hardcoded turns. The first 2 turns were achieved by timing and harcoding the turn. The top turn was achieved by detecting the thick black line across and then a hardcoded 180degree turn.

source code:

http://www.liscom.dk/lego/Lab5LineFollow/Lab5LineFollow.java 

http://www.liscom.dk/lego/Lab5LineFollow/Car.java 

Hardcoded turns and states:

// Hardcoded turns
if(iTimer == 0 && timer.elapsed() > 4500) {
iTimer++;
Car.forward(100, 60);
lastBlackSensor = LEFT_SENSOR;
Thread.sleep(800);
timer.reset();
}
if(iTimer == 1 && timer.elapsed() > 6300) {
iTimer++;
Car.forward(60, 100);
lastBlackSensor = RIGHT_SENSOR;
Thread.sleep(600);
timer.reset();
}

// Start of state selection
if(timer.elapsed()>4000 && light < blackWhiteThreshold && leftLight < blackWhiteThreshold && rightLight < blackWhiteThreshold && backLight < blackWhiteThreshold) {
state = STATE_GOAL;
}else if(iTimer== 2 && timer.elapsed()>4000 && light < blackWhiteThreshold && leftLight < blackWhiteThreshold && rightLight < blackWhiteThreshold) {
state = STATE_ALL_BLACK;
}else if(leftLight > blackWhiteThreshold && rightLight > blackWhiteThreshold && light > blackWhiteThreshold) {
state = STATE_FIND_BLACK;
} else {
state = STATE_PID;
}
// End of state selection   

switch(state) {

case STATE_PID:
if ( difference < 0 )
{ 
Car.forward(maxPower-power,maxPower);
LCD.drawString("PID: Left          ", 0, 7);
}
else if (0 < difference)
{
Car.forward(maxPower, maxPower-power);
LCD.drawString("PID: Right         ", 0, 7);     
}
break;

case STATE_FIND_BLACK:
if(lastBlackSensor==LEFT_SENSOR) {
Car.forward(70, 100);
LCD.drawString("Finding Black: Right", 0, 7);          
}else {
Car.forward(100, 70);
LCD.drawString("Finding Black: Left", 0, 7);     
}
break;

case STATE_ALL_BLACK:
iTimer = 3;
Sound.buzz();
LCD.drawString("HALFWAY        ", 0, 7);
Car.forward(100, 100);
Thread.sleep(400);
Car.forward(100, -100);
Thread.sleep(400);
lastBlackSensor = RIGHT_SENSOR;
timer.reset();
break;

case STATE_GOAL:
Car.forward(100,100);
Thread.sleep(100);
Car.forward(0,0);
Sound.buzz();
Thread.sleep(200);
Sound.buzz();
LCD.clearDisplay();
LCD.drawInt(goalTimer.elapsed(), 0, 0);
Thread.sleep(10000);
return;
}