public static void main(String[] args) throws InterruptedException {
LightSensor left = new LightSensor(SensorPort.S3);
LightSensor right = new LightSensor(SensorPort.S4);
while (true) {
System.out.println("left: " + left.getLightValue());
System.out.println("right: " + right.getLightValue());
if (left.getLightValue() < 45 && right.getLightValue() < 45) {
Motor.A.stop();
Thread.sleep(1245);
} else if (left.getLightValue() < 45) {
Motor.B.stop();
} else if (right.getLightValue() < 45) {
Motor.A.stop();
} else {
Motor.A.backward();
Motor.B.backward();
}
}
}
// Decode msg method
public void decodeMSG(Byte msg) throws IOException {
// Motor Movement Functions
// int currentSpeedA = Motor.A.getSpeed();
// int currentSpeedB = Motor.B.getSpeed();
if (msg == MOVE_FOWARD) {
Motor.A.backward();
Motor.B.backward();
} else if (msg == MOVE_FOWARD_LEFT) {
Motor.A.backward();
Motor.B.setSpeed(motorSpeed / 2);
Motor.B.backward();
} else if (msg == MOVE_FOWARD_RIGHT) {
Motor.A.setSpeed(motorSpeed / 2);
Motor.A.backward();
Motor.B.backward();
} else if (msg == ROTATE_LEFT) {
Motor.A.backward();
Motor.B.forward();
} else if (msg == ROTATE_RIGHT) {
Motor.A.forward();
Motor.B.backward();
} else if (msg == MOVE_BACKWARD) {
Motor.A.forward();
Motor.B.forward();
} else if (msg == STOP) {
Motor.A.setSpeed(motorSpeed);
Motor.B.setSpeed(motorSpeed);
Motor.A.stop();
Motor.B.stop();
}
// Motor set Speed Function
if (msg == SET_MOTOR_SPD) {
Byte rMsg = 40;
rMsg = dis.readByte();
int spd = 9 * rMsg;
motorSpeed = spd;
Motor.A.setSpeed(spd);
Motor.B.setSpeed(spd);
}
if (msg == CUSTOM_MOVE) {
int motorB = dis.read();
int motorA = dis.read();
Motor.A.setSpeed(motorA * 9);
Motor.B.setSpeed(motorB * 9);
Motor.A.backward();
Motor.B.backward();
}
// Connection close msg
if (msg == CLOSE_COMM) {
closeBTConnection();
}
}
/*
* This method makes the robot stop.
*/
public void stop() {
Motor.A.stop();
Motor.B.stop();
}