import java.io.DataInputStream;
import java.io.IOException;
import java.util.ArrayList;
import java.util.List;
import lejos.nxt.Button;
import lejos.nxt.ColorLightSensor;
import lejos.nxt.LCD;
import lejos.nxt.LightSensor;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.SensorPortListener;
import lejos.nxt.Sound;
import lejos.nxt.SoundSensor;
import lejos.nxt.UltrasonicSensor;
import lejos.nxt.addon.ColorSensor;
import lejos.nxt.comm.BTConnection;
import lejos.nxt.comm.Bluetooth;
import lejos.robotics.Colors;
import lejos.robotics.navigation.Pilot;
import lejos.robotics.navigation.TachoPilot;
/**
* Kazetora adalah robot yang menunggu sambungan bluetooth dari Hayate mengenai
* rute yang harus dia ambil.
*
* @author Fukushi Fighters
*
*/
public class TemanTotoro {
// 1 maju ke depan lurus
// 2 belok kanan
// 3 belok kiri
public enum State {
JALAN, SIMPANGAN, CARI, SAMPAI,
}
static State state = State.JALAN;
static Pilot pilot = new TachoPilot(5.6f, 12.f, Motor.C, Motor.A);
// static LightSensor ls = new LightSensor(SensorPort.S1);
static SoundSensor sensorSuara = new SoundSensor(SensorPort.S1);
static ColorLightSensor sensorWarna = new ColorLightSensor(SensorPort.S2,
ColorLightSensor.TYPE_COLORFULL);
static UltrasonicSensor sensorJarak = new UltrasonicSensor(SensorPort.S3);
static boolean searchSimpangan = true;
static int stateSound = 0;
static int DEPANBELAKANGSATUAN = 60;
static int KANANKIRISATUAN = 90;
static int SPACE = 20;
public static void main(String[] args) throws IOException {
// Motor A -> Roda
// Motor C -> Naik/Turun Pensil Turun = -180
// Motor B -> Rotasi Pensil Kiri = 90
/*
tulis1();
spasi();
tulis2();
spasi();
tulis3();
spasi();
tulis4();
spasi();
tulis5();
spasi();
tulis6();
spasi();
tulis7();
spasi();
tulis8();
spasi();
tulis9();
spasi();
tulis0();
spasi();
minus();
spasi();
equal();
*/
draw();
}
private static void minus(){
keBelakang();
tulisKiri();
keKanan();
keBelakang();
}
private static void equal(){
keBelakang();
tulisKiri();
keKanan();
keBelakang();
tulisKiri();
keKanan();
}
private static void spasi() {
Motor.A.rotate(-SPACE);
}
private static void tulis1() {
tulisBawah();
tulisBawah();
}
private static void tulis2() {
tulisKiri();
keKanan();
tulisBawah();
tulisKiri();
tulisBawah();
tulisKanan();
}
private static void tulis3() {
tulisKiri();
keKanan();
tulisBawah();
tulisKiri();
keKanan();
tulisBawah();
tulisKiri();
keKanan();
}
private static void tulis4() {
keKiri();
tulisBawah();
tulisKanan();
keDepan();
tulisBawah();
tulisBawah();
}
private static void tulis5() {
tulisKiri();
tulisBawah();
tulisKanan();
tulisBawah();
tulisKiri();
keKanan();
}
private static void tulis6() {
tulisKiri();
tulisBawah();
tulisKanan();
tulisBawah();
tulisKiri();
keDepan();
tulisBawah();
keKanan();
}
private static void tulis7() {
keKiri();
tulisKanan();
tulisBawah();
tulisBawah();
}
private static void tulis8() {
tulisBawah();
tulisBawah();
tulisKiri();
keKanan();
keDepan();
tulisKiri();
keKanan();
keDepan();
tulisKiri();
tulisBawah();
tulisBawah();
keKanan();
}
private static void tulis9() {
tulisKiri();
tulisBawah();
tulisKanan();
keDepan();
tulisBawah();
tulisBawah();
tulisKiri();
keKanan();
}
private static void tulis0() {
tulisKiri();
tulisBawah();
tulisBawah();
tulisKanan();
keDepan();
keDepan();
tulisBawah();
tulisBawah();
}
private static void keDepan() {
Motor.A.rotate(DEPANBELAKANGSATUAN / 2);
}
private static void keBelakang() {
Motor.A.rotate(-DEPANBELAKANGSATUAN / 2);
}
private static void keKiri() {
Motor.B.rotate(KANANKIRISATUAN);
}
private static void keKanan() {
Motor.B.rotate(-KANANKIRISATUAN);
}
private static void draw() throws IOException {
// Wait for connection
LCD.drawString("Menunggu koneksi", 0, 0);
LCD.refresh();
BTConnection btc = Bluetooth.waitForConnection();
DataInputStream dis = btc.openDataInputStream();
LCD.clearDisplay();
LCD.drawString("Sudah dapat koneksi", 0, 0);
LCD.refresh();
String equation = "";
char c = dis.readChar();
while(c != 'e') {
equation += c;
c = dis.readChar();
}
LCD.clearDisplay();
LCD.drawString("Persamaan", 0, 0);
LCD.refresh();
LCD.drawString(equation, 0, 1);
// Sekarang dipanggil untuk digambar
gambar(equation);
LCD.drawString("Mission Accomplish", 0, 2);
}
private static void gambar(String equ) {
for(int i=0; i<equ.length(); i++) {
char c = equ.charAt(i);
switch(c) {
case '0' :
tulis0();
break;
case '1' :
tulis1();
break;
case '2' :
tulis2();
break;
case '3' :
tulis3();
break;
case '4' :
tulis4();
break;
case '5' :
tulis5();
break;
case '6' :
tulis6();
break;
case '7' :
tulis7();
break;
case '8' :
tulis8();
break;
case '9' :
tulis9();
break;
case '-' :
minus();
break;
case '=' :
equal();
break;
}
spasi();
}
}
private static void tulisKanan() {
Motor.C.rotate(-180);
Motor.B.rotate(-KANANKIRISATUAN);
Motor.C.rotate(180);
}
private static void tulisKiri() {
Motor.C.rotate(-180);
Motor.B.rotate(KANANKIRISATUAN);
Motor.C.rotate(180);
}
private static void tulisAtas() {
Motor.C.rotate(-180);
Motor.A.rotate(DEPANBELAKANGSATUAN / 2);
Motor.C.rotate(180);
}
private static void tulisBawah() {
Motor.C.rotate(-180);
Motor.A.rotate(-DEPANBELAKANGSATUAN / 2);
Motor.C.rotate(180);
}
private static boolean isOutside() {
int colorVal = 0;
colorVal = sensorWarna.readValue();
return (colorVal != 6 && colorVal != 5);
}
private static void delay() {
try {
Thread.sleep(50);
} catch (InterruptedException e) {
// TODO Auto-generated catch block
// e.printStackTrace();
}
}
private static void dummy() {
jalan("31123");
}
private static void connect() {
try {
LCD.drawString("Menunggu koneksi", 0, 0);
LCD.refresh();
BTConnection btc = Bluetooth.waitForConnection();
DataInputStream dis = btc.openDataInputStream();
LCD.drawString("Sudah dapat koneksi", 0, 1);
LCD.refresh();
char read = dis.readChar();
String arah = "";
LCD.drawString("Pesan : " + read, 0, 2);
LCD.refresh();
while (read != 'e') {
// Ambil semua petunjuk arah dari Hayate sampai karakter
// sentinel
arah += read;
read = dis.readChar();
}
dis.close();
btc.close();
LCD.drawString("Arah " + arah, 0, 0);
LCD.refresh();
// jalan("" + arah);
} catch (Exception e) {
LCD.drawString(e.toString(), 0, 3);
LCD.refresh();
while (true) {
}
}
}
private static void jalan(String arah) {
// pilot.setMoveSpeed(pilot.getMoveSpeed() / 2);
// pilot.setMoveSpeed(pilot.getMoveSpeed() / 1.5f);
for (int i = 0; i < arah.length(); i++) {
while (searchSimpangan) {
while (onJalan()) { // jalan 40 - 43
pilot.forward();
}
while (onOutside()) { // outside 58 - 65
int the = 5;
pilot.rotate(the);
if (onOutside()) {
the = the * -2;
pilot.rotate(the);
}
}
while (onSimpangan()) {
searchSimpangan = false;
Sound.beep();
break;
}
}
int direction = Integer.parseInt("" + arah.charAt(i));
if (direction == 1) {
searchSimpangan = true;
while (onSimpangan())
pilot.forward();
} else if (direction == 2) {
searchSimpangan = true;
turnRight();
} else if (direction == 3) {
searchSimpangan = true;
turnLeft();
}
}
while (!onFinish()) {
while (onJalan()) { // jalan 40 - 43
pilot.forward();
}
while (onOutside()) { // outside 58 - 65
int the = 5;
pilot.rotate(the);
if (onOutside()) {
the = the * -2;
pilot.rotate(the);
}
}
}
Sound.buzz();
pilot.rotate(270);
Sound.beepSequenceUp();
}
public static boolean cari() {
int theta = 6;
pilot.rotate(theta);
while (onOutside()) {
theta = theta * -2;
pilot.rotate(theta);
}
return theta > 0;
}
// private static boolean onSimpangan() {
// return ls.readValue() >= 50 && ls.readValue() <= 65;
// }
//
// private static boolean onJalan() {
// return ls.readValue() > 25 && ls.readValue() < 38;
// }
//
// private static boolean onOutside() {
// return ls.readValue() > 39 && ls.readValue() < 48;
// }
private static boolean onOutside() {
return sensorWarna.readValue() == 6;
}
private static boolean onJalan() {
int colorVal = 0;
colorVal = sensorWarna.readValue();
return (colorVal != 6 && colorVal != 5);
}
private static void waitingSound() {
// TODO Auto-generated method stub
}
private static boolean onSimpangan() {
// Sound.twoBeeps();
return sensorWarna.readValue() == 5;
}
private static boolean onFinish() {
return sensorWarna.readValue() == 2;
}
private static void turnRight() {
pilot.travel(9f);
pilot.rotate(-90);
}
private static void turnLeft() {
pilot.travel(9f);
pilot.rotate(90);
}
private boolean isLoud() {
int loudness;
loudness = sensorSuara.readValue();
boolean loud = false;
LCD.drawInt(loudness, 0, 2);
LCD.refresh();
if (loudness > 65) {
loud = true;
}
return loud;
}
private void hearLoud() {
int loudness;
loudness = sensorSuara.readValue();
if (loudness > 85) {
LCD.drawInt(80, 0, 2);
LCD.refresh();
} else if (loudness > 75) {
LCD.drawInt(loudness, 0, 3);
LCD.refresh();
} else if (loudness > 65) {
LCD.drawInt(loudness, 0, 4);
LCD.refresh();
} else if (loudness > 55) {
LCD.drawInt(loudness, 0, 5);
LCD.refresh();
} else if (loudness > 45) {
LCD.drawInt(loudness, 0, 6);
LCD.refresh();
} else {
LCD.drawInt(loudness, 0, 7);
LCD.refresh();
}
}
}