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';
}
}
}
Ingen kommentarer:
Send en kommentar