import java.io.DataInputStream;
import java.io.DataOutputStream;
import java.io.IOException;
import java.util.ArrayList;
import java.util.List;
import javax.bluetooth.RemoteDevice;
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;
import lejos.robotics.proposal.SteeringPilot;
/**
* Kazetora adalah robot yang menunggu sambungan bluetooth dari Hayate mengenai
* rute yang harus dia ambil.
*
* @author Fukushi Fighters
*
*/
public class Totoro {
// 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 String PESAN = "";
static int stateSound = 0;
public static void main(String[] args) throws IOException {
//serius();
//System.out.println(processEquation("11-10="));
String stringKirim = processEquation("11-10=");
kirimPesan(stringKirim);
}
private static void serius() throws IOException {
// Jaringan Syaraf Tiruan
LVQ lvq = new LVQ(2, 40, new String[] { "One", "Zero" });
Neuron[] neurons = lvq.getNeurons();
// List<Double> w1 = neurons[0].getWeights();
// List<Double> w2 = neurons[1].getWeights();
double[] bobotOne = { 1.0, 0.2421261342440487, 0.24313725490196078,
0.0780280923326713, 0.01873087115751696, 0.02385184690784007,
0.19630484902011883, 0.24028118942536075, 0.24307929065513165,
0.2400346940074186, 0.08530653696497154, 0.0784313725490196,
0.0784313725490196, 0.0784313725490196, 0.0784313725490196,
0.07563327131924871, 0.07651024329898667, 0.0784313725490196,
0.08539753782418362, 0.1472920264005696 };
double[] bobotZero = { 1.0, 0.1255379044758955, 0.04216928206301075,
0.039487109326898175, 0.03931679834030101, 0.03748666994948015,
0.03937476258713017, 0.1224042537136646, 0.2453298072043821,
0.24441979861226124, 0.14002723014931992, 0.07347579121230842,
0.04022680693242189, 0.040284771179251035,
0.040284771179251035, 0.03931679834030101,
0.036421166798629216, 0.08342202608121181, 0.08903957442130953,
0.10912850631092447 };
double[] bobotMinus = { 1.0, 0.08241090542329975, 0.07761236481611082,
0.07755440056928166, 0.07755440056928166, 0.07955483994669969,
0.07955483994669969, 0.08435338055388863, 0.1283195542686032,
0.2432165650293459, 0.2627450980392157, 0.26466622728924866,
0.2667576675258787, 0.08031946518666962, 0.04221713510326079,
0.058601538924874255, 0.12312985778481687, 0.2637130708781657,
0.2633074991476032, 0.2365453822674658 };
double[] bobotEqual = { 1.0, 0.13613143456310423, 0.01766536800666603,
0.01873087115751696, 0.018484375739574815,
0.018484375739574815, 0.019495496397486895,
0.13535557464915748, 0.14808825337046047, 0.2420468241166636,
0.21216563910053887, 0.043079290655131636, 0.04395626263486957,
0.08353437282097984, 0.26639524361427835, 0.26557623588136947,
0.04672943623019424, 0.039349834952683946, 0.12573824889212887,
0.2615717975021357 };
String[] classTarget = { "One", "Zero", "Minus", "Equal" };
// List<Double> w1 = new ArrayList<Double>();
// List<Double> w2 = new ArrayList<Double>();
// for (int i = 0; i < bobotOne.length; i++) {
// w1.add(bobotOne[i]);
// w2.add(bobotZero[i]);
// }
// neurons[0].setWeights(w1);
// neurons[1].setWeights(w2);
//
// LCD.drawString("" + w1.size(), 0, 6);
// LCD.drawString("" + w2.size(), 0, 7);
// LCD.refresh();
// thread untuk merespon suara
// Thread soundMonitor = new Thread(new Runnable() {
//
// @Override
// public void run() {
// while (true) {
// int warnaVal = sensorWarna.readValue();
// int jarakVal = sensorJarak.getDistance();
// int soundVal = sensorSuara.readValue();
// LCD.drawString("Sound : " + soundVal, 0, 2);
// LCD.drawString("Jarak : " + jarakVal, 0, 3);
// LCD.drawString("Warna : " + warnaVal, 0, 4);
// LCD.refresh();
// if (sensorSuara.readValue() > 90) {
// pilot.stop();
// }
// }
//
// }
// });
// soundMonitor.setDaemon(true);
// soundMonitor.start();
pilot.setMoveSpeed(5f);
// beneran();
// palsu();
String winner = "";
while (!"=".equals(winner)) {
LCD.drawString("Klik tombol untuk START", 4, 4);
Button.ENTER.waitForPressAndRelease();
List<Double> input = new ArrayList<Double>();
// double[] one = {1.0, 0.30196078431372547, 1.0,
// 0.30196078431372547,
// 0.2980392156862745, 0.3058823529411765, 0.08235294117647059,
// 0.054901960784313725, 0.058823529411764705, 0.058823529411764705,
// 0.058823529411764705, 0.08627450980392157, 1.0, 1.0, 1.0, 1.0,
// 1.0,
// 1.0, 1.0, 1.0, 0.11372549019607843, 0.10588235294117647,
// 0.10196078431372549, 0.10196078431372549, 0.10196078431372549,
// 0.10196078431372549, 0.10588235294117647, 0.10588235294117647,
// 0.10588235294117647, 0.10588235294117647, 0.10588235294117647,
// 0.10588235294117647, 0.10980392156862745, 0.10980392156862745,
// 0.10980392156862745, 0.11372549019607843, 0.11764705882352941,
// 0.12549019607843137, 0.1411764705882353, 0.6901960784313725};
// double[] zero = {1.0, 0.19607843137254902, 0.08627450980392157,
// 0.08235294117647059, 0.0784313725490196, 0.0784313725490196,
// 0.0784313725490196, 0.08235294117647059, 0.0784313725490196,
// 0.07450980392156863, 0.0784313725490196, 0.07450980392156863,
// 0.0784313725490196, 0.0784313725490196, 0.08627450980392157,
// 0.08627450980392157, 0.09411764705882353, 0.09803921568627451,
// 0.3607843137254902, 0.3607843137254902, 0.20392156862745098,
// 0.09019607843137255, 0.08627450980392157, 0.08235294117647059,
// 0.0784313725490196, 0.0784313725490196, 0.0784313725490196,
// 0.0784313725490196, 0.0784313725490196, 0.0784313725490196,
// 0.0784313725490196, 0.0784313725490196, 0.0784313725490196,
// 0.08235294117647059, 0.08627450980392157, 0.08235294117647059,
// 0.09019607843137255, 0.12941176470588237, 0.1450980392156863,
// 1.0};
// for(int i=0; i<one.length; i++) {
// input.add(zero[i]);
// }
String jarak_capture = "";
int MOVE_VAL = 6;
int STEP = 10;
int[][] array = new int[5][2];
for (int k = 0; k < 2; k++) {
for (int i = 0; i < STEP; i++) {
pilot.travel(MOVE_VAL);
// jarak_capture += sensorJarak.getDistance() + " ";
int jarak = sensorJarak.getDistance();
array[i / 2][i % 2] = jarak;
input.add((double) jarak / 255);
LCD.drawString("Nilai " + jarak, 0, 6);
LCD.refresh();
delay();
LCD.clearDisplay();
while (isOutside()) {
// if(i == 18 || i == 19 || i == 20){
// break;
// }
int the = -6;
pilot.rotate(the);
if (isOutside()) {
the = the * -2;
pilot.rotate(the);
}
}
}
pilot.rotate(90);
// while (!Button.ENTER.isPressed()) {
//
// for (int i = 0; i < 5; i++) {
// jarak_capture = "";
// for (int j = 0; j < 2; j++) {
// jarak_capture += array[i][j] + " ";
// }
// LCD.drawString(jarak_capture, 0, i);
// }
// LCD.refresh();
// // LCD.drawString(jarak_capture, 0, 4);
// }
}
LCD.clear();
// while (!Button.ENTER.isPressed()) {
// LCD.refresh();
// LCD.drawString("Sebelum classification", 0, 0);
// LCD.refresh();
// }
// String winner = lvq.classification(input);
// /////////// Danger ////////////
double minDistance = Double.MAX_VALUE;
double distOne = 0;
for (int i = 0; i < input.size(); i++) {
double selisih = input.get(i) - bobotOne[i];
double kuadrat = selisih * selisih;
distOne += kuadrat;
}
distOne = Math.sqrt(distOne);
LCD.drawString("1 " + distOne, 0, 1);
LCD.refresh();
double distZero = 0;
for (int i = 0; i < input.size(); i++) {
double selisih = input.get(i) - bobotZero[i];
double kuadrat = selisih * selisih;
distZero += kuadrat;
}
distZero = Math.sqrt(distZero);
LCD.drawString("0 " + distZero, 0, 3);
double distMinus = 0;
for (int i = 0; i < input.size(); i++) {
double selisih = input.get(i) - bobotMinus[i];
double kuadrat = selisih * selisih;
distMinus += kuadrat;
}
distMinus = Math.sqrt(distMinus);
double distEqual = 0;
for (int i = 0; i < input.size(); i++) {
double selisih = input.get(i) - bobotEqual[i];
double kuadrat = selisih * selisih;
distEqual += kuadrat;
}
distEqual = Math.sqrt(distEqual);
winner = "";
distOne = Math.floor(distOne * 1000);
distZero = Math.floor(distZero * 1000);
distMinus = Math.floor(distMinus * 1000);
distEqual = Math.floor(distEqual * 1000);
double[] list = { distOne, distZero, distMinus, distEqual };
double min = 99999999.0;
for (int i = 0; i < 4; i++) {
if (list[i] < min)
min = list[i];
}
LCD.clear();
LCD.refresh();
LCD.drawString("" + distOne, 0, 1);
LCD.drawString("" + distZero, 0, 2);
LCD.drawString("" + distMinus, 0, 3);
LCD.drawString("" + distEqual, 0, 4);
// String winner = "";
if (distOne < distZero && distOne < distMinus
&& distOne < distEqual) {
// winner = "ONE";
winner = "1";
} else if (distZero < distOne && distZero < distMinus
&& distZero < distEqual) {
// winner = "ZERO";
winner = "0";
} else if (distMinus < distOne && distMinus < distZero
&& distMinus < distEqual) {
// winner = "MINUS";
winner = "-";
} else {
// winner = "EQUAL";
winner = "=";
}
PESAN += winner;
}
// if (distOne < distZero) {
// if(distOne < distMinus){
// if(distOne < distEqual){
// min = distOne;
// }
// }
// } else {
//
// }
// LCD.refresh();
// ///////////////////////////////
kirimPesan(processEquation(PESAN));
while (!Button.ENTER.isPressed()) {
LCD.drawString(processEquation(PESAN), 0, 5);
}
}
private static String processEquation(String equ) {
// Bagi untuk dua operand
//String[] operand = equ.split("-");
String[] operand = {"", ""};
int index = 0;
char chara = equ.charAt(index);
while(chara != '-') {
operand[0] += chara;
index++;
chara = equ.charAt(index);
}
index++;
chara = equ.charAt(index);
while(chara != '=') {
operand[1] += chara;
index++;
chara = equ.charAt(index);
}
int a = convertBinary(operand[0]);
int b= convertBinary(operand[1].substring(0, operand[1].length()));
int c = a-b;
return a+"-"+b+"="+c;
}
private static int convertBinary(String bin) {
int result = 0;
int len = bin.length();
for(int i=0; i<len; i++) {
if(bin.charAt(i) == '1') {
result += Math.pow(2, len-(i+1));
}
}
return result;
}
private static boolean kirimPesan(String pesan) throws IOException {
LCD.drawString("Mencoba menyambung", 0, 1);
LCD.refresh();
RemoteDevice partner = Bluetooth.getKnownDevice("NXT");
if (partner == null) {
LCD.drawString("partner tidak ketemu", 0, 4);
LCD.refresh();
return false;
}
BTConnection btc = Bluetooth.connect(partner);
if (btc == null) {
LCD.drawString("tidak bisa nyambung", 0, 5);
LCD.refresh();
return false;
}
LCD.drawString("Mulai mengirim", 0, 6);
LCD.refresh();
DataOutputStream dos = btc.openDataOutputStream();
for (int i = 0; i < pesan.length(); i++) {
char a = pesan.charAt(i);
LCD.drawString("Yang dikirim " + a, 0, 2);
dos.writeChar(a);
// dos.writeInt(a);
dos.flush();
}
dos.writeChar('e');
dos.flush();
dos.close();
LCD.drawString("Selesai mengirim", 0, 7);
btc.close();
return true;
}
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();
int read = dis.readInt();
String arah = "";
LCD.drawString("Pesan : " + read, 0, 2);
LCD.refresh();
while (read != 8) {
// Ambil semua petunjuk arah dari Hayate sampai karakter
// sentinel
arah += read;
read = dis.readInt();
}
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();
}
}
}