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

Pushed new partyfinder

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