Commit bb5ca761 authored by Peder Kronsgaard Detlefsen's avatar Peder Kronsgaard Detlefsen
Browse files

Added ComplexPartyFinder.

parent 3aa7638b
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;
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 controlLeft(int leftPower, int mode)
{
leftMotor.controlMotor(leftPower, mode);
}
public static void controlRight(int rightPower, int mode)
{
rightMotor.controlMotor(rightPower, mode);
}
}
import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.util.Delay;
public class PartyDriver {
public static void main (String[] aArg) {
PartyFinder follower = new PartyFinder();
LCD.drawString("PartyFinder", 0, 0);
LCD.drawString("Diff Avg: ", 0, 2);
LCD.drawString("Diff: ", 0, 3);
LCD.drawString("LeftSound: ", 0, 4);
LCD.drawString("RightSound: ", 0, 5);
while (! Button.ENTER.isDown()) {
Delay.msDelay(100);
}
follower.go();
while (! Button.ESCAPE.isDown())
{
LCD.drawInt(follower.getLeftSound(), 4,10,4);
LCD.drawInt(follower.getRightSound(), 4,10,5);
Delay.msDelay(30);
}
follower.stop();
LCD.clear();
LCD.drawString("Program stopped", 0, 0);
Delay.msDelay(2000);
}
}
import java.util.ArrayList;
import lejos.nxt.LCD;
import lejos.nxt.SensorPort;
import lejos.util.Delay;
public class PartyFinder extends Thread {
private boolean running;
private int sampleInterval = 30; // ms default value
private final int averageCount = 20;
private SensorPort sensorLeft = SensorPort.S2;
private SensorPort sensorRight = SensorPort.S3;
private int powerLeft, powerRight, soundLeft, soundRight, diff,
motorMode = Car.stop, diffAverage, soundLeftAvg, soundRightAvg;
private ArrayList<Integer> diffList = new ArrayList<Integer>();
private ArrayList<Integer> soundLeftList = new ArrayList<Integer>();
private ArrayList<Integer> soundRightList = new ArrayList<Integer>();
public PartyFinder() {
sensorLeft.setTypeAndMode(SensorPort.TYPE_SOUND_DB,
SensorPort.MODE_PCTFULLSCALE);
sensorRight.setTypeAndMode(SensorPort.TYPE_SOUND_DB,
SensorPort.MODE_PCTFULLSCALE);
}
public void go() {
running = true;
this.start();
}
public void stop() {
running = false;
}
public void run() {
// Do initial setup before main loop.
int counter = 0;
int threshold = 950;
while ( running ) {
// Read values from the sensors.
// More noise from motors behind the robot.
soundLeft = sensorLeft.readRawValue() + 125;
// Less noise in front.
soundRight = sensorRight.readRawValue();
soundLeftList.add(soundLeft);
soundRightList.add(soundRight);
if (soundLeftList.size() > averageCount) soundLeftList.remove(0);
if (soundRightList.size() > averageCount) soundRightList.remove(0);
diff = soundLeft - soundRight;
diffList.add(diff);
if (diffList.size() > averageCount) diffList.remove(0);
counter++;
if (counter < averageCount) continue;
diffAverage = getAverage(diffList);
soundLeftAvg = getAverage(soundLeftList);
soundRightAvg = getAverage(soundRightList);
powerLeft = 75;
powerRight = 75;
/*
* * Sensors:
* left = points backwards
* right = points forwards
*
* * Steps:
* 1. Read values from sensors
* 2. Compare sensor values to determine direction
* 3. Move forwards/backwards towards loud sound
* 4. Adjust angle based on difference between values.
* When forward:
* highest diff is straight ahead
* nearing zero diff when left of robot
* lower diff when right of robot
* When backwards
* lower diff is straight back (-60)
* lowest diff is right of robot (0)
* highest diff is left of robot (-150)
* 5. Save average of highest sound readings to
* know if we are getting closer.
* 6. When average doens't get lower we are at the party.
*/
if (soundLeftAvg > threshold + 125 && soundRightAvg > threshold) {
motorMode = Car.stop;
} else if (soundLeftAvg > soundRightAvg) {
motorMode = Car.forward;
} else if (soundLeftAvg < soundRightAvg) {
motorMode = Car.backward;
}
if (counter % averageCount == 0) {
threshold = (motorMode == Car.stop) ? 950 : 800;
}
// Adjust angle based on difference between values.
if (motorMode == Car.forward) {
if (diffAverage < 20) { // move left
powerLeft = 60;
} else if (diffAverage < 40) { // move right
powerRight = 60;
} else { // move straight
}
} else if (motorMode == Car.backward) {
if (diffAverage > -20) { // move right
powerRight = 60;
} else if (diffAverage < 100) { // move left
powerLeft = 60;
} else { // move straight
}
}
Car.controlLeft(powerLeft, motorMode);
Car.controlRight(powerRight, motorMode);
LCD.drawInt(diffAverage, 4, 10, 2);
LCD.drawInt(diff, 4, 10, 3);
Delay.msDelay(sampleInterval);
}
}
private int getAverage(ArrayList<Integer> values) {
float sum = 0;
for (Integer i : values) {
sum += i;
}
return (int) (sum / (float)values.size());
}
public int getLeftSound()
{
return soundLeftAvg;
}
public int getRightSound()
{
return soundRightAvg;
}
}
\ No newline at end of file
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