Commit 3d6f024f authored by Peder Kronsgaard Detlefsen's avatar Peder Kronsgaard Detlefsen
Browse files

Added code for lesson 4.

parent d29cc78f
import lejos.nxt.*;
/**
* A sensor that is able to distinguish a black/dark surface
* from a white/bright surface.
*
* Light percent values from an active light sensor and a
* threshold value calculated based on a reading over a
* black/dark surface and a reading over a light/bright
* surface is used to make the distinction between the two
* types of surfaces.
*
* @author Ole Caprani
* @version 20.02.13
*/
public class BlackWhiteSensor {
private LightSensor ls;
private int blackLightValue;
private int whiteLightValue;
private int blackWhiteThreshold;
public BlackWhiteSensor(SensorPort p)
{
ls = new LightSensor(p);
// Use the light sensor as a reflection sensor
ls.setFloodlight(true);
}
private int read(String color){
int lightValue=0;
//Vent p at vi slipper ENTER
while (Button.ENTER.isDown());
LCD.clear();
LCD.drawString("Press ENTER", 0, 0);
LCD.drawString("to callibrate", 0, 1);
LCD.drawString(color, 0, 2);
//S lnge vi ikke trykker enter afls vrdier og vent
while( !Button.ENTER.isDown() ){
lightValue = ls.readValue();
LCD.drawInt(lightValue, 4, 10, 2);
LCD.refresh();
}
return lightValue;
}
public void calibrate()
{
blackLightValue = read("black");
whiteLightValue = read("white");
// The threshold is calculated as the median between
// the two readings over the two types of surfaces
blackWhiteThreshold = (blackLightValue+whiteLightValue)/2;
}
public boolean black() {
return (ls.readValue()< blackWhiteThreshold);
}
public boolean white() {
return (ls.readValue()> blackWhiteThreshold);
}
public int light() {
return ls.readValue();
}
public int median() {
return blackWhiteThreshold;
}
}
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
private final static int forward = 1,
backward = 2,
stop = 3;
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);
}
}
import lejos.nxt.Button;
import lejos.nxt.ColorSensor;
import lejos.nxt.LCD;
import lejos.nxt.SensorPort;
import lejos.robotics.Color;
/**
* Test program for the LEGO Color Sensor in Full mode.
* A modified version of the program ColorSensorTest in samples.zip
*/
public class ColorSensorTest
{
static void displayColor(String name, int raw, int calibrated, int line)
{
LCD.drawString(name, 0, line);
LCD.drawInt(raw, 5, 6, line);
LCD.drawInt(calibrated, 5, 11, line);
}
public static void main(String [] args) throws Exception
{
ColorSensor cs = new ColorSensor(SensorPort.S1);
String colorNames[] = {"None", "Red", "Green", "Blue", "Yellow",
"Megenta", "Orange", "White", "Black", "Pink",
"Grey", "Light Grey", "Dark Grey", "Cyan"};
cs.setFloodlight(Color.WHITE);
while (!Button.ESCAPE.isDown())
{
LCD.drawString("Mode: " + "Full", 0, 0);
LCD.drawString("Color Raw Cal", 0, 1);
ColorSensor.Color vals = cs.getColor();
ColorSensor.Color rawVals = cs.getRawColor();
displayColor("Red", rawVals.getRed(), vals.getRed(), 2);
displayColor("Green", rawVals.getGreen(), vals.getGreen(), 3);
displayColor("Blue", rawVals.getBlue(), vals.getBlue(), 4);
displayColor("Light", cs.getRawLightValue(), cs.getLightValue(), 5);
LCD.drawString("Color: ", 0, 6);
LCD.drawString(colorNames[vals.getColor() + 1], 7, 6);
LCD.drawString("Color val: ", 0, 7);
LCD.drawInt(vals.getColor(), 3, 11, 7);
}
}
}
\ No newline at end of file
import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.SensorPort;
public class LightSensorTest {
public static void main (String[] args) {
BlackWhiteSensor sensor = new BlackWhiteSensor(SensorPort.S1);
sensor.calibrate();
while (!Button.ESCAPE.isDown()) {
LCD.drawString("White: " + sensor.white(), 0, 3);
LCD.drawString("Black: " + sensor.black(), 0, 4);
LCD.drawString("Light: ", 0, 5);
LCD.drawInt(sensor.light(), 4, 10, 5);
}
}
}
import lejos.nxt.*;
/**
* A simple line follower for the LEGO 9797 car with
* a light sensor. Before the car is started on a line
* a BlackWhiteSensor is calibrated to adapt to different
* light conditions and colors.
*
* The light sensor should be connected to port 3. The
* left motor should be connected to port C and the right
* motor to port B.
*
* @author Ole Caprani
* @version 20.02.13
*/
public class LineFollower
{
public static void main (String[] aArg)
throws Exception
{
final int power = 75;
ThreeColorSensor sensor = new ThreeColorSensor (SensorPort.S1);
sensor.calibrate();
while (!Button.ENTER.isUp());
while (!Button.ENTER.isDown());
LCD.clear();
LCD.drawString("Light: ", 0, 2);
int greenCounter = 0;
while (! Button.ESCAPE.isDown())
{
LCD.drawInt(sensor.light(),4,10,2);
LCD.refresh();
if ( sensor.black() ) {
Car.forward(power, 50);
greenCounter = 0;
} else if ( sensor.white() ) {
Car.forward(50, power);
greenCounter = 0;
} else if ( sensor.green() ) {
Car.forward(power, power);
greenCounter++;
}
if (greenCounter >= 50) {
Car.stop();
}
if (Button.ENTER.isDown()) {
greenCounter = 0;
}
Thread.sleep(10);
}
Car.stop();
LCD.clear();
LCD.drawString("Program stopped", 0, 0);
LCD.refresh();
}
}
import lejos.nxt.*;
/**
* A simple line follower for the LEGO 9797 car with
* a light sensor. Before the car is started on a line
* a BlackWhiteSensor is calibrated to adapt to different
* light conditions and colors.
*
* The light sensor should be connected to port 3. The
* left motor should be connected to port C and the right
* motor to port B.
*
* @author Ole Caprani
* @version 20.02.13
*/
public class LineFollowerCal
{
public static void main (String[] aArg)
throws Exception
{
final int power = 80;
BlackWhiteSensor sensor = new BlackWhiteSensor(SensorPort.S3);
sensor.calibrate();
LCD.clear();
LCD.drawString("Light: ", 0, 2);
while (! Button.ESCAPE.isDown())
{
LCD.drawInt(sensor.light(),4,10,2);
LCD.refresh();
if ( sensor.black() )
Car.forward(power, 0);
else
Car.forward(0, power);
Thread.sleep(10);
}
Car.stop();
LCD.clear();
LCD.drawString("Program stopped", 0, 0);
LCD.refresh();
}
}
import lejos.nxt.*;
/**
* A simple line follower for the LEGO 9797 car with
* a light sensor. Before the car is started on a line
* a BlackWhiteSensor is calibrated to adapt to different
* light conditions and colors.
*
* The light sensor should be connected to port 3. The
* left motor should be connected to port C and the right
* motor to port B.
*
* @author Ole Caprani
* @version 20.02.13
*/
public class PIDLineFollower
{
public static void main (String[] aArg)
throws Exception
{
BlackWhiteSensor sensor = new BlackWhiteSensor (SensorPort.S1);
sensor.calibrate();
int offset = sensor.median();
//int offset = 46;
final double dT = 0.015;
final double Pc = 1;
double Kp = 150 * 0.6; // Kc = 150, 0.6 * KC
double Ki = 2 * Kp * dT / Pc; // 2 * Kp * dT / Pc
double Kd = Kp * Pc / (8 * dT); // Kp * Pc / (8 * dT)
final int Tp = 65;
int integral = 0;
int lastError = 0;
int derivative = 0;
while (!Button.ENTER.isUp());
while (!Button.ENTER.isDown());
LCD.clear();
LCD.drawString("Light: ", 0, 1);
LCD.drawString("Error: ", 0, 2);
LCD.drawString("Integral: ", 0, 3);
LCD.drawString("Derivative: ", 0, 4);
LCD.drawString("Turn: ", 0, 5);
while (! Button.ESCAPE.isDown())
{
LCD.drawInt(sensor.light(),4,10,1);
LCD.refresh();
int lightValue = sensor.light();
int error = lightValue - offset;
integral += error;
derivative = error - lastError;
double turn = Kp * error + Ki * integral + Kd * derivative;
turn /= 100;
int powerLeft = Tp - (int)turn;
int powerRight = Tp + (int)turn;
Car.forward(powerLeft, powerRight);
lastError = error;
LCD.drawInt(error, 4, 10, 2);
LCD.drawInt(integral, 4, 10, 3);
LCD.drawInt(derivative, 4, 10, 4);
LCD.drawInt((int)turn, 4, 10, 5);
Thread.sleep(10);
}
Car.stop();
LCD.clear();
LCD.drawString("Program stopped", 0, 0);
LCD.refresh();
}
}
import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.SensorPort;
public class ThreeSensorTest {
public static void main (String[] args) {
ThreeColorSensor sensor = new ThreeColorSensor(SensorPort.S1);
sensor.calibrate();
while (!Button.ESCAPE.isDown()) {
LCD.drawString("White: " + sensor.white(), 0, 3);
LCD.drawString("Black: " + sensor.black(), 0, 4);
LCD.drawString("Green: " + sensor.green(), 0, 5);
LCD.drawString("Light: ", 0, 6);
LCD.drawInt(sensor.light(), 4, 10, 6);
}
}
}
Markdown is supported
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