Code from Lab session 6

Main Class

import lejos.nxt.*;

public class Main {

// Instantiating light sensors for port 1, 2 and 3

static LsSensor lsSensor1 = new LsSensor(SensorPort.S1);

static LsSensor lsSensor2 = new LsSensor(SensorPort.S2);

static LsSensor lsSensor3 = new LsSensor(SensorPort.S3);

// Speed control initialization

static int maxPower = 80;

static int meanPower = 60;

static int minPower = 0;

static int chosenPower = maxPower;

// Motor control states

static final int flt = 4;

static final int stop = 3;

static final int forward = 1;

static final int backward = 2;

// SideOfLine set as TRUE for driving with center sensor at right side of the line

// and set as FALSE for left side.

static boolean sideOfLine = true;

// True if 90 degrees corner has been detected

static boolean corner = false;

// True if 90 degrees corner has been detected

static boolean sharp = false;

// Time control for shifting line orientation, corner mode and motor reverse

static long time1 = 0;

// Time log for start of execution

static long time2 = 0;

// Array for the three light sensors

static char[] sensors = new char[3];

static int rightSensor = 0;

static int centerSensor = 1;

static int leftSensor = 2;

// Combined string of the sensors array

static String road;

public static void main (String[] aArg) throws Exception {

// Calibration of sensor.

setupSensor();

// Wait for user interaction

waitForEnter();

LCD.clearDisplay();

// Start time for driving

time2 = System.currentTimeMillis();

while( !Button.ESCAPE.isPressed() ){

lineFollower();

// Detection for 90 degrees clockwise corner

if (sensors[leftSensor]=='B' && sideOfLine && !corner){

time1 = System.currentTimeMillis();

LCD.drawString("Sensor 0 (R) in action", 0, 1);

corner = true;

sharp = true;

}

// Detection for 90 degrees counterclockwise corner

if (sensors[leftSensor]=='B' && !sideOfLine && !corner){

time1 = System.currentTimeMillis();

LCD.drawString("Sensor 2 (L) in action", 0, 1);

corner = true;

sharp = true;

}

// Reverse the robot to prepare a new line search

while(System.currentTimeMillis()-time1 <>corner && !Button.ESCAPE.isPressed()){

MotorPort.B.controlMotor(chosenPower, backward);

MotorPort.C.controlMotor(chosenPower,backward);

// Decrease speed in order to determine line more precise

chosenPower = meanPower;

}

if(corner){

if(System.currentTimeMillis()-time1 > 10000 && sharp){

chosenPower = maxPower;

sharp = false;

// toggle the side of line follower procedure

if(sideOfLine){

// Ensure that centerSensor is on black

while(sensors[centerSensor] == 'W' && !Button.ESCAPE.isPressed()){

readSensors();

turnLeft(chosenPower);

}

// Toggle side of the line detection

while(sensors[centerSensor] == 'B' && !Button.ESCAPE.isPressed()){

readSensors();

turnLeft(chosenPower);

}

} else {

// Ensure that centerSensor is on black

while(sensors[centerSensor] == 'W' && !Button.ESCAPE.isPressed()){

readSensors();

turnRight(chosenPower);

}

// Toggle side of the line detection

while(sensors[centerSensor] == 'B' && !Button.ESCAPE.isPressed()){

readSensors();

turnRight(chosenPower);

}

}

// Indicate side of line detection

sideOfLine = !sideOfLine;

}

// Corner option ended

if(System.currentTimeMillis()-time1 > 15000){

corner = false;

}

}

// Write total time for a complete drive

if(road.equals("BBB")){

LCD.drawInt((int) (System.currentTimeMillis()-time1), 0, 4);

}

}

}

static void lineFollower(){

readSensors();

LCD.drawString(road, 0, 0);

LCD.refresh();

// Line following at the left side of the line

if (sensors[centerSensor] == 'B' && !sideOfLine) {

turnLeft(chosenPower);

}

if (sensors[centerSensor] == 'W' && !sideOfLine) {

turnRight(chosenPower);

}

// Line following at the right side of the line

if (sensors[centerSensor] == 'B' && sideOfLine) {

turnRight(chosenPower);

}

if (sensors[centerSensor] == 'W' && sideOfLine) {

turnLeft(chosenPower);

}

}

static void turnLeft(int power){

if(!sharp || sideOfLine){

// Normal line following

MotorPort.B.controlMotor(power, forward);

MotorPort.C.controlMotor(0,flt);

}else{

// Sharp turn for 90 degrees corner

MotorPort.B.controlMotor(power, forward);

MotorPort.C.controlMotor(power,backward);

}

}

static void turnRight(int power){

if(!sharp || !sideOfLine){

// Normal line following

MotorPort.B.controlMotor(0,flt);

MotorPort.C.controlMotor(power, forward);

}else{

// Sharp turn for 90 degrees corner

MotorPort.B.controlMotor(power,backward);

MotorPort.C.controlMotor(power, forward);

}

}

static void goForward(int power){

MotorPort.B.controlMotor(power, forward);

MotorPort.C.controlMotor(power, forward);

}

static void readSensors(){

sensors[leftSensor] = lsSensor1.colorDetection();

sensors[centerSensor] = lsSensor2.colorDetection();

sensors[leftSensor] = lsSensor3.colorDetection();

road = sensors[leftSensor] + "" +sensors[centerSensor] +"" + sensors[leftSensor]+"";

// Error detection (Green should only appear on all three sensors otherwise it is black)

if (!road.equals("GGG")){

for(int i =0 ; i <3;>

if(sensors[i] == 'G'){

sensors[i] = 'B';

}

}

}

road = sensors[leftSensor] + "" +sensors[centerSensor] +"" + sensors[leftSensor]+"";

}

// Calibration for the sensors by user interaction

static void setupSensor(){

LCD.drawString("Press ENTER cal ->", 0, 0);

LCD.drawString("White:", 0, 1);

LCD.refresh();

waitForEnter();

calSensor('W');

LCD.drawString("White: Ok", 0, 1);

LCD.drawString("Black:", 0, 2);

LCD.refresh();

waitForEnter();

calSensor('B');

LCD.drawString("Black: Ok", 0, 2);

LCD.drawString("Green:", 0, 3);

LCD.refresh();

waitForEnter();

calSensor('G');

LCD.drawString("Green: Ok", 0, 3);

LCD.refresh();

lsSensor1.setTreshhold();

lsSensor2.setTreshhold();

lsSensor3.setTreshhold();

}

// Sub function for the calibration

static void calSensor(char theColor){

lsSensor1.calLsSensor(theColor);

lsSensor2.calLsSensor(theColor);

lsSensor3.calLsSensor(theColor);

}

// Completed task for press on enter button

static void waitForEnter(){

while( Button.ENTER.isPressed() );

while( !Button.ENTER.isPressed() );

}

}

Sensor Class

import lejos.nxt.*;

import lejos.nxt.addon.RCXLightSensor;

public class LsSensor {

private RCXLightSensor ls;

private int blackLightValue;

private int whiteLightValue;

private int greenLightValue;

public int blackWhiteThreshold;

public int blackGreenThreshold;

public int greenWhiteThreshold;

// Method for instantiating light sensors

public LsSensor(SensorPort p){

ls = new RCXLightSensor(p);

ls.setFloodlight(true);

}

// Read light sensor value

public int readLsSensor(){

return ls.readValue();

}

// Calibrating light sensors for either black, white or green

public void calLsSensor(char theColor){

// B = Black, W = White, G=Green

switch(theColor){

case 'B':

blackLightValue = readLsSensor();

break;

case 'W':

whiteLightValue = readLsSensor();

break;

case 'G':

greenLightValue = readLsSensor();

break;

default:

break;

}

}

// Set threshold values based on calibrated color values.

public void setTreshhold(){

blackWhiteThreshold = (greenLightValue+whiteLightValue)/2;

blackGreenThreshold = (blackLightValue+greenLightValue)/2;

greenWhiteThreshold = (greenLightValue+whiteLightValue)/2;

}

// Detection of color

public char colorDetection(){

int lightValue = ls.readValue();

if (lightValue <= blackGreenThreshold){

return 'B';

}

else if (lightValue <= greenWhiteThreshold){

return 'G';

}

else {

return 'W';

}

}

}

Lab session 4

25092009
Attendes: Jesper Jakobsen, Mads Hansen Lund og Michael Nygaard Pedersen
Goal
The goal is to build and code a two wheel self-balancing robot based on the NXT main unit.

Sub goals
The robot software and construction should be based on the software from Ref1. This demanding task should outline the limitations with the NXT (including sensor) and embedded Java as a development environment.

Plan
· Build and evaluate the robot from Ref1
· Code the robot and evaluate the result based on default coefficients
· Experiments involving different environments to evaluate the performance of the light sensor
· Evaluate the PID regulator performance with different coefficients

Results
PIDThe lab session made it obvious that the NXT has its limitations. There is of cause quantization issues, sampling time issues and saturation issues when implementing a “high speed” control algorithm. All the issues are illustrated in a simplified way in Figure 1.

Figure 1 - Simplified control model

The NXT motor is modeled as a simple first order transfer function and the quantizer is modeling the A/D converter in the light sensor. The useful resolution is experienced as 10 bit. The OptoGain is assuming a linear relationship between the motor correction and measurement (This is only true, if the robot is tilting back and forth without moving back or forth). The saturation is implemented because of the motor power interval.
Figure 2 shows the simplified motor step response, which indicates all the mentioned issues. The sampling frequency is assumed to be 200Hz. The system pole is set with respect to sampling frequency and has an oscillation frequency around 10Hz (related to system pole).

Figure 2 - Step response with default coefficients

Description of experiment
The experiment deals with the well known concept of a balancing two wheeled robot (Ref2).
PID (proportional-integral-derivative) controller
The PID controller has three control parameters, which is known proportional, integral and derivative respectively (shown in Figure 3). The proportional parameter regulates directly from the error, which in our case is the "distance" error. If the proportional parameter is too big, it will result in an excessive over shoot, which will put the robot unwieldy state. The proportional regulator maintains a steady state error of the distance. The steady state error is optimized by the integral parameter. The integral parameter regulates on a integrated value of the distance error integrated by Euler integral.
By increasing the proportional - and the integral parameters the over / under shoot grows “proportional”. The over / under shoot maintains in unwieldy state for the robot. Instead of decreasing the proportional - and the integral parameters the derivative parameter can be used. The derivative parameter regulates on the slope from the error. The slope from the error is in our case the speed.

Figure 3 – The block diagram showing concept of the PID controller (Ref3).

By observing the robot the parameters were adjusted based on this knowledge.
The code
A Sejway object is instantiated in the main method followed by different three method calls: sej.getBalance(), sej.pidControl() and sej.shutDown().
public static void main(String[] args) {
LCD.drawString("Level: ", 0, 0);
Sejway sej = new Sejway();
sej.getBalancePos();
sej.pidControl();
sej.shutDown();
}
The getBalance() method continuously assigns the current value read from the NXT light sensor to the global variable offset until the ENTER button is pressed. Offset is the setpoint used in the PID control algorithm.
The pidControl() method obviously contains the PID control algorithm, which determines a pid_val and sets the motor power (same for both motors) according to this calculated value. The polarity of pid_val determines the direction of the motors (i.e. forward or backwards).
A data logger routine was added to the pidControl() method (shown below).
DataLogger dl = new DataLogger("Sample.txt");
int posLevel;
posLevel = normVal - offset;
LCD.drawInt(posLevel,3,7,0);
dl.writeSample(posLevel);
A DataLogger object is instantiated. The current position is sampled and logged into a file named Sample.txt.
The pidControl() method loops until the ESCAPE button is pressed after which the NXT robot is shut down.

Observations
The robot was in all cases very rough in its way of adjusting the position and it was really balancing for more than a few seconds. It is possible to get it balancing; no doubt about that – but battery condition and harmonic light are the most important parameters.
It has become obvious that although the light sensor can be used for the experiment it is not the ideal sensor for the job. The limited time at this lab session was used to observe the behavior of the robot under different light condition, different surfaces with different coefficients.

Pictures of Lego model
Different approaches were investigated. Figure 4 shows the standard robot from Ref1.

Figure 4 - Picture of robot
Figure 5 shows a modified edition, where the light sensor measurement is mechanical amplified. This construction will improve the light sensor range and make the robot correspond quicker for a smaller angular error.

Figure 5 - Picture of modified robot

Problems encountered
The robot was very unstable in most cases and it impossible to get robot to balance for more than a few seconds. The battery was not able to be fully charged and this was also a problem, because the motor would not be powered with full effect.

Conclusion
Based on the simple simulation and observations from the experiments, it can be concluded that the setup lies very close to system oscillations frequency. That means if the battery is not fully charged the robot will oscillate because of the physics in the system. The robot will balance under the right conditions with a fine tuned PID, but a different approach must be considered if the system should work properly.
The code contains several mysterious factors, which might or might not be based on scientific calculations or measurements. But because of the limited time this was not investigated.

References
Ref 1 http://www.philohome.com/nxtway/nxtway.htm
Ref2 http://www.segway.dk/products/
Ref3 http://en.wikipedia.org/wiki/File:Pid-feedback-nct-int-correct.png