Commit 3aa7638b authored by Casper's avatar Casper
Browse files

Pushed new partyfinder

parent d9e721d9
......@@ -3,13 +3,13 @@ public class PartyFinder {
static final double turnDegree = 0.08797653958;
static final long sampleInterval = 25;
static MotorPort leftMotor = MotorPort.C;
static MotorPort rightMotor = MotorPort.B;
static ADSensorPort soundSensorLeft = SensorPort.S2;
static ADSensorPort soundSensorRight = SensorPort.S3;
static int leftValue;
static int rightValue;
static int diff;
......@@ -23,38 +23,35 @@ public class PartyFinder {
leftValue = soundSensorLeft.readRawValue();
rightValue = soundSensorRight.readRawValue();
if(leftValue < 940 && rightValue < 940) {
diff = leftValue - rightValue;
diff = leftValue - rightValue;
if(diff < 0) {
// Party is to right
right(75);
}
else if(diff > 0) {
// Party is to left
left(75);
}
else {
// Party is straight ahead ( or behind you :( )
forward(75);
}
} else {
stop();
if(diff < 0) {
// Party is to right
right(70);
}
else if(diff > 0) {
// Party is to left
left(70);
}
else {
// Party is straight ahead ( or behind you :( )
forward(70);
}
// Debug info
LCD.drawInt(leftValue,4,0,1);
LCD.drawInt(rightValue,4,0,2);
LCD.drawInt(diff,4,0,3);
Thread.sleep(sampleInterval);
}
}
public static void stop() {
leftMotor.controlMotor(0, MotorPort.STOP);
rightMotor.controlMotor(0, MotorPort.STOP);
}
public static void forward( int power)
{
leftMotor.controlMotor(power, MotorPort.FORWARD);
......@@ -63,11 +60,11 @@ public class PartyFinder {
public static void left( int power)
{
leftMotor.controlMotor(power, MotorPort.FORWARD);
rightMotor.controlMotor(power / 4, MotorPort.FORWARD);
rightMotor.controlMotor(power / 3, MotorPort.FORWARD);
}
public static void right( int power)
{
leftMotor.controlMotor(power / 4, MotorPort.FORWARD);
leftMotor.controlMotor(power / 3, MotorPort.FORWARD);
rightMotor.controlMotor(power, MotorPort.FORWARD);
}
}
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