Commit ef43b3d7 authored by Casper's avatar Casper
Browse files

Merge branch 'master' of gitlab.au.dk:lego-group-8/lego

parents e69b3f27 b03d109a
import lejos.nxt.*;
/**
* A locomotion module with methods to drive
* a car with two independent motors. The left motor
* should be connected to port C and the right motor
* to port B.
*
* @author Ole Caprani
* @version 17.4.08
*/
public class Car
{
// Commands for the motors
public final static int forward = 1,
backward = 2,
stop = 3,
floats = 4;
private static MotorPort leftMotor = MotorPort.C;
private static MotorPort rightMotor= MotorPort.B;
private Car()
{
}
public static void stop()
{
leftMotor.controlMotor(0,stop);
rightMotor.controlMotor(0,stop);
}
public static void forward(int leftPower, int rightPower)
{
leftMotor.controlMotor(leftPower,forward);
rightMotor.controlMotor(rightPower,forward);
}
public static void backward(int leftPower, int rightPower)
{
leftMotor.controlMotor(leftPower,backward);
rightMotor.controlMotor(rightPower,backward);
}
public static void controlLeft(int power, int mode) {
leftMotor.controlMotor(power, mode);
}
public static void controlRight(int power, int mode) {
rightMotor.controlMotor(power, mode);
}
}
import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.util.Delay;
public class WFTester {
public static void main (String[] aArg) {
int distance, power;
WallFollower follower = new WallFollower();
LCD.drawString("WallFollower", 0, 0);
LCD.drawString("Distance: ", 0, 3);
LCD.drawString("LeftPower: ", 0, 4);
LCD.drawString("RightPower: ", 0, 5);
while (! Button.ENTER.isDown()) {
Delay.msDelay(100);
}
follower.go();
while (! Button.ESCAPE.isDown())
{
distance = follower.getDistance();
//power = follower.getPower();
LCD.drawInt(distance,4,10,3);
LCD.drawInt(follower.getLeftPower(), 4,10,4);
LCD.drawInt(follower.getRightPower(), 4,10,5);
Delay.msDelay(30);
}
follower.stop();
LCD.clear();
LCD.drawString("Program stopped", 0, 0);
Delay.msDelay(2000);
}
}
import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;
import lejos.util.Delay;
public class WallFollower extends Thread {
private static final int littleTooClose = 28;
private static final int tooClose = 25;
private static final int wayTooClose = 22;
private static final int littleTooFar = 30;
private static final int tooFar = 32;
private static final int wayTooFar = 34;
private boolean running;
private int sampleInterval = 30; // ms default value
private UltrasonicSensor us = new UltrasonicSensor(SensorPort.S1);
private final int noObject = 255;
private int distance, x, desiredDistance = 25, // cm, default value
power, minPower = 50, maxPower = 100; // default values
private float diff, Pgain = 2.0f; // default value
private int motorLeft, motorRight, powerLeft, powerRight;
public WallFollower() {
}
public void go() {
running = true;
this.start();
}
public void stop() {
running = false;
}
public void run() {
x = distance = us.getDistance();
diff = 0;
while ( running ) {
distance = us.getDistance();
diff = x - distance;
x = distance;
motorLeft = Car.forward;
motorRight = Car.forward;
powerLeft = powerRight = 75;
if (x <= littleTooClose) { // Close to the wall
if (x <= wayTooClose) {
motorRight = Car.backward;
powerLeft = 65;
powerRight = 65;
} else if (x <= tooClose) {
powerRight = 55;
} else if (x <= littleTooClose) {
powerRight = 65;
}
} else { // Far from the wall
if (x >= wayTooFar) {
if (diff < 2) {
powerLeft = 55;
} else {
powerLeft = 55;
}
} else if (x >= tooFar) {
powerLeft = 60;
} else if (x >= littleTooFar) {
powerLeft = 65;
}
}
Car.controlLeft(powerLeft, motorLeft);
Car.controlRight(powerRight, motorRight);
//Delay.msDelay(sampleInterval);
}
}
public int getDistance() {
return distance;
}
public int getLeftPower()
{
return powerLeft;
}
public int getRightPower()
{
return powerRight;
}
}
\ No newline at end of file
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment