diff --git a/ExplorationHex.txt b/ExplorationHex.txt new file mode 100644 index 0000000..bf48a9a --- /dev/null +++ b/ExplorationHex.txt @@ -0,0 +1 @@ +ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff \ No newline at end of file diff --git a/ObstacleHex.txt b/ObstacleHex.txt new file mode 100644 index 0000000..c1e459d --- /dev/null +++ b/ObstacleHex.txt @@ -0,0 +1 @@ +0400000800000201c00002000400080010202040408000000000000003f80000000000000200 \ No newline at end of file diff --git a/src/algorithms/Direction.java b/src/algorithms/Direction.java new file mode 100644 index 0000000..db43f4a --- /dev/null +++ b/src/algorithms/Direction.java @@ -0,0 +1,9 @@ +package algorithms; + +public enum Direction +{ + RIGHT, + LEFT, + UP, + DOWN; +} \ No newline at end of file diff --git a/src/algorithms/ExplorationTypes.java b/src/algorithms/ExplorationTypes.java new file mode 100644 index 0000000..70111eb --- /dev/null +++ b/src/algorithms/ExplorationTypes.java @@ -0,0 +1,30 @@ +package algorithms; + +public enum ExplorationTypes{ + EMPTY(0), OBSTACLE(1),UNEXPLORED_EMPTY(2),UNEXPLORED_OBSTACLE(3); + + int type_val = 2; + public int getValue() + { + return this.type_val; + } + + public static int toInt(String a) { + switch(a) { + case "EMPTY": + return 0; + case "OBSTACLE": + return 1; + case "UNEXPLORED_EMPTY": + return 2; + case "UNEXPLORED_OBSTACLE": + return 3; + } + return -1; + }; + // enum constructor - cannot be public or protected + private ExplorationTypes(int type_val) + { + this.type_val = type_val; + } +}; \ No newline at end of file diff --git a/src/algorithms/Main.java b/src/algorithms/Main.java new file mode 100644 index 0000000..554b405 --- /dev/null +++ b/src/algorithms/Main.java @@ -0,0 +1,537 @@ +package algorithms; + +import java.time.Duration; +import java.time.Instant; +import java.util.LinkedList; +import java.util.Queue; +import java.util.Stack; +import java.util.Scanner; + +import javax.swing.JFrame; + + +enum State{ + IDLE, + WAITINGFORCOMMAND, + EXPLORATION, + FASTESTPATHHOME, + FASTESTPATH, + DONE, + RESETFASTESTPATHHOME, + SENDINGMAPDESCRIPTOR, + +} + +enum OperatingSystem{ + Windows, + Linux +} + +public class Main { +// JLabel stepsLabel = new JLabel("No. of Steps to Calibration"); +// JTextField calibrate = new JTextField(""); +// JButton update = new JButton("update"); + + + public static void main(String[] args){ + //instantiate objects and variables + String OS = System.getProperty("os.name").toLowerCase(); + + OperatingSystem theOS = OperatingSystem.Windows; + int wayx = 1; + int wayy = 1; + + if(OS.indexOf("win") >= 0) + theOS = OperatingSystem.Windows; + else if((OS.indexOf("nix") >= 0 || OS.indexOf("nux") >= 0 || OS.indexOf("aix") > 0 )) + theOS = OperatingSystem.Linux; + + State currentState; + JFrame frame = null; + + if(theOS == OperatingSystem.Windows) + { + frame= new JFrame("MDP Simulator"); + frame.setSize(600, 820); + } + Instant starts = null; + Instant end = null; + Map map = new Map(); + + //////////////////////IMPORTANT VARIABLE/////////////////////////////////////////////////////////////////////// + boolean simulator = true; + //////////////////////IMPORTANT VARIABLE/////////////////////////////////////////////////////////////////////// + + if(simulator) { + int[][] test= new int[][] + { + {0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0}, + {0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0}, + {0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0}, + {0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0} + }; + //for debugging purposes + MapIterator.printExploredResultsToFile(test, "C://Users//PIZZA 3.0//Desktop//test.txt"); + MapIterator.ArraytoHex((test)); + map.setMapArray(test); + } + RobotInterface theRobot; + Visualization viz = new Visualization(); + currentState = State.WAITINGFORCOMMAND; + PacketFactory pf = null; + Queue recvPackets = null; + Astar as = null; + Node waypoint = null; + + //the simulator requires the rendering frame to be activated + if(simulator) { + //the class and initialisation for the simulated robot + theRobot = new Robot(1,18, Direction.RIGHT, map); + //3 front, 2 right, 1(Long range) left + Sensor s1 = new Sensor(3,SensorLocation.FACING_RIGHT, 1, 1, theRobot.x, theRobot.y); + Sensor s2 = new Sensor(3,SensorLocation.FACING_RIGHT, 1, 0, theRobot.x, theRobot.y); + Sensor s3 = new Sensor(3,SensorLocation.FACING_DOWN, 1, 0, theRobot.x, theRobot.y); + Sensor s4 = new Sensor(3,SensorLocation.FACING_RIGHT, 1, -1, theRobot.x, theRobot.y); + Sensor s5 = new Sensor(3,SensorLocation.FACING_DOWN, -1, 0, theRobot.x, theRobot.y); + Sensor s6 = new Sensor(5,SensorLocation.FACING_TOP, 1, -1, theRobot.x, theRobot.y); + + + Sensor[] Sensors = {s1,s2,s3,s4,s5,s6}; + theRobot.addSensors(Sensors); + + viz.setRobot(theRobot); + theRobot.setViz(viz); + theRobot.setSpeed(10f); + + if(theOS == OperatingSystem.Windows) + { + frame.getContentPane().add(viz); + frame.setVisible(true); + frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE); + frame.setResizable(true); + } + } + else + { + //initialize real robot, communications, and sensors + recvPackets = new LinkedList(); + pf = new PacketFactory(recvPackets); + theRobot = new RealRobot(1,18, Direction.RIGHT, map, pf); + + RealSensor s1 = new RealSensor(4,SensorLocation.FACING_RIGHT, 1, 1, theRobot.x, theRobot.y); + RealSensor s2 = new RealSensor(4,SensorLocation.FACING_RIGHT, 1, 0, theRobot.x, theRobot.y); + RealSensor s3 = new RealSensor(4,SensorLocation.FACING_DOWN, 1, 1, theRobot.x, theRobot.y); + RealSensor s4 = new RealSensor(4,SensorLocation.FACING_RIGHT, 1, -1, theRobot.x, theRobot.y); + RealSensor s5 = new RealSensor(4,SensorLocation.FACING_DOWN, -1, 1, theRobot.x, theRobot.y); + RealSensor s6 = new RealSensor(5,SensorLocation.FACING_TOP, 1, -1, theRobot.x, theRobot.y); + + RealSensor[] Sensors = {s1,s2,s3,s4,s5,s6}; + theRobot.addSensors(Sensors); + viz.setRobot(theRobot); + theRobot.setViz(viz); + + if(theOS == OperatingSystem.Windows) + { + frame.getContentPane().add(viz); + frame.setVisible(true); + frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE); + frame.setResizable(true); + System.out.print("Waiting for command"); + currentState = State.WAITINGFORCOMMAND; + } + + + } + + //init the algo classes + exploration1 exe = new exploration1(null, simulator, theRobot, viz, map); + exe.initStartPoint(1,18); + while(currentState != State.DONE) + { + switch(currentState){ + + case IDLE: + break; + + case WAITINGFORCOMMAND: + System.out.println("\n------------------------------WaitingForCommand Case------------------------------\n"); + //terminal UI for simulator + if(simulator) { + Scanner sc = new Scanner(System.in); + System.out.println("Please enter state:"); + System.out.println("1) Set Waypoint"); + System.out.println("2) Set robot position"); + System.out.println("3) Start Exploration"); + System.out.println("4) Start Fastest Path"); + System.out.println("5) Stop Instruction"); + System.out.println("6) Reset Instruction"); + System.out.println("7) Get Map Descriptor"); + System.out.println("8) Set speed for fastest path, default = 10f"); + int scanType = sc.nextInt(); + +// sc.close(); + if(scanType == 1) { + System.out.println("Please enter x coordinate: "); + wayx = sc.nextInt(); + System.out.println("Please enter y coordinate: "); + wayy = sc.nextInt(); + //set robot waypoint + System.out.println("setting waypoint position at :" + wayx+ ", " + wayy); + waypoint = new Node(wayx, wayy); + map.setWaypointClear(wayx, wayy); + } + else if(scanType == 2) { + //set robot robot position + System.out.println("Please enter x coordinate: "); + int getx = sc.nextInt(); + System.out.println("Please enter y coordinate: "); + int gety = sc.nextInt(); + //set robot waypoint + System.out.println("Moving robot to:" + getx+ ", " + gety); + theRobot.setRobotPos(getx, gety, Direction.RIGHT); + } + else if(scanType == 3) { + starts = Instant.now(); + currentState = State.EXPLORATION; + } + else if(scanType == 4) { + starts = Instant.now(); + currentState = State.FASTESTPATH; + } + else if(scanType == 5) { + // currentState = State.FASTESTPATHHOME; + } + else if(scanType == 6) { + // currentState = State.RESETFASTESTPATHHOME; + System.out.println("Reseting Map..."); + map.resetMap(); + theRobot.setface(Direction.RIGHT); + theRobot.x = 1; + theRobot.y = 18; + map.resetMap(); + viz.repaint(); + } + else if (scanType == 7) + theRobot.sendMapDescriptor(); + else if (scanType == 8) { + System.out.println("Please input intended speed for fastest path\n(1000/stepsPerSecond): "); + float stepsPerSecond = sc.nextInt(); + theRobot.setSpeed(stepsPerSecond); + } + break; + } + else{ + //real robot begin listening to commands from rpi + System.out.print("\nListening\n"); + //pf.sc.sendPacket("Donald Trump!"); + pf.listen(); + if(recvPackets.isEmpty()) + continue; + System.out.println("recvPackets is not empty"); + Packet pkt = recvPackets.remove(); + System.out.println(pkt.getType()); + if(pkt.getType() == Packet.SetWayPointi) { + wayx = pkt.getX(); + wayy = pkt.getY(); + //set robot waypoint + System.out.println("setting waypoint position at :" + wayx+ ", " + wayy); + waypoint = new Node(wayx, wayy); + map.setWaypointClear(wayx, wayy); + currentState = State.WAITINGFORCOMMAND; + } + else if(pkt.getType() == Packet.setRobotPosition) { + //set robot robot position + System.out.println("-----------------Setting robot position--------------"); + theRobot.setRobotPos(pkt.getX(), pkt.getY(), pkt.getDirection()); + } + else if(pkt.getType() == Packet.StartExploration) { + starts = Instant.now(); + currentState = State.EXPLORATION; + } + else if(pkt.getType() == Packet.StartFastestPath) { + starts = Instant.now(); + currentState = State.FASTESTPATH; + } + else if(pkt.getType() == Packet.StopInstruction) { + currentState = State.FASTESTPATHHOME; + } + else if(pkt.getType() == Packet.ResetInstruction) { + currentState = State.RESETFASTESTPATHHOME; + System.out.println("Resetting Map..."); + map.resetMap(); + theRobot.setface(Direction.RIGHT); + theRobot.x = 1; + theRobot.y = 18; + map.resetMap(); + viz.repaint(); + } + else if (pkt.getType() == Packet.GETMAPi) + theRobot.sendMapDescriptor(); + else { + System.out.println("Invalid Packet!!"); + continue; + } + break; + } + case EXPLORATION: + //init an explore algo class and call StartExploration() + System.out.println("---------------------------------Exploration case---------------------------------\n"); + if (!simulator) + theRobot.LookAtSurroundings(); + int DoSimulatorExplorationResult = exe.DoSimulatorExploration(); + + if(simulator) + { + //will return true once the exploration is done(when the robot reaches the starting point again) + if(DoSimulatorExplorationResult == 1) + { + Scanner sc = new Scanner(System.in); + theRobot.deactivateSensors(); + System.out.println("Go to fastest path? \n 1=yes \n 2=no"); + int choice = sc.nextInt(); +// sc.close(); + if(choice == 1) + currentState = State.FASTESTPATH; + else + currentState = State.WAITINGFORCOMMAND; + System.out.println("ending Exploration..."); + }//else + // currentState = State.WAITINGFORCOMMAND; + } + else + { +// theRobot.LookAtSurroundings(); + //pf.sc.sendPacket(Packet.INITIALCALIBRATE); + //will return true once the exploration is done(when the robot reaches the starting point again) + if(DoSimulatorExplorationResult == 1) + { + //send the packet to say that exploration is done + System.out.println("ending Exploration..."); + //PathDrawer.removePath(); + //theRobot.sendMapDescriptor(); + end = Instant.now(); + System.out.println("Time: " + Duration.between(starts, end)); + + ((RealRobot) theRobot).sendMapDescriptorRpi(); + + PacketFactory.sc.sendPacket(Packet.StartExplorationTypeFin + "$"); + + // Send map descriptor + System.out.println("------------------------------Sending this useless descriptor------------------------------\n"); + System.out.println("doing map descriptor"); + MapIterator.printExploredResultsToFile(map.getMapArray(), "theExplored.txt"); + MapIterator.printExploredResultsToHex("ExplorationHex.txt"); + MapIterator.printObstacleResultsToFile(map.getMapArray(), "theObstacle.txt"); + MapIterator.printObstacleResultsToHex("ObstacleHex.txt"); + pf.sendCMD("B:stat:Exploration mdf:" + MapIterator.mapDescriptorP1Hex + "$"); + pf.sendCMD("B:stat:Obstacle mdf:" + MapIterator.mapDescriptorP2Hex + "$"); + pf.sendCMD("B:stat:finish_exe_mdf$"); + currentState = State.WAITINGFORCOMMAND; + + try { + Thread.sleep(10000); + } catch (InterruptedException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + theRobot.initial_Calibrate(); + pf.setFlag(false); + + } else if (DoSimulatorExplorationResult == -1) { + System.out.println("JARRETT: Robot wants to reset prematurely. Resetting exe and robot..."); + System.out.println("JARRETT: PLEASE BRING ROBOT BACK TO 1,18 FACING LEFT, THEN SEND IC COMMAND, THEN START EXPLORE (IT SHOULD BE RIGHT FACING AFTER IC)!"); + +// viz = new Visualization(); + currentState = State.WAITINGFORCOMMAND; +// pf = null; +// recvPackets = null; + as = null; + waypoint = null; + +// RobotInterface theRobot; + theRobot = new RealRobot(1,18, Direction.RIGHT, map, pf); + + RealSensor s1 = new RealSensor(4,SensorLocation.FACING_RIGHT, 1, 1, theRobot.x, theRobot.y); + RealSensor s2 = new RealSensor(4,SensorLocation.FACING_RIGHT, 1, 0, theRobot.x, theRobot.y); + RealSensor s3 = new RealSensor(4,SensorLocation.FACING_DOWN, 1, 1, theRobot.x, theRobot.y); + RealSensor s4 = new RealSensor(4,SensorLocation.FACING_RIGHT, 1, -1, theRobot.x, theRobot.y); + RealSensor s5 = new RealSensor(4,SensorLocation.FACING_DOWN, -1, 1, theRobot.x, theRobot.y); + RealSensor s6 = new RealSensor(5,SensorLocation.FACING_TOP, 1, -1, theRobot.x, theRobot.y); + + RealSensor[] Sensors = {s1,s2,s3,s4,s5,s6}; + theRobot.addSensors(Sensors); + viz.setRobot(theRobot); + theRobot.setViz(viz); + + map.resetMap(); +// theRobot.setface(Direction.RIGHT); +// theRobot.x = 1; +// theRobot.y = 18; + // REINTIALIZE +// map = new Map(); // TODO: JARRETT FIX MAP + exe = new exploration1(null, simulator, theRobot, viz, map); + exe.initStartPoint(1,18); + } + } + if(DoSimulatorExplorationResult != 1) + currentState = State.WAITINGFORCOMMAND; + break; + case FASTESTPATHHOME: + //update the map nodes, then create a new astar path + map.updateMap(); + //Astar as1 = new Astar(map.getNodeXY(theRobot.x, theRobot.y), map.getNodeXY(5, 10)); + Astar as1 = new Astar(map.getNodeXY(theRobot.x, theRobot.y), map.getNodeXY(1, 18)); + + //send it to the robot to handle the instruction + theRobot.getFastestInstruction(as1.getFastestPath()); + System.out.print("finished fastest path home"); + + if(simulator) + currentState = State.FASTESTPATH; + else + currentState = State.WAITINGFORCOMMAND; + + break; + case RESETFASTESTPATHHOME: + //update the map nodes, then create a new astar path + map.updateMap(); + Astar as3 = new Astar(map.getNodeXY(theRobot.x, theRobot.y), map.getNodeXY(1, 18)); + + //send it to the robot to handle the instruction + theRobot.getFastestInstruction(as3.getFastestPath()); + System.out.print("finished fastest path home.. resetting map..."); + map.resetMap(); + theRobot.x = 1; + theRobot.y = 18; + //currentState = State.FASTESTPATH; + currentState = State.WAITINGFORCOMMAND; + + break; + case FASTESTPATH: + //init fastest path from startNode to goalNode + System.out.println("-------------------------------------FastestPath case-----------------------------------\n"); + if(simulator) + { + theRobot.initial_Calibrate(); + //update the map nodes, then create a new astar path + map.updateMap(); + waypoint = map.getNodeXY(wayx, wayy); + Astar as31 = new Astar(map.getNodeXY(theRobot.x, theRobot.y),waypoint); + Astar as2 = new Astar(waypoint, map.getNodeXY(13, 1)); + Stack as31GFP = as31.getFastestPath(); + if(as31GFP.isEmpty()){ + Astar as4 = new Astar(map.getNodeXY(theRobot.x, theRobot.y),map.getNodeXY(13,1)); + PathDrawer.update(theRobot.x, theRobot.y, as4.getFastestPath()); + theRobot.getFastestInstruction(as4.getFastestPath()); + PathDrawer.removePath(); + } + else { + PathDrawer.update(theRobot.x, theRobot.y, as31GFP); + theRobot.getFastestInstruction(as31.getFastestPath()); + PathDrawer.update(theRobot.x, theRobot.y, as2.getFastestPath()); + theRobot.getFastestInstruction(as2.getFastestPath()); + PathDrawer.removePath(); + //send it to the robot to handle the instruction + } + currentState = State.SENDINGMAPDESCRIPTOR; + System.out.print("finished fastest path TO GOAL"); + + } + else + { + //update the map nodes, then create a new astar path + //testing empty map + //set empty + + pf.sendCMD(Packet.StartFastestPathTypeOkANDROID + "$"); // B // TODO: @JARRETT see if necessary or not + pf.sendCMD(Packet.StartFastestPathTypeOkARDURINO + "$"); // A // TODO: @JARRETT see if necessary or not + //NOTE + map.updateMap(); + + Stack stack = null; + if(waypoint == null) { + System.out.println("NO waypoint."); + as = new Astar(map.getNodeXY(theRobot.x, theRobot.y), map.getNodeXY(13, 1)); + stack = as.getFastestPath(); + theRobot.getFastestInstruction(stack); + + } + else { + int x1 = waypoint.getX(); + int y1 = waypoint.getY(); + System.out.println("going to fastest path with waypoint of " + x1 + "," + y1); + waypoint = map.getNodeXY(x1, y1); + as = new Astar(map.getNodeXY(theRobot.x, theRobot.y), waypoint); + Astar as2 = new Astar(waypoint, map.getNodeXY(13, 1)); + stack = as2.getFastestPath(); + Stack stack2 = as.getFastestPath(); + + if(!stack.isEmpty() && !stack2.isEmpty()) { + System.out.println("going to waypoint..."); + stack.addAll(stack2); + theRobot.getFastestInstruction(stack); + + } + else { + System.out.println("failed to go to waypoint"); + System.out.println("going to goal without waypoint"); + as = new Astar(map.getNodeXY(theRobot.x, theRobot.y), map.getNodeXY(13, 1)); + stack = as.getFastestPath(); + theRobot.getFastestInstruction(stack); + } + } + + + //create the int[] frm the stack + //send the whole entire packet to rpi + viz.repaint(); + end = Instant.now(); + System.out.println("Time : " +Duration.between(starts, end)); +// currentState = State.SENDINGMAPDESCRIPTOR; + currentState = State.WAITINGFORCOMMAND; // TODO: @JARRETT REMOVED COS OF STUPID BUG WHICH ROBOT DIES BEFORE REACHING END GOAL + + } + break; + + case SENDINGMAPDESCRIPTOR: + System.out.println("------------------------------Sending this useless descriptor------------------------------\n"); + System.out.println("doing map descriptor"); + + + MapIterator.printExploredResultsToFile(map.getMapArray(), "theExplored.txt"); + MapIterator.printExploredResultsToHex("ExplorationHex.txt"); + + MapIterator.printObstacleResultsToFile(map.getMapArray(), "theObstacle.txt"); + MapIterator.printObstacleResultsToHex("ObstacleHex.txt"); + if(!simulator) { + pf.sendCMD("B:stat:Exploration mdf:" + MapIterator.mapDescriptorP1Hex + "$"); + pf.sendCMD("B:stat:Obstacle mdf:" + MapIterator.mapDescriptorP2Hex + "$"); + + pf.sendCMD("B:stat:finish_exe_mdf$"); + } + currentState = State.WAITINGFORCOMMAND; + default: + break; + } + } + } + + + SocketClient cs = new SocketClient("192.168.4.4", 8081); +} \ No newline at end of file diff --git a/src/algorithms/Map.java b/src/algorithms/Map.java new file mode 100644 index 0000000..1f69cf6 --- /dev/null +++ b/src/algorithms/Map.java @@ -0,0 +1,474 @@ +package algorithms; + +import javax.swing.*; +import javax.swing.Timer; +import java.awt.*; +import java.awt.event.ActionEvent; +import java.awt.event.ActionListener; +import java.util.*; + + +public class Map{ + int[][] SimulatedmapArray; + int[][] mapArray; + int[][] mapScoreArray; + int[][] mapArray2; + + boolean turnoffgrid=false; + boolean turnoffgrid2=false; + boolean turnoffgrid3=false; + + int gridExplored= 0; + double exploredPercentage; + final static int WIDTH = 15; + final static int HEIGHT = 20; + final static int sizeofsquare = 38; + public static Node[][] NodeArray = new Node[HEIGHT][WIDTH]; + + public Map(){ + gridExplored = 0; + this.mapArray = new int[][]{ + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2} + }; + this.mapArray2 = new int[][]{ + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2} + }; + setScoreArray(); + } + + + public Map(int[][] mapDisplay){ + //this is not used + + mapArray = mapDisplay; + initializeNodes(); + initializeNeighbors(); + setScoreArray(); + +} + + public void setScoreArray() { + mapScoreArray = new int[HEIGHT][WIDTH]; + + for(int y = 0 ; y < mapScoreArray.length; y++){ + for(int x = 0; x < mapScoreArray[y].length;x++){ + mapScoreArray[y][x] = 0; + + } + } + int confirmed = -1000; + mapScoreArray[17][0] = confirmed; + mapScoreArray[19][0] = confirmed; + mapScoreArray[18][0] = confirmed; + mapScoreArray[17][1] = confirmed; + mapScoreArray[19][1] = confirmed; + mapScoreArray[18][1] = confirmed; + mapScoreArray[17][2] = confirmed; + mapScoreArray[19][2] = confirmed; + mapScoreArray[18][2] = confirmed; + + mapScoreArray[0][12] = confirmed; + mapScoreArray[0][13] = confirmed; + mapScoreArray[0][14] = confirmed; + mapScoreArray[1][12] = confirmed; + mapScoreArray[1][13] = confirmed; + mapScoreArray[1][14] = confirmed; + mapScoreArray[2][12] = confirmed; + mapScoreArray[2][13] = confirmed; + mapScoreArray[2][14] = confirmed; + SimulatedmapArray = new int[HEIGHT][WIDTH]; + + + } + + public int[][] getMapArray() { + return mapArray; +} + + +public void setMapArray(int[][] mapArray) { + this.mapArray = mapArray; + //this.mapArray2 = mapArray; + //SimulatedmapArray = mapArray.clone(); + initializeNodes(); + initializeNeighbors(); + + for(int y = 0 ; y < SimulatedmapArray.length; y++){ + for(int x = 0; x < SimulatedmapArray[y].length;x++){ + SimulatedmapArray[y][x] = mapArray[y][x]; + //System.out.print(SimulatedmapArray[y][x]); + } + //System.out.print("\n"); + } +} +public void setMapScore(int x, int y, int score) +{ + //System.out.print(" X = " + x + " Y + " + y + " \n"); + mapScoreArray[y][x] += score; + + + +} + +//update probabilities on map display for simulation and real robot. +public void updateMapWithScore() +{ + for(int y = 0 ; y < mapScoreArray.length; y++){ + for(int x = 0; x < mapScoreArray[y].length;x++){ + if(mapScoreArray[y][x] == 0) { + mapArray[y][x] = ExplorationTypes.toInt("UNEXPLORED_EMPTY"); + //mapArray2[y][x] = ExplorationTypes.toInt("UNEXPLORED_EMPTY"); + } + else if(mapScoreArray[y][x] > 0) { + mapArray[y][x] = ExplorationTypes.toInt("OBSTACLE"); + //mapArray2[y][x] = ExplorationTypes.toInt("OBSTACLE"); + } + else if(mapScoreArray[y][x] < 0) { + mapArray[y][x] = ExplorationTypes.toInt("EMPTY"); + //mapArray2[y][x] = ExplorationTypes.toInt("EMPTY"); + } + ////////////////////////////////////////////////////////////////////might need changing///////////////////////////////////////////////////////// + /*if(mapScoreArray[y][x] == -1) { + //mapArray[y][x] = ExplorationTypes.toInt("UNEXPLORED_EMPTY"); + mapArray2[y][x] = ExplorationTypes.toInt("UNEXPLORED_EMPTY"); + }*/ + } + } + +} + +public void optimiseFP(){ + for(int i=0;i 0) { //move left + Node left = NodeArray[r][c-1]; + NodeArray[r][c].addNeighbors(left); + NodeArray[r][c].setLeft(left); + } + if (c < WIDTH - 1) { //move right + Node right = NodeArray[r][c + 1]; + NodeArray[r][c].addNeighbors(right); + NodeArray[r][c].setRight(right); + + } + if (r < HEIGHT-1) { // down + Node down = NodeArray[r + 1][c]; + NodeArray[r][c].addNeighbors(down); + NodeArray[r][c].setDown(down); + } + if (r > 0) { // up + Node up = NodeArray[r - 1][c]; + NodeArray[r][c].addNeighbors(up); + NodeArray[r][c].setUp(up); + } + } + } +} + + + public void calculateClearance() { + Node node; + for (int r = 0; r < HEIGHT; r++) { + columnloop: + for (int c = 0; c < WIDTH; c++) { + node = NodeArray[r][c]; + node.setClearance(0); + + if (node.isObstacle()) { + node.setClearance(0); + continue; + } + + + for (int i = -1; i < 2; i++) { + for (int j = -1; j < 2; j++) { + if (r + i >= HEIGHT || r + i < 0 || c + j >= WIDTH || c + j < 0) { + continue columnloop; + } + if(NodeArray[r + i][c + j].isObstacle()) + continue columnloop; + + + } + } + + + + node.setClearance(3); + + } + + } +} + + +public void MapUpdate(int x, int y, int flag) { +//for receiving of Data + if(x > 0 || y > 0 || x < WIDTH || y < HEIGHT) { + mapArray[y][x] = flag; + } +} + + public void calculatePercentageExplored(){ + for(int y = 0 ; y < mapArray.length; y++){ + for(int x = 0; x < mapArray[y].length;x++){ + if(mapArray[y][x] != 2 || mapArray[y][x] != ExplorationTypes.toInt("EMPTY")){ + gridExplored++; + } + } + } + exploredPercentage = gridExplored/WIDTH*HEIGHT; + } + + public void paintGrid(Graphics g){ + if(mapArray == null) + paintMapGridEmpty(g); + else{ + paintMap(g); + } + } + public void setWaypointClear(int x, int y) { + int confirmed = -9999; + for(int i = -1; i < 2;i++) { + for(int j = -1; j < 2; j++) { + if(y+i < HEIGHT && x+j < WIDTH && (y+i) >=0 && (x+j) >=0) + mapScoreArray[y+i][x+j] = confirmed; + } + } + + + } + + + public void paintMapGridEmpty(Graphics g){ + //change for actual robot + int distanceY = 0; + int distanceX = 0; + for(int i=0; i < 20;i++){ + //draw Y + distanceX = 0; + g.drawRect(10, 10+distanceY, sizeofsquare, sizeofsquare); + + for(int j=0; j < 15;j++){ + //draw X + g.drawRect(10+distanceX, 10+distanceY, sizeofsquare, sizeofsquare); + distanceX += sizeofsquare; + } + distanceY += sizeofsquare; + } + } + + +public void paintMap(Graphics g){ + + + Graphics2D g2 = (Graphics2D)g; + g2.setRenderingHint(RenderingHints.KEY_ANTIALIASING, + RenderingHints.VALUE_ANTIALIAS_ON); + String mapScore = ""; + int distanceY = 0; + int distanceX = 0; + for(int i=0; i < mapArray.length;i++){ + //draw Y + distanceX = 0; + g.drawRect(10, 10+distanceY, sizeofsquare, sizeofsquare); + + for(int j=0; j < mapArray[i].length;j++){ + //draw X + g.setColor(Color.WHITE); + g.drawRect(10+distanceX, 10+distanceY, sizeofsquare, sizeofsquare); + if(mapArray[i][j]==ExplorationTypes.toInt("OBSTACLE")){ + //paint obstacle + g.setColor(Color.BLACK); + g.fillRect(10+distanceX, 10+distanceY, sizeofsquare, sizeofsquare); + } + else if((i==18 && j ==0)||(i==18 && j ==1)||(i==18 && j ==2)|| + (i==17 && j ==0)||(i==17 && j ==1)||(i==17 && j ==2)|| + (i==19 && j ==0)||(i==19 && j ==1)||(i==19 && j ==2)) + { + //paint start location + g.setColor(Color.BLUE); + g.fillRect(10+distanceX, 10+distanceY, sizeofsquare, sizeofsquare); + } + else if((i==0 && j ==12)||(i==0 && j ==13)||(i==0 && j ==14)|| + (i==1 && j ==12)||(i==1 && j ==13)||(i==1 && j ==14)|| + (i==2 && j ==12)||(i==2 && j ==13)||(i==2 && j ==14)) + { + //paint goal location + g.setColor(Color.GREEN); + g.fillRect(10+distanceX, 10+distanceY, sizeofsquare, sizeofsquare); + } + else if(mapArray[i][j]==ExplorationTypes.toInt("UNEXPLORED_EMPTY")){ + g.setColor(Color.LIGHT_GRAY); + g.fillRect(10+distanceX, 10+distanceY, sizeofsquare, sizeofsquare); + } + g.setColor(Color.BLACK); + g2.drawString(Integer.toString(mapScoreArray[i][j]),20+distanceX,30+distanceY); + distanceX += sizeofsquare; + } + distanceY += sizeofsquare; + } + + + + + + + } + + public void generateMapDescriptorExplored() { + //for generating MapDescriptorExplored. + int[][] ExploredMap = new int[HEIGHT][WIDTH]; + for(int y = 0; y < HEIGHT; y++) { + for(int x = 0; x < WIDTH; x++) { + if(mapArray[y][x] != ExplorationTypes.toInt("UNEXPLORED_EMPTY")||mapArray[y][x] != ExplorationTypes.toInt("UNEXPLORED_OBSTACLE")) { + ExploredMap[y][x] = 1; + } + else + ExploredMap[y][x] = 0; + } + } + MapIterator.printExploredResultsToFile(ExploredMap, "ExploredMapDescriptor.txt"); + } + + public void generateMapDescriptor() { + MapIterator.printExploredResultsToFile(mapArray, "Map.txt"); + } + + public void setEmpty() { + this.mapArray = new int[][]{ + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} + }; + + } + + public void resetMap() { + // TODO Auto-generated method stub + this.mapArray = new int[][]{ + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}, + {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2} + }; + setScoreArray(); + updateMap(); + } + +} \ No newline at end of file diff --git a/src/algorithms/MapIterator.java b/src/algorithms/MapIterator.java new file mode 100644 index 0000000..2459169 --- /dev/null +++ b/src/algorithms/MapIterator.java @@ -0,0 +1,340 @@ +package algorithms; + +import java.io.*; +import java.util.LinkedList; +import java.util.Scanner; + +public class MapIterator { + + final static int WIDTH = 15; + final static int HEIGHT = 20; + + static int[][] mapDescriptorP1; + static int[][] mapDescriptorP2; + + static String mapDescriptorP1Hex; + static String mapDescriptorP2Hex; + + static int knownGrids; + + public MapIterator () { + mapDescriptorP1 = new int[HEIGHT][WIDTH]; + mapDescriptorP2 = new int[HEIGHT][WIDTH]; + + knownGrids = 0; + } + public static void init() + { + mapDescriptorP1 = new int[HEIGHT][WIDTH]; + mapDescriptorP2 = new int[HEIGHT][WIDTH]; + + knownGrids = 0; + } + public static int[][] IterateTextFile(String fileName) { + int[][] fileMap = new int[HEIGHT][WIDTH]; + + File file = new File(fileName); + + BufferedReader br = null; + try { + br = new BufferedReader(new FileReader(file)); + } catch (FileNotFoundException e) { + e.printStackTrace(); + System.out.println("File not found!"); + } + int count = HEIGHT-1; + String st; + try { + while ((st = br.readLine()) != null && count >= 0) + { + for (int i=0; i < st.length(); i++) { + if(Character.getNumericValue(st.charAt(i)) == ExplorationTypes.toInt("EMPTY")) + fileMap[count][i] = ExplorationTypes.toInt("UNEXPLORED_EMPTY"); + else if(Character.getNumericValue(st.charAt(i)) == ExplorationTypes.toInt("OBSTACLE")) + fileMap[count][i] = ExplorationTypes.toInt("UNEXPLORED_OBSTACLE"); + + } + count--; + } + } catch (IOException e) { + e.printStackTrace(); + } + return fileMap; + } + + + public static void printExploredResultsToFile(int[][] results, String fileName) { + + if(mapDescriptorP1 == null) + init(); + + BufferedWriter bw = null; + FileWriter fw = null; + + try { + fw = new FileWriter(fileName); + bw = new BufferedWriter(fw); + StringBuilder sb = new StringBuilder(); + StringBuilder hexSB = new StringBuilder(); + + //count the amount of known grids to adjust the padding of '11's + knownGrids = 0; + + // padding + sb.append("11" +System.getProperty("line.separator")); + hexSB.append("11"); + + //iterate through the 2d map + for (int w = mapDescriptorP1.length-1; w >= 0 ; w--) { + for (int h = 0; h < mapDescriptorP1[0].length ; h ++) { + if(results[w][h] == ExplorationTypes.toInt("EMPTY") || results[w][h] == ExplorationTypes.toInt("OBSTACLE")) + { + sb.append(1); + mapDescriptorP1[w][h] = 1; + hexSB.append(1); + knownGrids++; + } + else + { + sb.append(0); + mapDescriptorP1[w][h] = 0; + hexSB.append(0); + } + } + sb.append(System.getProperty("line.separator")); + } + // padding + sb.append("11"); + hexSB.append("11"); + + + //write the string to the file + bw.write(sb.toString()); + + //save the descriptor as a string in the class + mapDescriptorP1Hex = hexSB.toString(); + } + catch (IOException e) { + System.out.println("Not possible to write!"); + } + + finally { + try { + if (bw != null) + bw.close(); + if (fw != null) + fw.close(); + } catch (IOException ex) { + ex.printStackTrace(); + } + } + } + + + public static void printExploredResultsToHex(String fileName) { + + //initialised to null + BufferedWriter bw = null; + FileWriter fw = null; + + try { + fw = new FileWriter(fileName); + bw = new BufferedWriter(fw); + + //convert the string of 1s and 0s to hex, then write it to the filename provided + mapDescriptorP1Hex = formatStringToHexadecimal(mapDescriptorP1Hex); + System.out.println(mapDescriptorP1Hex); + bw.write(mapDescriptorP1Hex); + } + catch (IOException e) { + System.out.println("Not possible to write!"); + } + + finally { + try { + if (bw != null) + bw.close(); + if (fw != null) + fw.close(); + } catch (IOException ex) { + ex.printStackTrace(); + } + } + } + + + public static void printObstacleResultsToFile(int[][] results, String fileName) { + BufferedWriter bw = null; + FileWriter fw = null; + + + //count the number of known grids, will return 0 if divisible by 4 + int numKnownGrids = knownGrids % 4; + if(numKnownGrids != 0) + numKnownGrids += 1; + + try { + fw = new FileWriter(fileName); + bw = new BufferedWriter(fw); + //bw.write(formatStringToHexadecimal(results)); + StringBuilder sb = new StringBuilder(); + StringBuilder hexSB = new StringBuilder(); + + System.out.print("MapDescriptorP1.length: "+mapDescriptorP1.length); //20 + System.out.print("mapDescriptorP1[0].length: "+mapDescriptorP1[0].length); //15 + System.out.println("\n"); + + + for (int w = mapDescriptorP1.length-1; w >= 0 ; w--) { + for (int h = 0; h < mapDescriptorP1[0].length ; h ++) { + + //if its explored, then input the information for it + if(mapDescriptorP1[w][h] == 1) + { + if(results[w][h] == ExplorationTypes.toInt("EMPTY")) + { + sb.append(0); + hexSB.append(0); + } + else if(results[w][h] == ExplorationTypes.toInt("OBSTACLE")) + { + sb.append(1); + hexSB.append(1); + } + + //if(w==0 && h==mapDescriptorP1[0].length-1) { sb.append(0); sb.append(0); hexSB.append(0); hexSB.append(0); } + + } + + + } + sb.append(System.getProperty("line.separator")); + } + + bw.write(sb.toString()); + + //save the descriptor as a string in the class + mapDescriptorP2Hex = hexSB.toString(); + } + catch (IOException e) { + System.out.println("Not possible to write!"); + } + + finally { + try { + if (bw != null) + bw.close(); + if (fw != null) + fw.close(); + } catch (IOException ex) { + ex.printStackTrace(); + } + } + } + + public static void printObstacleResultsToHex(String fileName) { + BufferedWriter bw = null; + FileWriter fw = null; + + try { + fw = new FileWriter(fileName); + bw = new BufferedWriter(fw); + + //convert the string of 1s and 0s to hex, then write it to the filename provided + mapDescriptorP2Hex = formatStringToHexadecimal(mapDescriptorP2Hex); + System.out.println(mapDescriptorP2Hex); + bw.write(mapDescriptorP2Hex); + } + catch (IOException e) { + System.out.println("Not possible to write!"); + } + + finally { + try { + if (bw != null) + bw.close(); + if (fw != null) + fw.close(); + } catch (IOException ex) { + ex.printStackTrace(); + } + } + } + public static String ArraytoString(int[][] intresults) { + StringBuilder sb = new StringBuilder(); + sb.append(""); + sb.append(intresults); + return sb.toString(); + } + + + public static void ArraytoHex(int[][] intresults) { + StringBuilder sb = new StringBuilder(); + sb.append("11"); + for (int h = 0; h < intresults[0].length; h++) { + for (int w = 0; w < intresults.length; w ++) { + sb.append(intresults[w][h]); + } + } + sb.append("11"); + String hexi = formatStringToHexadecimal(sb.toString()); + System.out.println(hexi); + } + + public static String formatStringToHexadecimal(String string) { + int start = 0; + int end = 7; + String sub; + int decimal; + String hexStr = ""; + + StringBuilder stringBuilder = new StringBuilder(); + try { + while (start != string.length()) { + sub = string.substring(start, end+1); + decimal = Integer.parseInt(sub, 2); + + if (decimal < 16) + stringBuilder.append("0"); + + hexStr = Integer.toString(decimal,16); + stringBuilder.append(hexStr); + start += 8; + end += 8; + } + } + catch (Exception e) { + System.out.println("Doing exception for string to hex"); + StringBuilder sb = new StringBuilder(); + + int length = string.length() - start; + // if left with 0 character -> length = -1 ; start = length + // if left 1 character -> length = 0 ; start = length + 1 + // + int count = 8; + while (length > 0) { + sb.append(string.charAt(start)); + start += 1; + length -= 1; + count -= 1; + } + + while (count > 0) { + sb.append("0"); + count -= 1; + } + + decimal = Integer.parseInt(sb.toString(), 2); + if (decimal < 16) { + // if it is less than 16, the hexadecimal produced will only be one digit instead of 2 + stringBuilder.append("0"); + } + hexStr = Integer.toString(decimal,16); + stringBuilder.append(hexStr); + + } + return stringBuilder.toString(); + } + + +} \ No newline at end of file diff --git a/src/algorithms/Packet.java b/src/algorithms/Packet.java new file mode 100644 index 0000000..813ac76 --- /dev/null +++ b/src/algorithms/Packet.java @@ -0,0 +1,214 @@ +package algorithms; + +public class Packet{ + //for mapping amount of instructions together + + final static char ARDUINO = 'A'; + final static char ANDROID = 'B'; + final static char PC = 'P'; + final static char RPI = 'R'; + + + + //packet types + final static int StartExploration = 1; + final static int StartFastestPath = 2; + final static int StopInstruction = 3; + final static int ResetInstruction = 4; + final static int SetWayPointi = 5; + final static int setRobotPosition = 6; + final static int setObstacle = 7; + final static int takePhoto = 8; + + + //instruction to map to packet Instruction. + final static int FORWARDi = 1; + final static int TURNRIGHTi = 2; + final static int TURNLEFTi = 3; + final static int REVERSEi = 4; + public static final int CALIBRATEi = 5; + final static int PHOTOi = 6; + + + + //for movement of the robot. + final static String FORWARD = "forward"; + final static String TURNRIGHT = "right"; + final static String TURNLEFT = "left"; + final static String REVERSE = "reverse"; + + //start exploration from android + final static String Ok = "ok"; + final static String Cmd = "cmd"; + final static String Set = "set"; + final static String Splitter = ":"; + final static String Stat = "stat"; + + + final static String StartExplorationType = "explore"; + final static String StartExplorationTypeOkARD = "A:ok:start_explore"; + final static String StartExplorationTypeOk = "B:ok:start_explore"; + final static String StartExplorationTypeFin = "B:ok:finish_explore"; //after going back to start location + + //start fastestpath from android + final static String StartFastestPathType = "path";//with waypoint from android + final static String StartFastestPathTypeOk = "start_path"; //send to android + final static String StartFastestPathTypeFin = "finish_path"; //send to android + final static String startExplore = ANDROID + Splitter + Ok + Splitter + StartExplorationTypeOk; + + //Send take photo command to rpi + final static String PhotoPacket = "cam"; //sends the command to take pictures along with x,y co-ordinates and direction +/* final static String sendPhotoCmd = RPI + Splitter + Ok + PhotoPacket + photoData*/ + + + //stop the robot from android + final static String Stop = "stop";//send from android + final static String StopOk = "B:ok:stop";//send to android + + final static String Reset = "reset"; //from android + final static String ResetOK = "B:ok:reset";//send to android + + //need to process this string to become x y coordinate of robot in map + final static String SetRobotPos = "startposition"; + final static String SetRobotPosOk = "B:ok:startposition"; + final static String SetWayPoint = "waypoint"; + final static String SetWayPointOK = "ok:waypoint"; + + //for map obstacle + final static String Map = "map"; + final static String Block = "block"; + + final static String TURNLEFTCMD = ARDUINO + Splitter + Cmd + Splitter + TURNLEFT; + final static String TURNRIGHTCMD =ARDUINO + Splitter + Cmd + Splitter + TURNRIGHT; + final static String FORWARDCMD = ARDUINO + Splitter + Cmd + Splitter + FORWARD; + final static String REVERSECMD = ARDUINO + Splitter + Cmd + Splitter + REVERSE; + + final static String TURNLEFTCMDANDROID = ANDROID + Splitter + Stat + Splitter + TURNLEFT; + final static String TURNRIGHTCMDANDROID =ANDROID + Splitter + Stat + Splitter + TURNRIGHT; + final static String FORWARDCMDANDROID = ANDROID + Splitter + Stat + Splitter + FORWARD; + final static String REVERSECMDANDROID = ANDROID + Splitter + Stat + Splitter + REVERSE; + + final static String SIDECALIBRATE = "A:cmd:sc"; // TODO: @Jarrett removed $ + final static String LEFTCALIBRATE = "A:cmd:lsc"; + final static String FRONTCALIBRATE = "A:cmd:fc"; + final static String INITIALCALIBRATE = "A:cmd:ic"; + final static String SIDETURNCALIBRATE = "A:cmd:frontc"; + + final static String ACKKNOWLEDGE = "P:cmd:ack"; + + final static String MAPDESCRIPTORCMD = "B:map:set:"; + final static String MAPDESCRIPTORCMDRPI = "D:map:set:"; + + final static int GETMAPi = 10; + + final static String GETMAP ="getmap"; + + + final static String StartFastestPathTypeOkANDROID = "A:ok:start_path"; //send to android + final static String StartFastestPathTypeOkARDURINO = "B:ok:start_path"; //send to android + + + + + + int type = 0; + int x = 0; + int y = 0; + Direction Direction = null ; + int[] SensorData = null; + + + public Packet(int type) { + //for packet with only type + this.type = type; + } + + + public Packet(int type, int x, int y) { + //for waypoint packets + this.type = type; + this.x = x; + this.y = y; + } + + + public Packet(int type, int x, int y, Direction direction) { + //for robot position with direction + this.type = type; + this.x = x; + this.y = y; + Direction = direction; + } + + + + + public Packet(int type, int x, int y, Direction direction, int[] sensorData) { + //for map obstacle data + super(); + this.type = type; + this.x = x; + this.y = y; + Direction = direction; + SensorData = sensorData; + } + + + public Packet(int setobstacle2, int[] data) { + // TODO Auto-generated constructor stub + this.type = setobstacle2; + this.SensorData = data; + } + + + public int[] getSensorData() { + return SensorData; + } + + + public void setSensorData(int[] sensorData) { + SensorData = sensorData; + } + + + public int getType() { + return type; + } + + + public void setType(int type) { + this.type = type; + } + + + public int getX() { + return x; + } + + + public void setX(int x) { + this.x = x; + } + + + public int getY() { + return y; + } + + + public void setY(int y) { + this.y = y; + } + + + public Direction getDirection() { + return Direction; + } + + + public void setDirection(Direction direction) { + Direction = direction; + } + + +} \ No newline at end of file diff --git a/src/algorithms/PacketFactory.java b/src/algorithms/PacketFactory.java new file mode 100644 index 0000000..4cc9248 --- /dev/null +++ b/src/algorithms/PacketFactory.java @@ -0,0 +1,476 @@ +package algorithms; + +import java.io.IOException; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.LinkedList; +import java.util.Queue; +import java.util.Stack; + +import javax.swing.text.Position; + + +public class PacketFactory implements Runnable{ + + static SocketClient sc = null; + int delay = 0; + int whatever = 0; + int waypoint_x = 0, waypoint_y = 0; + boolean camRun = true; + //multi-threading this + //have a reference to a queue of strings or commands or something. + //will add commands that maps to do something on the robot to this. + //will have also a send queue where we can send data out. + Queue buffer; + //this might cause a problem with the buffer where we have old or repeated(handled by tcp) packets receiving. fuck. + String ip = "192.168.4.4"; +// String ip = "127.0.0.1"; + + String PreviousPacket = null; + + public void resetPreviousPacket() { + PreviousPacket = null; + } + public void setPreviousPacket(String Prev) { + PreviousPacket = Prev; + } + + int port = 8081; + final static int FORWARDi = 1; + final static int TURNRIGHTi = 2; + final static int TURNLEFTi = 3; + final static int REVERSEi = 4; + public static final int CALIBRATEi = 5; + final static int PHOTOi = 6; + boolean explorationflag = false; + final static byte UPDATESENSOR = 0x1; + public PacketFactory() { + + } + + public PacketFactory(Queue buffer2) { + sc = new SocketClient(ip, port); + sc.connectToDevice(); + this.buffer = buffer2; + + + } + + public void reconnectToDevice() { + sc.closeConnection(); + sc.connectToDevice(); + } + + @Override + public void run() { + //this cause me the problem of not having the + //fucking packetFactory to send. thus i need a sending buffer too... + while(true) { + listen(); + } + + } + + public void listen() { + boolean flag = true; + String data = null; + while(data==null) { + data = sc.receivePacket(explorationflag, PreviousPacket); + } + + System.out.println("receiving Data : " + data); + String removeDataString = data.replace("P:", ""); + if(explorationflag == false) + // TODO: RESET COMES TO HERE DURING EXPLORATION 2 @JARRETT + processPacket(removeDataString); + else + recvSensorOrStop(removeDataString); + + + + } + + + public void processPacket(String packetString) { + String[] splitPacket = packetString.split(Packet.Splitter); + if(splitPacket[1].equalsIgnoreCase(Packet.StartExplorationType)) { + //start exploration + //when exploration end and reach the start position + //send ok:exploration to android + System.out.println("starting exploration..."); + buffer.add(new Packet(Packet.StartExploration)); + System.out.print("*******************************************received packet for Exploration*********************************************\n"); + sc.sendPacket(Packet.StartExplorationTypeOk + "$"); + sc.sendPacket(Packet.StartExplorationTypeOkARD + ":" + whatever +"$"); + + explorationflag = true; + + + + }else if(splitPacket[1].equalsIgnoreCase(Packet.StartFastestPathType)) { + //start fastest path. just send the whole stack of instruction at once + //need to get the x,y value of the waypoint + //send to android that it is ready to move. + System.out.println("*****************************************received packet for fastest path**************************************\n"); + + buffer.add(new Packet(Packet.StartFastestPath)); + + } + else if(splitPacket[1].equalsIgnoreCase(Packet.Stop)) { + //stop moving robot FUCK i need multi thread this. + //i need a stopping flag god damn it + //interrupt exploration + buffer.add(new Packet(Packet.StopInstruction)); + sc.sendPacket(Packet.StopOk); + } + else if(splitPacket[1].equalsIgnoreCase(Packet.Reset)) { + //carry robot back to starting point. + //reset map + // TODO: RESET COMES TO HERE DURING EXPLORATION 1 @JARRETT + buffer.add(new Packet(Packet.ResetInstruction)); + sc.sendPacket(Packet.ResetOK); + System.out.println("sending ok reset"); + } + else if(splitPacket[1].equals(Packet.GETMAP)) { + buffer.add(new Packet(Packet.GETMAPi)); + } + + else if(splitPacket[0].equals(Packet.Set)) { + if(splitPacket[1].equalsIgnoreCase(Packet.SetRobotPos)) { + //remove bracket, split by comma , set first as x second as y + //set robot direction and x and y +// this is a problem now. does not handle the robot Position +// String directionTemp = splitPacket[3];//set direction for robot +// +// String[] waypointcoord = splitPacket[2].replace("[", "").split(","); +// int x = Integer.parseInt(waypointcoord[0]); +// int y = Integer.parseInt(waypointcoord[1]); +// System.out.println("setting robot position at :" + x + ", " + y + "with robot facing " + directionTemp); +// buffer.add(new Packet(Packet.setRobotPosition, x, y, direction)); + sc.sendPacket(Packet.SetRobotPosOk); + } + else if(splitPacket[1].equalsIgnoreCase(Packet.SetWayPoint)) { + //remove bracket, split by comma , set first as x second as y + String[] waypointcoord = splitPacket[2].replace("[", "").replace("]", "").split(","); + int x = Integer.parseInt(waypointcoord[0]); + int y = Integer.parseInt(waypointcoord[1]); + //set robot position waypoint for the fastest path. after setting this, we will send all instruction to raspberry and not do anything. + //allow faster execution when android sends the command to start fastest path. + //must edit set robot position. + buffer.add(new Packet(Packet.SetWayPointi, x, y)); + sc.sendPacket("A:"+Packet.SetWayPointOK); + } + } + else { + System.out.println("String received is invalid..."); + } + + } + + + public void recvSensorOrStop(String packetString) { + System.out.println("*************************************recvSensorOrStop called*********************************************\n"); + String[] commandSplit = packetString.split(Packet.Splitter); + if(commandSplit[0].equalsIgnoreCase(Packet.Map)) { + if(commandSplit[1].equalsIgnoreCase("sensor")) { + int[] data = new int[6]; + String[] sensorData = commandSplit[2].replace(" ","").replace("[", "").replace("]", "").split(","); + for(int i = 0; i < sensorData.length; i++) { + data[i] = Integer.parseInt(sensorData[i]); + } + buffer.add(new Packet(Packet.setObstacle, data)); + + } + } + else if(commandSplit[1].equalsIgnoreCase(Packet.Stop)) { + //stop moving robot FUCK i need multi thread this. + //i need a stopping flag god damn it + //interrupt exploration + sc.sendPacket(Packet.StopOk); + explorationflag = false; + buffer.add(new Packet(Packet.StopInstruction)); + } + else if(commandSplit[1].equalsIgnoreCase(Packet.Reset)) { + //stop whatever is going on + //carry robot back to starting point. + //reset map + sc.sendPacket(Packet.ResetOK); + System.out.println("sending ok reset"); + explorationflag = false; + buffer.add(new Packet(Packet.ResetInstruction)); + + }else { + System.out.println("Data received and ignored."); + } + + } + + + public boolean getFlag() { + return explorationflag; + } + + public void setFlag(boolean flag) { + this.explorationflag = flag; + } + + public Packet getLatestPacket() { + if(buffer.isEmpty()) + return null; + return buffer.remove(); + } + public void sideTurnCalibrate() { + //we need a return packet after calibration? + sc.sendPacket(Packet.SIDETURNCALIBRATE); + setPreviousPacket(Packet.SIDETURNCALIBRATE); + } + + //for debugging purposes only + public void sideCalibrate(int x, int y, int directionNum) { + System.out.println("debug side calibrate"); + sc.sendPacket(Packet.SIDECALIBRATE + "$"); +// sc.sendPacket(Packet.SIDECALIBRATE + Packet.Splitter + x + Packet.Splitter + y + Packet.Splitter + directionNum + "$"); +/* try { + Thread.sleep((int) (delay)); + }catch (Exception e){ + System.out.println("You ran into an error you idiot. Get a life."); + } + sendPhotoDataToRpi();*/ +// sendPhotoDataToRpi(); + setPreviousPacket(Packet.SIDECALIBRATE); + } + + //for debugging purposes only + public void frontCalibrate(int x, int y, int directionNum) { + System.out.println("debug front calibrate"); +// if (isFacingWall(x, y, directionNum)) { +// if(camRun) sc.sendPacket(Packet.FRONTCALIBRATE + Packet.Splitter + "-1" + Packet.Splitter + "-1" + Packet.Splitter + "-1" + "$"); +// else sc.sendPacket(Packet.FRONTCALIBRATE); +// } +// else { +// if(camRun) sc.sendPacket(Packet.FRONTCALIBRATE + Packet.Splitter + x + Packet.Splitter + y + Packet.Splitter + directionNum + "$"); +// else sc.sendPacket(Packet.FRONTCALIBRATE); +// } + sc.sendPacket(Packet.FRONTCALIBRATE + "$"); +// sc.sendPacket(Packet.FRONTCALIBRATE + Packet.Splitter + x + Packet.Splitter + y + Packet.Splitter + directionNum + "$"); + setPreviousPacket(Packet.FRONTCALIBRATE); + } + + //for debugging purposes only + public void leftCalibrate(int x, int y, int directionNum) { + System.out.println("debug left calibrate"); +// if (isFacingWall(x, y, directionNum)) { +// if (camRun) sc.sendPacket(Packet.LEFTCALIBRATE + Packet.Splitter + "-1" + Packet.Splitter + "-1" + Packet.Splitter + "-1" + "$"); +// else sc.sendPacket(Packet.LEFTCALIBRATE); +// } +// else { +// if(camRun) sc.sendPacket(Packet.LEFTCALIBRATE + Packet.Splitter + x + Packet.Splitter + y + Packet.Splitter + directionNum + "$"); +// else sc.sendPacket(Packet.LEFTCALIBRATE); +// } + sc.sendPacket(Packet.LEFTCALIBRATE); +// sc.sendPacket(Packet.FRONTCALIBRATE + Packet.Splitter + x + Packet.Splitter + y + Packet.Splitter + directionNum + "$"); + setPreviousPacket(Packet.LEFTCALIBRATE); + } + + public void initialCalibrate() { + sc.sendPacket(Packet.INITIALCALIBRATE); + } + + public boolean createFullMovementPacketToArduino(Queue instructions) { + //create one whole command for multiple movement + //send to both android and arduino + String toSend = null; + int count = 1; + int temp = 0; + if(instructions==null || instructions.isEmpty()) + return false; + System.out.println("Sending instruction"); + int subinstruct = instructions.remove(); + while(!instructions.isEmpty()) { + temp = instructions.remove(); + if(subinstruct == temp && count < 10 && subinstruct == Packet.FORWARDi) { + count++; + if(!instructions.isEmpty()) { + continue; + } + } + if(subinstruct == FORWARDi) { + toSend = Packet.FORWARDCMD + Packet.Splitter + count + "$"; +// System.out.println("Sending "+ toSend + "..."); + } + else if(subinstruct == TURNRIGHTi) { + toSend = Packet.TURNRIGHTCMD + Packet.Splitter + 0 + "$"; +// System.out.println("Sending "+ toSend + "..."); + } + else if(subinstruct == TURNLEFTi) { + toSend = Packet.TURNLEFTCMD + Packet.Splitter + 0 + "$"; +// System.out.println("Sending "+ toSend + "..."); + } + sc.sendPacket(toSend); + /*try { + Thread.sleep(850); + } catch (InterruptedException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + }*/ + + + count = 1; + if(instructions.isEmpty() && subinstruct!=temp) { + if(temp == FORWARDi) { + toSend = Packet.FORWARDCMD + Packet.Splitter + count + "$"; +// System.out.println("Sending "+ toSend + "..."); + } + else if(temp == TURNRIGHTi) { + toSend = Packet.TURNRIGHTCMD + Packet.Splitter + 0 + "$"; +// System.out.println("Sending "+ toSend + "..."); + } + else if(temp == TURNLEFTi) { + toSend = Packet.TURNLEFTCMD + Packet.Splitter + 0 + "$"; + System.out.println("Sending "+ toSend + "..."); + } + /*try { + Thread.sleep(1000); + } catch (InterruptedException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + }*/ + sc.sendPacket(toSend); + break; + } + subinstruct = temp; + + } + sideTurnCalibrate(); + return true; + + //either send here or return string...(unsure) + } + + + + public int getWaypoint_X() { + return waypoint_x; + } + + public int getWaypoint_Y() { + return waypoint_y; + } + + public void sendWholeMap(Map mapP) { + //transpose the array... + int[][] map = mapP.getMapArray(); + String mapCmd = Packet.MAPDESCRIPTORCMD + "["; + int[][] newMapArray = new int[Map.WIDTH][Map.HEIGHT]; + for(int i = 0 ; i < Map.HEIGHT; i++) { + for(int j = 0; j < Map.WIDTH; j++) { + newMapArray[j][i] = map[i][j] ; + } + } + for(int i = 0 ; i < Map.WIDTH; i++) { + mapCmd += Arrays.toString(newMapArray[i]); + if(i != Map.WIDTH-1) + mapCmd += ","; + } + mapCmd += "]$"; + sc.sendPacket(mapCmd); + //transpose finished + + //send array to android. + } + + public void sendWholeMapRpi(Map mapP) { + //transpose the array... + int[][] map = mapP.getMapArray(); + String mapCmd = Packet.MAPDESCRIPTORCMDRPI + "["; + int[][] newMapArray = new int[Map.WIDTH][Map.HEIGHT]; + for(int i = 0 ; i < Map.HEIGHT; i++) { + for(int j = 0; j < Map.WIDTH; j++) { + newMapArray[j][i] = map[i][j] ; + } + } + for(int i = 0 ; i < Map.WIDTH; i++) { + mapCmd += Arrays.toString(newMapArray[i]); + if(i != Map.WIDTH-1) + mapCmd += ","; + } + mapCmd += "]$"; + sc.sendPacket(mapCmd); + //transpose finished + + //send array to android. + } + + public boolean isFacingWall(int x, int y, int directionNum) { + +// directionNum + +// case UP: +// return 0; + +// case RIGHT: +// return 1; + +// case DOWN: +// return 2; + +// case LEFT: +// return 3; + + if ( (y == 1 && directionNum == 0) || // facing top wall + (y == 18 && directionNum == 2) || // facing bottom wall + (x == 1 && directionNum == 3) || // facing left wall + (x == 13 && directionNum == 1) ) // facing right wall + return true; + else + return false; + + } + + public boolean createOneMovementPacketToArduino(int instruction, int x, int y, int directionNum) { + //for one by one exploration + //create one Packet for one movement + //send to both android and arduino + String instructionString = null; + String instructionString2 = null; + if(instruction == FORWARDi) { + instructionString = Packet.FORWARDCMD; + instructionString2 = Packet.FORWARDCMDANDROID; + System.out.println("Sending a Forward Packet"); + } + else if(instruction == TURNRIGHTi) { + instructionString = Packet.TURNRIGHTCMD; + instructionString2 = Packet.TURNRIGHTCMDANDROID; + System.out.println("Sending a Turn right Packet"); + } + else if(instruction == TURNLEFTi) { + instructionString = Packet.TURNLEFTCMD; + instructionString2 = Packet.TURNLEFTCMDANDROID; + System.out.println("Sending a Turn Left Packet"); + } + else if(instruction == REVERSEi) { + instructionString = Packet.REVERSECMD; + instructionString2 = Packet.REVERSECMDANDROID; + + System.out.println("Sending a Reverse Packet"); + } + else { + System.out.println("Error: Wrong format"); + return false; + } + + instructionString = instructionString + Packet.Splitter + "1" + "$"; + sc.sendPacket(instructionString); + + instructionString2 = instructionString2 + Packet.Splitter + "1" + "$"; + sc.sendPacket(instructionString2); + + setPreviousPacket(instructionString); + return true; + //either send here or return string...(unsure) + + } + public void sendCMD(String cmd) { + sc.sendPacket(cmd); + } +} \ No newline at end of file diff --git a/src/algorithms/PathDrawer.java b/src/algorithms/PathDrawer.java new file mode 100644 index 0000000..fbd9c58 --- /dev/null +++ b/src/algorithms/PathDrawer.java @@ -0,0 +1,79 @@ +package algorithms; + +import java.awt.BasicStroke; +import java.awt.Color; +import java.awt.Graphics; +import java.awt.Graphics2D; +import java.util.Stack; + +public final class PathDrawer { + + static int robotX; + static int robotY; + static Stack path; + + static Stack unexploredAreas; + PathDrawer(){ + + } + public static void update(int robotx, int roboty, Stack inPath) + { + robotX = robotx; + robotY = roboty; + path = inPath; + + } + public static void updateUnexploredAreas(Stack InUnexploredAreas) { + unexploredAreas = InUnexploredAreas; + } + public static void removePath() + { + //dispose of the path after drawing + path.removeAllElements(); + } + public static void drawPath(Graphics g) + { + if(path == null || path.empty()) return; + + //set the stroke size to a larger width + Graphics2D g2 = (Graphics2D) g; + g2.setStroke(new BasicStroke(10)); + + //set color to cyan + g.setColor(Color.CYAN); + + //draw the first line from the robot to the first node + g.drawLine(30+robotX*Map.sizeofsquare,30+robotY*Map.sizeofsquare, + 30+path.get(path.size()-1).getX()*Map.sizeofsquare,30+path.get(path.size()-1).getY()*Map.sizeofsquare); + + //g.drawLine(30+robotX*Map.sizeofsquare,30+robotY*Map.sizeofsquare, + // 30+path.get(0).getX()*Map.sizeofsquare,30+path.get(0).getY()*Map.sizeofsquare); + + + //draw the rest of the lines + for(int i = 0; i < path.size()-1; i++) + { + //x_g=10+(x-1)*Map.sizeofsquare + g.drawLine(30+path.get(i).getX()*Map.sizeofsquare,30+path.get(i).getY()*Map.sizeofsquare, + 30+path.get(i+1).getX()*Map.sizeofsquare,30+path.get(i+1).getY()*Map.sizeofsquare); + } + + //dispose of the path after drawing + //path.removeAllElements(); + } + public static void drawGrid(Graphics g) + { + if(unexploredAreas == null || unexploredAreas.empty()) return; + + //set color to cyan + g.setColor(Color.RED); + + + //draw the the grids + for(int i = 0; i < unexploredAreas.size(); i++) + { + //g.drawRect(10+(unexploredAreas.get(i)[0]-1)*Map.sizeofsquare, 10+(unexploredAreas.get(i)[1]-1)*Map.sizeofsquare, 40, 40); + g.fillRect(10+(unexploredAreas.get(i)[0])*Map.sizeofsquare, 10+(unexploredAreas.get(i)[1])*Map.sizeofsquare, 38, 38); + } + } +} \ No newline at end of file diff --git a/src/algorithms/RealRobot.java b/src/algorithms/RealRobot.java new file mode 100644 index 0000000..c1ccb81 --- /dev/null +++ b/src/algorithms/RealRobot.java @@ -0,0 +1,633 @@ +package algorithms; + +import java.util.LinkedList; +import java.util.Queue; +import java.util.Stack; + +public class RealRobot extends RobotInterface { + boolean wantToReset = false; + RealSensor[] Sen; + // ArrayList TraverseNodes = new ArrayList(); + PacketFactory pf = null; + //static SocketClient sc = null; + int directionNum = -1; + int delay = 150; + int scdelay = 0; + int sideCalibrateCount = 0; + int frontCalibrateCount = 0; + int sideCalibrateNum = 3; + int FrontCalibrateNum =3; + float stepsPerSecond = 10f; + boolean sideCalibrated = false; + boolean frontCalibrated = false; +/* boolean hitWallFront=false; + boolean hitWallRight=false;*/ + boolean stepByStep = true; + boolean fastestcalibrate = false; //needs to calibrate for fastest + int count = 0; + int numsteps = 0; + int[][] mapConfirmed = new int[][]{ + {7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 1, 1, 1}, + {7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 1, 1, 1}, + {7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 1, 1, 1}, + {7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7}, + {7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7}, + {7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7}, + {7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7}, + {7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7}, + {7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7}, + {7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7}, + {7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7}, + {7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7}, + {7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7}, + {7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7}, + {7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7}, + {7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7}, + {7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7}, + {1, 1, 1, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7}, + {1, 1, 1, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7}, + {1, 1, 1, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7} + }; + + public void setSpeed(float stepsPerSecond){ + this.stepsPerSecond = stepsPerSecond; + } + + public RealRobot(int x, int y, Direction facing, Map map, PacketFactory pf){ + //starting postiion + super(); + this.pf = pf; + this.x = x; + this.y = y; + this.facing = facing; + this.map = map; + SenseRobotLocation(); + + //this.mapArray = mapArray; + //map = new Map(mapArray); + } + + public int getDirectionNum() { + switch(facing){ + case UP: + return 0; + case RIGHT: + return 1; + case DOWN: + return 2; + case LEFT: + return 3; + default: + return -1; + } + } + + public void addSensors(RealSensor[] sensors) { + this.Sen = sensors; + } + + public void updateSensor() { + for(int i=0;i < Sen.length; i++) { + Sen[i].updateRobotLocation(x, y); + } + } + + public void deactivateSensors() + { + Sen = new RealSensor[0]; + } + + + public void moveRobot(){ + numsteps = 1; + System.out.print("moving robot\n"); + int movementDistance = 1; + + if(facing == Direction.UP) { + y -= movementDistance; + mapConfirmed[y+1][x-1] = 1; + mapConfirmed[y+1][x] = 1; + mapConfirmed[y+1][x+1] = 1; + } + else if(facing == Direction.DOWN) { + y += movementDistance; + mapConfirmed[y-1][x-1] = 1; + mapConfirmed[y-1][x] = 1; + mapConfirmed[y-1][x+1] = 1; + } + else if(facing == Direction.RIGHT) { + x += movementDistance; + mapConfirmed[y][x-1] = 1; + mapConfirmed[y][x] = 1; + mapConfirmed[y][x+1] = 1; + } + else if(facing == Direction.LEFT) { + x -= movementDistance; + mapConfirmed[y][x-1] = 1; + mapConfirmed[y][x] = 1; + mapConfirmed[y][x+1] = 1; + } + + + if(stepByStep) { + + count++; + if(count % 4 == 0) { + sendMapDescriptor(); + } + pf.createOneMovementPacketToArduino(Packet.FORWARDi, x, y, getDirectionNum()); + //sc.sendPacket("R:"+"cam:"+x+":"+y+":"+directionNum); + //update the location for the robot in the sensors + updateSensor(); + + //make sensors "sense" the surrounding + LookAtSurroundings(); + + } + + viz.repaint(); + + } + + public void reverse(){ + int movementDistance = 1; + + if(facing == Direction.UP) + y += movementDistance; + else if(facing == Direction.DOWN) + y -= movementDistance; + else if(facing == Direction.RIGHT) + x -= movementDistance; + else if(facing == Direction.LEFT) + x += movementDistance; + + if(stepByStep) { + pf.createOneMovementPacketToArduino(Packet.REVERSEi, x, y, getDirectionNum()); + //sc.sendPacket("R:"+"cam:"+x+":"+y+":"+directionNum); + //update the location for the robot in the sensors + updateSensor(); + + //make sensors "sense" the surrounding + LookAtSurroundings(); + + } + + viz.repaint(); + } + + public boolean getWantToReset() { + return this.wantToReset; + } + + public void LookAtSurroundings() { + Packet pck = null; + System.out.println("Waiting for Sensor Packets"); + while(pck == null || pck.type != Packet.setObstacle) { + pf.listen(); + System.out.println("++++++++++++++++++++++++++++++++++++++Dequeues buffer++++++++++++++++++++++++++++++++++++++++++\n"); + pck = pf.getLatestPacket(); + if(pck == null) { + System.out.println("++++++++++++++++++++++++++++++++++++++Packet is Null (Need to Reset Instruction)+++++++++++++++++++++++++++++++++++++++++++\n"); + continue; + } + System.out.println(pck.getType()); + if(pck.type== Packet.ResetInstruction) { + // TODO: RESET COMES TO HERE DURING EXPLORATION 3 @JARRETT + System.out.println("JALEPENO!"); + this.wantToReset = true; + this.map.resetMap(); + x = 1; + y = 18; + facing = Direction.RIGHT; + this.viz.repaint(); + return; // TODO: JARRETT ADDED FOR RESET + } + } + System.out.println("+++++++++++++++++++++++++++++++++++++Getting Sensor Data+++++++++++++++++++++++++++++++++++++++\n"); + int[] data = pck.getSensorData(); + // int countF=0; + // int countR=0; + for(int i=0;i < Sen.length; i++) { + //sensePlaceHolder = +// System.out.println("-------------Start sensing-------------"); +// System.out.println("Sensor: "+(i+1)+"\tRange: "+Sen[i].range); + Sen[i].Sense(map, data[i],mapConfirmed); + } + viz.repaint(); + + } + + + //for testing purpose currently + + + public void turnRight() { + System.out.print("turn right robot\n"); + //use this to change direction for robot instead + switch(facing) { + //turn right + case RIGHT: + facing = Direction.DOWN; + break; + case LEFT: + facing = Direction.UP; + break; + case UP: + facing = Direction.RIGHT; + break; + case DOWN: + facing = Direction.LEFT; + break; + } + + + //use this to change direction for robot instead + for(int i=0;i < Sen.length; i++) { + Sen[i].ChangeDirectionRight(); + } + if(stepByStep) { + pf.createOneMovementPacketToArduino(Packet.TURNRIGHTi, x, y, getDirectionNum()); + //sc.sendPacket("R:"+"cam:"+x+":"+y+":"+directionNum); + //update location for the robot in the sensors + updateSensor(); + + //make sensors "sense" the surrounding + LookAtSurroundings(); + + } + viz.repaint(); + + //wait for sensor update sensor data + } + public void turnLeft() { + System.out.print("turn left robot\n"); + + + //use this to change direction for robot instead + switch(facing) { + case RIGHT: + facing = Direction.UP; + break; + case LEFT: + facing = Direction.DOWN; + break; + case UP: + facing = Direction.LEFT; + break; + case DOWN: + facing = Direction.RIGHT; + break; + } + //change sensor direction to follow robot + for(int i=0;i < Sen.length; i++) { + Sen[i].ChangeDirectionLeft(); + } + if(stepByStep) { + pf.createOneMovementPacketToArduino(Packet.TURNLEFTi, x, y, getDirectionNum()); + //update the location for the robot in the sensors + updateSensor(); + //make sensors "sense" the surrounding + LookAtSurroundings(); + + + } + viz.repaint(); + + } + + public void SenseRobotLocation() { + + for(int i = -1; i <= 1; i++) + { + for(int j = -1; j <= 1; j++) + map.getMapArray()[y+i][x+j] = ExplorationTypes.toInt("EMPTY"); + } + } + + public boolean getFastestInstruction(Stack fast) { + stepByStep = false; + int numberofPacketCalibrate = 10; + int counttocalibrate = 0; + Queue instruction = new LinkedList(); + System.out.println("starting Fastest Path"); + + if (fast==null) { + System.out.println("NULL DATA! no fastest path."); + return false; + } + while(!fast.isEmpty()) { + Node two = (Node) fast.pop(); + counttocalibrate++; + System.out.println("Y" + two.getY()); + if(two.getX() > x) { + switch(facing) { + //turn right the fastest way + case RIGHT: + break; + case LEFT: + turnLeft(); + instruction.add(Packet.TURNLEFTi); + turnLeft(); + instruction.add(Packet.TURNLEFTi); + break; + case UP: + turnRight(); + instruction.add(Packet.TURNRIGHTi); + break; + case DOWN: + turnLeft(); + instruction.add(Packet.TURNLEFTi); + break; + } + + } + else if(two.getX() < x) { + switch(facing) { + //turn right the fastest way + //to face left + case RIGHT: + turnLeft(); + instruction.add(Packet.TURNLEFTi); + turnLeft(); + instruction.add(Packet.TURNLEFTi); + break; + case LEFT: + break; + case UP: + turnLeft(); + instruction.add(Packet.TURNLEFTi); + break; + case DOWN: + turnRight(); + instruction.add(Packet.TURNRIGHTi); + break; + } + + } + else if(two.getY() < y) { + //facing up + switch(facing) { + //to face up + case RIGHT: + turnLeft(); + instruction.add(Packet.TURNLEFTi); + break; + case LEFT: + turnRight(); + instruction.add(Packet.TURNRIGHTi); + break; + case UP: + break; + case DOWN: + turnLeft(); + instruction.add(Packet.TURNLEFTi); + turnLeft(); + instruction.add(Packet.TURNLEFTi); + break; + } + + } + else if(two.getY() > y) { + while(facing!= Direction.DOWN) { + switch(facing) { + //turn right the fastest way + case RIGHT: + turnRight(); + instruction.add(Packet.TURNRIGHTi); + break; + case LEFT: + turnLeft(); + instruction.add(Packet.TURNLEFTi); + break; + case UP: + turnLeft(); + instruction.add(Packet.TURNLEFTi); + turnLeft(); + instruction.add(Packet.TURNLEFTi); + break; + case DOWN: + break; + } + } + + } + + moveRobot(); + instruction.add(Packet.FORWARDi); + + } + stepByStep = true; + pf.createFullMovementPacketToArduino(instruction);//sends data. + return true; + + } + + public boolean calibrateWallFastestPath() { + boolean thirdflag = false; + boolean fourthflag = false; + boolean fifthflag = false; + //use this to change direction for robot instead + switch(facing) { + case RIGHT: + if(isBlocked(x-1, y-2)) + thirdflag = true; + if(isBlocked(x, y-2)) + fourthflag = true; + if(isBlocked(x+1, y-2)) + fifthflag = true; + break; + case LEFT: + if(isBlocked(x-1, y+2)) + thirdflag = true; + if(isBlocked(x, y+2)) + fourthflag = true; + if(isBlocked(x+1, y+2)) + fifthflag = true; + break; + case UP: + if(isBlocked(x+2, y-1)) + thirdflag = true; + if(isBlocked(x+2, y)) + fourthflag = true; + if(isBlocked(x+2, y+1)) + fifthflag = true; + break; + case DOWN: + if(isBlocked(x-2, y-1)) + thirdflag = true; + if(isBlocked(x-2, y)) + fourthflag = true; + if(isBlocked(x-2, y+1)) + fifthflag = true; + break; + } + if(thirdflag && fourthflag && fifthflag) { + return true; + } + return false; + + } + + public void finalIC() { + switch(facing) { + case RIGHT: + break; + case LEFT: + turnLeft(); + turnLeft(); + break; + case UP: + turnRight(); + break; + case DOWN: + turnLeft(); + break; + } + pf.initialCalibrate(); + } + + + + + + + public boolean doStepFastestPath() + { + /*stepByStep = true; + getFastestInstruction(); + //return false; + return true;*/ + //getFastestInstruction(fastestPath); + + if(instructionsForFastestPath.isEmpty()) + return true; + //if not empty then continue doing the path + else + { + frontCalibrated = false; + sideCalibrated = false; + int instruction = (Integer) instructionsForFastestPath.remove(0); + switch(instruction) + { + case Packet.TURNRIGHTi: + turnRight(); + //System.out.print("turning left" + x + y + '\n'); + break; + case Packet.TURNLEFTi: + turnLeft(); + //System.out.print("turning right" + x + y + '\n'); + break; + case Packet.FORWARDi: + if(sideCalibrateCount>=sideCalibrateNum) { + if (canSide_Calibrate()) { + System.out.println("Right calibrating\n+++++++++++++++++++++++++++++++++"); + side_Calibrate(); + sideCalibrateCount=0; + } else if (canLeft_Calibrate()) { + System.out.println("Left calibrating\n---------------------------------"); + left_Calibrate(); + sideCalibrateCount=0; + } + sideCalibrated=true; + //else sideCalibrateCount++; + } + if(frontCalibrateCount>=FrontCalibrateNum) { + if (canFront_Calibrate()) { + front_Calibrate(); + frontCalibrateCount=0; + frontCalibrated = true; + } + //else frontCalibrateCount++; + } + if (!frontCalibrated) frontCalibrateCount++; + if(!sideCalibrated) sideCalibrateCount++; + moveRobot(); + //System.out.print("move forward" + x + y + '\n'); + break; + } + } + return false; + } + + + @Override + public void addSensors(Sensor[] sensors) { + // TODO Auto-generated method stub + } + + @Override + public void side_Calibrate() { + pf.sideCalibrate(x, y, (getDirectionNum()+1)%4); + try { + Thread.sleep((int) (scdelay)); + }catch (Exception e){ + System.out.println("You ran into an error you idiot. Get a life."); + } +// pf.sendPhotoDataToRpi(x,y,(directionNum+1)%4); + LookAtSurroundings(); + } + + + + + @Override + public void front_Calibrate() { + System.out.println("front calibrating"); + pf.frontCalibrate(x, y, (getDirectionNum()+1)%4); + LookAtSurroundings(); + } + + @Override + public void left_Calibrate() { + System.out.println("left calibrating"); + pf.leftCalibrate(x, y, (getDirectionNum()-1)%4); + LookAtSurroundings(); + } + + + @Override + public void initial_Calibrate() { + // TODO Auto-generated method stub + switch(facing) { + //make sure the robot is facing the left wall for calibration. + case RIGHT: + turnLeft(); + turnLeft(); + break; + case LEFT: + break; + case UP: + turnLeft(); + break; + case DOWN: + turnRight(); + break; + + } + pf.initialCalibrate(); + System.out.println("#########################################initial Calibrating...#########################################"); + facing = Direction.RIGHT; + //update the android orientation + String instructionString2 = Packet.TURNLEFTCMDANDROID + Packet.Splitter + "1" + "$"; + pf.sendCMD(instructionString2); + pf.sendCMD(instructionString2); + + viz.repaint(); + + } + + + + @Override + public void sendMapDescriptor() { + pf.sendWholeMap(map); + + } + + public void sendMapDescriptorRpi() { + pf.sendWholeMapRpi(map); + + } + + +} \ No newline at end of file diff --git a/src/algorithms/RealSensor.java b/src/algorithms/RealSensor.java new file mode 100644 index 0000000..338e98f --- /dev/null +++ b/src/algorithms/RealSensor.java @@ -0,0 +1,159 @@ +package algorithms; + +//need check data + +public class RealSensor extends Sensor{ + + + public RealSensor(int range, SensorLocation currentDirection, int locationOnRobot_x, int locationOnRobot_y, + int robot_x, int robot_y) { + super(range, currentDirection, locationOnRobot_x, locationOnRobot_y, robot_x, robot_y); + // TODO Auto-generated constructor stub + } + + public boolean SenseLocation(Map map, int x, int y, int distanceFromRobot, boolean hitWall){ + int score = 0; + + if(distanceFromRobot == 1) + score = -34; + else if(distanceFromRobot == 2) + score = -21; + else if(distanceFromRobot == 3) + score = -8; + else if(distanceFromRobot == 4) + score = -5; + else if(distanceFromRobot == 5) + score = -2; + else if(distanceFromRobot == 6) + score = -2; + else if(distanceFromRobot == 7) + score = -2; + else if(distanceFromRobot == 8) + score = -2; + else if(distanceFromRobot == 9) + score = -2; + +// System.out.println("X: "+x+"\tY: "+y+"\tScore: "+score); + + if(x < Map.WIDTH && y < Map.HEIGHT && x >= 0 && y >= 0) + { +// System.out.println("*******X: "+x+"\tY: "+y+"\tScore: "+score); + //flip the score to positive to indicate that it is a block + if(hitWall) + score = -score; + + map.setMapScore(x,y,score); + } + + return hitWall; + } + public boolean Sense(Map map, int data, int[][] mapConfirmed) { + int nextLocationX = 0; + int nextLocationY = 0; + //boolean flag = false; + + //is true after robot hits a wall, to prevent it from sensing further + boolean hitWall = false; + boolean hitWallret = false; + + for(int i = 1; i <= range; i++) { + + //make sure it is in the map range and bound. + if(currentDirection ==SensorLocation.FACING_RIGHT) { + nextLocationX = robot_x+locationOnRobot_x+i; + nextLocationY = robot_y+locationOnRobot_y; + } + + else if(currentDirection == SensorLocation.FACING_LEFT) { + nextLocationX = robot_x+locationOnRobot_x-i; + nextLocationY = robot_y+locationOnRobot_y; + } + else if(currentDirection == SensorLocation.FACING_TOP) { + nextLocationX = robot_x+locationOnRobot_x; + nextLocationY = robot_y+locationOnRobot_y-i; + } + else{ + nextLocationX = robot_x+locationOnRobot_x; + nextLocationY = robot_y+locationOnRobot_y+i; + } + +// System.out.println("next X: "+nextLocationX+"\tnext Y: "+nextLocationY + "\ti:" + i + "\tdata:" + data); + + //hitwill will be true when sensor sensed a wall + if(!hitWall) + { + //when the sensor sensed a wall, then everything after that will be given score 0 + if(i == data){ + hitWall = true; + if(SenseLocation(map, nextLocationX, nextLocationY, 0, hitWall) && i==1) + hitWallret=true; + //hitWallret = true; + } +// System.out.println("-----------------Call sense location----------------"); + SenseLocation(map,nextLocationX, nextLocationY, i, hitWall); + } + //send a 0 to signify that this is behind a wall + else + SenseLocation(map,nextLocationX, nextLocationY, 0, hitWall); + } + System.out.println(hitWall); + + //update the map score after "sensing" + map.updateMapWithScore(); + + return hitWallret; + } + + public boolean SenseRight(Map map, int data, int[][] mapConfirmed) { + int nextLocationX = 0; + int nextLocationY = 0; + //boolean flag = false; + + //is true after robot hits a wall, to prevent it from sensing further + boolean hitWall = false; + boolean hitWallret = false; + + for(int i = 1; i <= range; i++) { + + //make sure it is in the map range and bound. + if(currentDirection ==SensorLocation.FACING_RIGHT) { + nextLocationX = robot_x+locationOnRobot_x+i; + nextLocationY = robot_y+locationOnRobot_y; + } + + else if(currentDirection == SensorLocation.FACING_LEFT) { + nextLocationX = robot_x+locationOnRobot_x-i; + nextLocationY = robot_y+locationOnRobot_y; + } + else if(currentDirection == SensorLocation.FACING_TOP) { + nextLocationX = robot_x+locationOnRobot_x; + nextLocationY = robot_y+locationOnRobot_y-i; + } + else{ + nextLocationX = robot_x+locationOnRobot_x; + nextLocationY = robot_y+locationOnRobot_y+i; + } + + //hitwill will be true when sensor sensed a wall + if(!hitWall) + { + //when the sensor sensed a wall, then everything after that will be given score 0 + if(i == data){ + hitWall = true; + if(SenseLocation(map, nextLocationX, nextLocationY, 0, hitWall) && i==2) + hitWallret=true; + //hitWallret = true; + } + SenseLocation(map,nextLocationX, nextLocationY, i, hitWall); + } + //send a 0 to signify that this is behind a wall + else + SenseLocation(map,nextLocationX, nextLocationY, 0, hitWall); + } + + //update the map score after "sensing" + map.updateMapWithScore(); + + return hitWallret; + } +} \ No newline at end of file diff --git a/src/algorithms/Robot.java b/src/algorithms/Robot.java new file mode 100644 index 0000000..cfec78b --- /dev/null +++ b/src/algorithms/Robot.java @@ -0,0 +1,347 @@ +package algorithms; + +import java.util.Stack; + + +public class Robot extends RobotInterface { + Sensor[] Sen; + boolean hitWallFront=false; + boolean hitWallRight=false; + int sideCalibrateCount = 0; + int frontCalibrateCount = 0; + int sideCalibrateNum = 3; + int FrontCalibrateNum = 3; + float stepsPerSecond = 1f; + boolean frontCalibrated = false; + boolean sideCalibrated = false; +// ArrayList TraverseNodes = new ArrayList(); + + public Robot(int x, int y, Direction facing, Map map){ + //starting postition + super(); + this.x = x; + this.y = y; + this.facing = facing; + this.map = map; + hitWallFront = false; + hitWallRight = false; + instructionsForFastestPath = new Stack(); + + SenseRobotLocation(); + } + + + public void addSensors(Sensor[] sensors) { + this.Sen = sensors; + + //make sensors "sense" the surrounding + LookAtSurroundings(); + } + + public void updateSensor() { + for(int i=0;i < Sen.length; i++) { + Sen[i].updateRobotLocation(x, y); + } + } + + public void deactivateSensors() + { + Sen = new Sensor[0]; + } + + public void setSpeed(float stepsPerSecond){ + this.stepsPerSecond = stepsPerSecond; + } + + public void moveRobot(){ + //System.out.print("moving forward\n"); + int movementDistance = 1; + + if(facing == Direction.UP) + y -= movementDistance; + else if(facing == Direction.DOWN) + y += movementDistance; + else if(facing == Direction.RIGHT) + x += movementDistance; + else if(facing == Direction.LEFT) + x -= movementDistance; + + //update the location for the robot in the sensors + updateSensor(); + + //make sensors "sense" the surrounding + LookAtSurroundings(); + + + } + + public void reverse(){ + int movementDistance = 1; + + if(facing == Direction.UP) + y += movementDistance; + else if(facing == Direction.DOWN) + y -= movementDistance; + else if(facing == Direction.RIGHT) + x -= movementDistance; + else if(facing == Direction.LEFT) + x += movementDistance; + + //update the location for the robot in the sensors + updateSensor(); + + //make sensors "sense" the surrounding + LookAtSurroundings(); + + + } + + public void LookAtSurroundings() { + boolean sensePlaceHolder; + boolean sensePlaceHolder1; + int countF=0; + int countR=0; + //SenseRobotLocation(); + for(int i=0;i < Sen.length; i++) { + sensePlaceHolder = Sen[i].Sense(map, 0, null); + sensePlaceHolder1 = Sen[i].SenseRight(map, 0, null); + if((i<=1||i==3) && sensePlaceHolder){ + countF++; + if(countF==3)hitWallFront=true; + else hitWallFront=false; + } + //System.out.println("!!!!!!!!!!!!!!!!!!!!!Front Wall hit !!!!!!!!!!!!!!!!!!!!\n");} + if((i==2||i==4) && sensePlaceHolder1){ + countR++; + if(countR==2) + hitWallRight=true; + else + hitWallRight=false; + } + //System.out.println("?????????????????????Right wall hit ????????????????????\n"); + } + System.out.println(countR); + if(hitWallFront && hitWallRight){ + System.out.println(":::::::::::::::::::::::::::::::::::hit both walls::::::::::::::::::::::::::::::::::::\n"); + front_Calibrate(); + side_Calibrate(); + hitWallFront=false; + hitWallRight=false; + } + } + + public void SenseRobotLocation() { + + for(int i = -1; i <= 1; i++) + { + for(int j = -1; j <= 1; j++) + map.getMapArray()[y+i][x+j] = ExplorationTypes.toInt("EMPTY"); + } + /*mapArray[y][x] = ExplorationTypes.toInt("EMPTY"); + mapArray[y][x-1] = ExplorationTypes.toInt("EMPTY");; + mapArray[y][x+1] = ExplorationTypes.toInt("EMPTY");; + mapArray[y-1][x] = ExplorationTypes.toInt("EMPTY");; + mapArray[y-1][x+1] = ExplorationTypes.toInt("EMPTY");; + mapArray[y-1][x-1] = ExplorationTypes.toInt("EMPTY");; + mapArray[y+1][x] = ExplorationTypes.toInt("EMPTY");; + mapArray[y+1][x-1] = ExplorationTypes.toInt("EMPTY");; + mapArray[y+1][x+1] = ExplorationTypes.toInt("EMPTY");;*/ + } + + public void turnRight() { + //System.out.print("turn right\n"); + //use this to change direction for robot instead + switch(facing) { + case RIGHT: + facing = Direction.DOWN; + break; + case LEFT: + facing = Direction.UP; + break; + case UP: + facing = Direction.RIGHT; + break; + case DOWN: + facing = Direction.LEFT; + break; + } + for(int i=0;i < Sen.length; i++) { + Sen[i].ChangeDirectionRight(); + } + //make sensors "sense" the surrounding + LookAtSurroundings(); + } + public void turnLeft() { + //System.out.print("turn left\n"); + //use this to change direction for robot instead + switch(facing) { + case RIGHT: + facing = Direction.UP; + break; + case LEFT: + facing = Direction.DOWN; + break; + case UP: + facing = Direction.LEFT; + break; + case DOWN: + facing = Direction.RIGHT; + } + //change sensor direction to follow robot + for(int i=0;i < Sen.length; i++) { + Sen[i].ChangeDirectionLeft(); + } + //make sensors "sense" the surrounding + LookAtSurroundings(); + } + + + public boolean getFastestInstruction(Stack fast) { + byte[] instruction= new byte[100]; + int instcount = 0; + if(fast==null) + return true; + while(!fast.isEmpty()) { + Node two = (Node) fast.pop(); + try { + Thread.sleep((long) (1000/stepsPerSecond)); + + //System.out.println("Y" + two.getY()); + if(two.getX() > x) { + while(facing!= Direction.RIGHT) { + turnRight(); + } + moveRobot(); + } + else if(two.getX() < x) { + while(facing!= Direction.LEFT) { + turnLeft(); + } + moveRobot(); + } + else if(two.getY() < y) { + while(facing!= Direction.UP) { + turnLeft(); + } + moveRobot(); + } + else /*if(two.getY() < one.getY()) */{ + while(facing!= Direction.DOWN) { + turnRight(); + } + moveRobot(); + } + viz.repaint(); + } catch (InterruptedException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + } + return true; + } + public boolean doStepFastestPath() + { + //once the instructions are empty, current fastest path is done + if(instructionsForFastestPath.isEmpty()) + return true; + //if not empty then continue doing the path + else + { + frontCalibrated = false; + sideCalibrated = false; + int instruction = (Integer) instructionsForFastestPath.remove(0); + switch(instruction) + { + case Packet.TURNRIGHTi: + turnRight(); + //System.out.print("turning left" + x + y + '\n'); + break; + case Packet.TURNLEFTi: + turnLeft(); + //System.out.print("turning right" + x + y + '\n'); + break; + case Packet.FORWARDi: + if(sideCalibrateCount>=sideCalibrateNum) { + if (canSide_Calibrate()) { + System.out.println("Right calibrating\n+++++++++++++++++++++++++++++++++"); + side_Calibrate(); + sideCalibrateCount=0; + } else if (canLeft_Calibrate()) { + System.out.println("Left calibrating\n---------------------------------"); + left_Calibrate(); + sideCalibrateCount=0; + } + sideCalibrated = true; + //else sideCalibrateCount++; + } + if(frontCalibrateCount>=FrontCalibrateNum) { + if (canFront_Calibrate()) { + System.out.println("Front calibrating\n&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&"); + front_Calibrate(); + frontCalibrateCount=0; + frontCalibrated = true; + } + //else frontCalibrateCount++; + } + if (!frontCalibrated) frontCalibrateCount++; + if(!sideCalibrated) sideCalibrateCount++; + moveRobot(); + break; + } + } + return false; + } + + @Override + public void addSensors(RealSensor[] sensors) { + // TODO Auto-generated method stub + + } + + @Override + public boolean isObstacleOrWallFront() { + // TODO Auto-generated method stub + return false; + } + + @Override + public boolean getWantToReset() { + return false; + } + + + @Override + public void initial_Calibrate() { + // TODO Auto-generated method stub + + } + + @Override + public void sendMapDescriptor() { + //sendWholeMap(map); + // TODO Auto-generated method stub + + } + + + @Override + public void side_Calibrate() { + System.out.println("Side calibrating"); + // TODO Auto-generated method stub + + } + + @Override + public void left_Calibrate(){ + System.out.println("Left Calibrate"); + //TODO Auto-generated method stub + } + + + @Override + public void front_Calibrate() { + System.out.println("Front calibrating"); + // TODO Auto-generated method stub + + } +} \ No newline at end of file diff --git a/src/algorithms/RobotInterface.java b/src/algorithms/RobotInterface.java new file mode 100644 index 0000000..cbfc12c --- /dev/null +++ b/src/algorithms/RobotInterface.java @@ -0,0 +1,466 @@ +package algorithms; + +import java.awt.Color; +import java.awt.Graphics; +import java.util.LinkedList; +import java.util.Queue; +import java.util.Stack; + +public abstract class RobotInterface { + Visualization viz; + + //the x and y position of the robot on the map + int x; int y; + + //the x and y coordinates to draw the robot on screen + int x_g, y_g; + //radius of the circle to draw the robot + final static int radius = 114; + + //the facing direction of the robot + Direction facing; + + //the map reference + Map map; + final int WIDTH = 15; + final int HEIGHT = 20; + + //fastest path from a point to another point + Stack fastestPath = new Stack(); + //the converted int instructions from the above stack array +// Stack instructionsForFastestPath; + Stack instructionsForFastestPath = new Stack(); + + public abstract void addSensors(RealSensor[] sensors); + public abstract void addSensors(Sensor[] sensors); + public abstract void LookAtSurroundings(); + public abstract void SenseRobotLocation(); + public abstract void moveRobot();//move robot forward + public abstract void turnLeft(); + public abstract void turnRight(); + public abstract boolean getFastestInstruction(Stack fast); + public abstract void deactivateSensors(); + public abstract void reverse(); + public abstract void side_Calibrate(); + public abstract void left_Calibrate(); + public abstract void front_Calibrate(); + public abstract void initial_Calibrate(); + public abstract void sendMapDescriptor(); + public abstract boolean doStepFastestPath(); + public abstract void setSpeed(float stepsPerSecond); + //set the fastest path for the robot to follow + public void setFastestInstruction(Stack fast, int targetX, int targetY) + { + //for the purpose of this function only, to input the instructions into the stack + Direction tempFacing = facing; + int tempX = x; + int tempY = y; + Node next = null; + + //loop until the stack(fast) is empty + while(true) + { + //if the stack is empty, means the nodes have fully been converted + //next step will be to TURN towards the correct direction + if(fast.empty())break; + else + { + next = (Node) fast.pop(); + if(next.getX() > tempX) { + if(tempFacing!= Direction.RIGHT) + tempFacing = getShortestTurnInstruction(tempFacing, Direction.RIGHT); + + tempX += 1; + } + else if(next.getX() < tempX) { + if(tempFacing!= Direction.LEFT) + tempFacing = getShortestTurnInstruction(tempFacing, Direction.LEFT); + + tempX -= 1; + } + else if(next.getY() < tempY) { + if(tempFacing!= Direction.UP) + tempFacing = getShortestTurnInstruction(tempFacing, Direction.UP); + + tempY -= 1; + } + else if(next.getY() > tempY) { + if(tempFacing!= Direction.DOWN) + tempFacing = getShortestTurnInstruction(tempFacing, Direction.DOWN); + + tempY += 1; + } + + instructionsForFastestPath.add(Packet.FORWARDi); + } + } + } + + public Direction simulateTurnRight(Direction tempFacing) { + switch(tempFacing) { + case RIGHT: + tempFacing = Direction.DOWN; + break; + case LEFT: + tempFacing = Direction.UP; + break; + case UP: + tempFacing = Direction.RIGHT; + break; + case DOWN: + tempFacing = Direction.LEFT; + break; + } + return tempFacing; + } + + public Direction simulateTurnLeft(Direction tempFacing) + { + switch(tempFacing) { + case RIGHT: + tempFacing = Direction.UP; + break; + case LEFT: + tempFacing = Direction.DOWN; + break; + case UP: + tempFacing = Direction.LEFT; + break; + case DOWN: + tempFacing = Direction.RIGHT; + } + return tempFacing; + } + public Direction getShortestTurnInstruction(Direction tempFacing, Direction targetFacing) + { + Stack listOfTurnInstructions = new Stack(); + + if(tempFacing == Direction.RIGHT) + { + if(targetFacing == Direction.UP) + instructionsForFastestPath.add(Packet.TURNLEFTi); + + else if(targetFacing == Direction.DOWN) + instructionsForFastestPath.add(Packet.TURNRIGHTi); + + else if(targetFacing == Direction.LEFT) + { + instructionsForFastestPath.add(Packet.TURNLEFTi); + instructionsForFastestPath.add(Packet.TURNLEFTi); + } + + } + else if(tempFacing == Direction.LEFT) + { + if(targetFacing == Direction.UP) + instructionsForFastestPath.add(Packet.TURNRIGHTi); + + else if(targetFacing == Direction.DOWN) + instructionsForFastestPath.add(Packet.TURNLEFTi); + + else if(targetFacing == Direction.RIGHT) + { + instructionsForFastestPath.add(Packet.TURNLEFTi); + instructionsForFastestPath.add(Packet.TURNLEFTi); + } + + } + else if(tempFacing == Direction.UP) + { + if(targetFacing == Direction.LEFT) + instructionsForFastestPath.add(Packet.TURNLEFTi); + + else if(targetFacing == Direction.RIGHT) + instructionsForFastestPath.add(Packet.TURNRIGHTi); + + else if(targetFacing == Direction.DOWN) + { + instructionsForFastestPath.add(Packet.TURNLEFTi); + instructionsForFastestPath.add(Packet.TURNLEFTi); + } + + } + else if(tempFacing == Direction.DOWN) + { + if(targetFacing == Direction.RIGHT) + instructionsForFastestPath.add(Packet.TURNLEFTi); + + else if(targetFacing == Direction.LEFT) + instructionsForFastestPath.add(Packet.TURNRIGHTi); + + else if(targetFacing == Direction.UP) + { + instructionsForFastestPath.add(Packet.TURNLEFTi); + instructionsForFastestPath.add(Packet.TURNLEFTi); + } + + } + //return listOfTurnInstructions; + return targetFacing; + } + public boolean canSide_Calibrate() + { + //returns true if the right side of the robot have blocks to use to calibrate + if(facing == Direction.LEFT && isBlocked(x-1, y-2) && isBlocked(x+1, y-2)) + return true; + else if(facing == Direction.RIGHT && isBlocked(x-1, y+2) && isBlocked(x+1, y+2)) + return true; + else if(facing == Direction.DOWN && isBlocked(x-2, y-1) && isBlocked(x-2, y+1)) + return true; + else if(facing == Direction.UP && isBlocked(x+2, y-1) && isBlocked(x+2, y+1)) + return true; + + + return false; + } + + public boolean canFront_Calibrate() + { + //returns true if the right side of the robot have blocks to use to calibrate + if(facing == Direction.LEFT && isBlocked(x-2, y-1) && isBlocked(x-2, y) && isBlocked(x-2, y+1)) + return true; + else if(facing == Direction.RIGHT && isBlocked(x+2, y-1) && isBlocked(x+2, y) && isBlocked(x+2, y+1)) + return true; + else if(facing == Direction.DOWN && isBlocked(x-1, y+2) && isBlocked(x, y+2) && isBlocked(x+1, y+2)) + return true; + else if(facing == Direction.UP && isBlocked(x-1, y-2) && isBlocked(x, y-2) && isBlocked(x+1, y-2)) + return true; + + + return false; + } + + public boolean canLeft_Calibrate() + { + //returns true if the right side of the robot have blocks to use to calibrate + if(facing == Direction.LEFT && isBlocked(x-1, y+2) && isBlocked(x+1, y+2)) + return true; + else if(facing == Direction.RIGHT && isBlocked(x-1, y-2) && isBlocked(x+1, y-2)) + return true; + else if(facing == Direction.DOWN && isBlocked(x+2, y-1) && isBlocked(x+2, y+1)) + return true; + else if(facing == Direction.UP && isBlocked(x-2, y-1) && isBlocked(x-2, y+1)) + return true; + + + return false; + } + + public int getX() { + // TODO Auto-generated method stub + return x; + } + + + public void setRobotPos(int x, int y, Direction facing) { + this.x = x; + this.y = y; + this.facing = facing; + } + + + public int getY() { + // TODO Auto-generated method stub + return y; + } + + public Visualization getViz() { + return viz; + } + + public void setface(Direction facing){ + this.facing = facing; + } + + public void setViz(Visualization viz) { + this.viz = viz; + } + + public boolean isObstacleOrWallFront(){ + //System.out.println("Current Position: " + x +" " + y); + switch(facing){ + case UP: + if(isBlocked(x-1,y-2) || isBlocked(x,y-2) ||isBlocked(x+1,y-2)) + return true; + break; + case DOWN: + if(isBlocked(x-1,y+2) ||isBlocked(x,y+2) ||isBlocked(x+1,y+2)) + return true; + break; + case LEFT: + if(isBlocked(x-2,y-1) ||isBlocked(x-2,y) ||isBlocked(x-2,y+1)) + return true; + break; + case RIGHT: + if(isBlocked(x+2,y-1) ||isBlocked(x+2,y) ||isBlocked(x+2,y+1)){ + return true; + } + break; + + } + return false; + } + + + public boolean checkLeftBound(int xi, int yi) { + if((xi < 0 )){ + + //System.out.println("ERROR: out of left bound"); + return true; + } + return false; + } + + public boolean checkTopBound(int xi, int yi) { + if((yi < 0 )){ + //System.out.println("ERROR: out of top bound"); + return true; + } + return false; + } + public boolean checkBottomBound(int xi, int yi) { + if(yi > (HEIGHT)) { + //System.out.println("ERROR: out of bottom bound"); + return true; + } + return false; + } + public boolean checkRightBound(int xi, int yi) { + if(xi > (WIDTH) ){ + //System.out.println("ERROR: out of right bound"); + return true; + } + return false; + } + + public boolean checkObstacle(int xi, int yi) { + if(yi >= HEIGHT || xi >= WIDTH || yi < 0 || xi < 0) { + return true; + } + else if(map.getMapArray()[yi][xi] == ExplorationTypes.toInt("OBSTACLE")){ + //System.out.println("grid is obstacle"); + return true; + } + return false; + } + + //input the direction of checking and return true if robot can move that direction + public boolean isAbleToMove(Direction dir) { + + return isAbleToMove(dir,this.x,this.y); + } + public boolean isAbleToMove(Direction dir, int x, int y) { + boolean canMove = true; + System.out.println("Direction: "+dir+"\nx coord: "+x+"\ny coord: "+y); + switch (dir) + { + case LEFT: + for (int i = -1; i < 2; i++) + { + //if part of the robot is out of bounds or going to hit a wall + if (checkLeftBound(x - 1, y) || checkObstacle(x - 1 - 1, y + i)) + { + canMove = false; + break; + } + } + break; + + + case RIGHT: + for (int i = -1; i < 2; i++) + { + //if part of the robot is out of bounds or going to hit a wall + if (checkRightBound(x + 1, y) || checkObstacle(x + 1 + 1, y + i)) + { + canMove = false; + break; + } + } + break; + + case UP: + for (int i = -1; i < 2; i++) + { + //if part of the robot is out of bounds or going to hit a wall + if (checkTopBound(x, y - 1) || checkObstacle(x + i, y - 1 - 1)) + { + canMove = false; + break; + } + } + break; + + case DOWN: + + for (int i = -1; i < 2; i++) + { + //if part of the robot is out of bounds or going to hit a wall + if (checkBottomBound(x, y + 1) || checkObstacle(x + i, y + 1 + 1)) + { + canMove = false; + break; + } + } + break; + } + return canMove; + } + public boolean isBlocked(int xi, int yi){ + //return 1 if obstacle, 0 if no obstacle, -1 if out of bound + boolean rflag= false, tflag= false, bflag= false, lflag = false,obflag = false; + lflag = checkLeftBound(xi, yi); + tflag = checkTopBound(xi, yi); + rflag = checkRightBound(xi, yi); + bflag = checkBottomBound(xi, yi); + obflag = checkObstacle(xi, yi); + if(lflag||tflag||rflag||bflag||obflag) { + return true; + } + + if(map.getMapArray()[yi][xi] == ExplorationTypes.toInt("UNEXPLORED_EMPTY") || + map.getMapArray()[yi][xi] == ExplorationTypes.toInt("UNEXPLORED_OBSTACLE")) + return true; + + return false; + } + + //returns false if cannot move + public boolean canRobotMoveHere(int x, int y) + { + for(int i = -1; i <= 1; i++) + { + for(int j = -1; j <= 1; j++) + { + if(isBlocked(x+i, y+j)) + return false; + } + } + + return true; + } + public void paintRobot(Graphics g){ + g.setColor(Color.RED); + x_g=10+(x-1)*Map.sizeofsquare; + y_g=10+(y-1)*Map.sizeofsquare; + g.fillArc(x_g, y_g, radius, radius, 0, 360); + + g.setColor(Color.BLUE); + + int dirOffsetX = 0; + int dirOffsetY = 0; + + if(facing == Direction.UP) + dirOffsetY = -45; + else if(facing == Direction.DOWN) + dirOffsetY = 45; + else if(facing == Direction.LEFT) + dirOffsetX = -45; + else if(facing == Direction.RIGHT) + dirOffsetX = 45; + + g.fillArc(x_g + 45 + dirOffsetX, y_g + 45+ dirOffsetY, 20, 20, 0, 360); + } + + public abstract boolean getWantToReset(); +} \ No newline at end of file diff --git a/src/algorithms/Sensor.java b/src/algorithms/Sensor.java new file mode 100644 index 0000000..09fee9c --- /dev/null +++ b/src/algorithms/Sensor.java @@ -0,0 +1,371 @@ +package algorithms; + +import java.util.HashMap; + +public class Sensor{ + + private static final int WIDTH = 15; + private static final int HEIGHT = 20; + int range; + SensorLocation currentDirection; + int locationOnRobot_x; + int locationOnRobot_y; + int robot_x; + int robot_y; + boolean hitWallCheck; + int[] sensor_XY = new int[2]; + HashMap coordinatesLeft; + HashMap coordinatesRight; + + + + public Sensor(int range, SensorLocation currentDirection, int locationOnRobot_x, int locationOnRobot_y, int robot_x, + int robot_y) { + super(); + this.range = range; + this.currentDirection = currentDirection; + this.locationOnRobot_x = locationOnRobot_x; + this.locationOnRobot_y = locationOnRobot_y; + this.robot_x = robot_x; + this.robot_y = robot_y; + this.hitWallCheck = false; + } + + + + + public void initDirection() { + switch (currentDirection) + { + case FACING_TOP: + locationOnRobot_x = -1; locationOnRobot_y = 0; + break; + case FACING_TOPRIGHT: + locationOnRobot_x = -1; locationOnRobot_y = 1; + break; + case FACING_RIGHT: + locationOnRobot_x = 1; locationOnRobot_y = -1; + break; + case FACING_BOTTOMRIGHT: + locationOnRobot_x = 1; locationOnRobot_y = 1; + break; + case FACING_DOWN: + locationOnRobot_x = 1; locationOnRobot_y = 0; + break; + case FACING_BOTTOMLEFT: + locationOnRobot_x = 1; locationOnRobot_y = -1; + break; + case FACING_LEFT: + locationOnRobot_x = 0; locationOnRobot_y = -1; + break; + case FACING_TOPLEFT: + locationOnRobot_x = -1; locationOnRobot_y = -1; + break; + } + + + + } + + public void ChangeDirectionLeft() { + switch (currentDirection) + { + case FACING_TOP: + currentDirection = currentDirection.FACING_LEFT; + break; + case FACING_TOPRIGHT: + currentDirection = currentDirection.FACING_TOPLEFT; + break; + case FACING_RIGHT: + currentDirection = currentDirection.FACING_TOP; + break; + case FACING_BOTTOMRIGHT: + currentDirection = currentDirection.FACING_TOPRIGHT; + break; + case FACING_DOWN: + currentDirection = currentDirection.FACING_RIGHT; + break; + case FACING_BOTTOMLEFT: + currentDirection = currentDirection.FACING_BOTTOMRIGHT; + break; + case FACING_LEFT: + currentDirection = currentDirection.FACING_DOWN; + break; + case FACING_TOPLEFT: + currentDirection = currentDirection.FACING_BOTTOMLEFT; + break; + } + + if(locationOnRobot_x == 1 && locationOnRobot_y == -1) + { + locationOnRobot_x = -1; + locationOnRobot_y = -1; + } + else if(locationOnRobot_x == 1 && locationOnRobot_y == 0) + { + locationOnRobot_x = 0; + locationOnRobot_y = -1; + } + else if(locationOnRobot_x == 1 && locationOnRobot_y == 1) + { + locationOnRobot_x = 1; + locationOnRobot_y = -1; + } + else if(locationOnRobot_x == 0 && locationOnRobot_y == 1) + { + locationOnRobot_x = 1; + locationOnRobot_y = 0; + } + else if(locationOnRobot_x == -1 && locationOnRobot_y == 1) + { + locationOnRobot_x = 1; + locationOnRobot_y = 1; + } + else if(locationOnRobot_x == -1 && locationOnRobot_y == 0) + { + locationOnRobot_x = 0; + locationOnRobot_y = 1; + } + else if(locationOnRobot_x == -1 && locationOnRobot_y == -1) + { + locationOnRobot_x = -1; + locationOnRobot_y = 1; + } + else if(locationOnRobot_x == 0 && locationOnRobot_y == -1) + { + locationOnRobot_x = -1; + locationOnRobot_y = 0; + } + } + + public void ChangeDirectionRight() { + + + switch (currentDirection) + { + case FACING_TOP: + currentDirection = currentDirection.FACING_RIGHT; + break; + case FACING_TOPRIGHT: + currentDirection = currentDirection.FACING_BOTTOMRIGHT; + break; + case FACING_RIGHT: + currentDirection = currentDirection.FACING_DOWN; + break; + case FACING_BOTTOMRIGHT: + currentDirection = currentDirection.FACING_BOTTOMLEFT; + break; + case FACING_DOWN: + currentDirection = currentDirection.FACING_LEFT; + break; + case FACING_BOTTOMLEFT: + currentDirection = currentDirection.FACING_TOPLEFT; + break; + case FACING_LEFT: + currentDirection = currentDirection.FACING_TOP; + break; + case FACING_TOPLEFT: + currentDirection = currentDirection.FACING_TOPRIGHT; + break; + } + + if(locationOnRobot_x == 1 && locationOnRobot_y == -1) + { + locationOnRobot_x = 1; + locationOnRobot_y = 1; + } + else if(locationOnRobot_x == 1 && locationOnRobot_y == 0) + { + locationOnRobot_x = 0; + locationOnRobot_y = 1; + } + else if(locationOnRobot_x == 1 && locationOnRobot_y == 1) + { + locationOnRobot_x = -1; + locationOnRobot_y = 1; + } + else if(locationOnRobot_x == 0 && locationOnRobot_y == 1) + { + locationOnRobot_x = -1; + locationOnRobot_y = 0; + } + else if(locationOnRobot_x == -1 && locationOnRobot_y == 1) + { + locationOnRobot_x = -1; + locationOnRobot_y = -1; + } + else if(locationOnRobot_x == -1 && locationOnRobot_y == 0) + { + locationOnRobot_x = 0; + locationOnRobot_y = -1; + } + else if(locationOnRobot_x == -1 && locationOnRobot_y == -1) + { + locationOnRobot_x = 1; + locationOnRobot_y = -1; + } + else if(locationOnRobot_x == 0 && locationOnRobot_y == -1) + { + locationOnRobot_x = 1; + locationOnRobot_y = 0; + } + + } + + public void updateRobotLocation(int x, int y) { + robot_x = x; + robot_y = y; + } + + public boolean SenseLocation(Map map, int x, int y, int distanceFromRobot){ + boolean hitWall = false; + + int score = 0; + + if(distanceFromRobot == 1) + score = -70; + else if(distanceFromRobot == 2) + score = -40; + else if(distanceFromRobot == 3) + score = -8; + else if(distanceFromRobot == 4) + score = -5; + else if(distanceFromRobot == 5) + score = -2; + else if(distanceFromRobot == 6) + score = -2; + else if(distanceFromRobot == 7) + score = -2; + else if(distanceFromRobot == 8) + score = -2; + else if(distanceFromRobot == 9) + score = -2; + + + if(x < WIDTH && y < HEIGHT && x >= 0 && y >= 0) + { + //flip the score to positive to indicate that it is a block + if(map.SimulatedmapArray[y][x] == ExplorationTypes.toInt("OBSTACLE") || + map.SimulatedmapArray[y][x] == ExplorationTypes.toInt("UNEXPLORED_OBSTACLE")) + { + score = -score; + hitWall = true; + System.out.print(" X = " + x + " Y + " + y + " score \n"); + } + map.setMapScore(x,y,score); + } + else + hitWall=true; + + return hitWall; + } + + + public boolean Sense(Map map, int data, int[][] notWorkinghe){ + //have to make sure does not overshort boundary of environment + + int nextLocationX = 0; + int nextLocationY = 0; + + //is true after robot hits a wall, to prevent it from sensing further + boolean hitWall = false; + boolean hitWallret = false; + + for(int i = 1; i < range+1; i++) { + + //if(hitWall) + // break; + + //make sure it is in the map range and bound. + if(currentDirection ==SensorLocation.FACING_RIGHT) { + nextLocationX = robot_x+locationOnRobot_x+i; + nextLocationY = robot_y+locationOnRobot_y; + } + + else if(currentDirection == SensorLocation.FACING_LEFT) { + nextLocationX = robot_x+locationOnRobot_x-i; + nextLocationY = robot_y+locationOnRobot_y; + } + else if(currentDirection == SensorLocation.FACING_TOP) { + nextLocationX = robot_x+locationOnRobot_x; + nextLocationY = robot_y+locationOnRobot_y-i; + } + else{ + //System.out.print("should be sensing right wall\n"); + nextLocationX = robot_x+locationOnRobot_x; + nextLocationY = robot_y+locationOnRobot_y+i; + } + + //hitwill will be true when senselocation returns a true which indicates that a wall has been met + if(!hitWall){ + // System.out.println("next X: "+nextLocationX+"\tnext Y: "+nextLocationY); + hitWall = SenseLocation(map,nextLocationX, nextLocationY, i); + if(SenseLocation(map, nextLocationX, nextLocationY, 0) && i==1) + hitWallret =true; + } + //send a 0 to signify that this is behind a wall + else + SenseLocation(map,nextLocationX, nextLocationY, 0); + } + //hitWallret=SenseLocation(map,nextLocationX, nextLocationY, 0); + + //update the map score after "sensing" + map.updateMapWithScore(); + return hitWallret; + } + + public boolean SenseRight(Map map, int data, int[][] notWorkinghe){ + //have to make sure does not overshort boundary of environment + + int nextLocationX = 0; + int nextLocationY = 0; + + //is true after robot hits a wall, to prevent it from sensing further + boolean hitWall = false; + boolean hitWallret = false; + + for(int i = 1; i < range+1; i++) { + + //if(hitWall) + // break; + + //make sure it is in the map range and bound. + if(currentDirection ==SensorLocation.FACING_RIGHT) { + nextLocationX = robot_x+locationOnRobot_x+i; + nextLocationY = robot_y+locationOnRobot_y; + } + + else if(currentDirection == SensorLocation.FACING_LEFT) { + nextLocationX = robot_x+locationOnRobot_x-i; + nextLocationY = robot_y+locationOnRobot_y; + } + else if(currentDirection == SensorLocation.FACING_TOP) { + nextLocationX = robot_x+locationOnRobot_x; + nextLocationY = robot_y+locationOnRobot_y-i; + } + else{ + //System.out.print("should be sensing right wall\n"); + nextLocationX = robot_x+locationOnRobot_x; + nextLocationY = robot_y+locationOnRobot_y+i; + } + + //hitwill will be true when senselocation returns a true which indicates that a wall has been met + if(!hitWall){ + hitWall = SenseLocation(map,nextLocationX, nextLocationY, i); + if(SenseLocation(map, nextLocationX, nextLocationY, 0) && i==2) + hitWallret = true; + } + //send a 0 to signify that this is behind a wall + else + SenseLocation(map,nextLocationX, nextLocationY, 0); + + + } + //hitWallret=SenseLocation(map,nextLocationX, nextLocationY, 0); + + //update the map score after "sensing" + map.updateMapWithScore(); + return hitWallret; + } + +} \ No newline at end of file diff --git a/src/algorithms/SensorLocation.java b/src/algorithms/SensorLocation.java new file mode 100644 index 0000000..3a0f49b --- /dev/null +++ b/src/algorithms/SensorLocation.java @@ -0,0 +1,28 @@ +package algorithms; + +public enum SensorLocation{ + FACING_TOP(0), + FACING_TOPRIGHT(1), + FACING_RIGHT(2), + FACING_BOTTOMRIGHT(3), + FACING_DOWN(4), + FACING_BOTTOMLEFT(5), + FACING_LEFT(6), + FACING_TOPLEFT(7); + + int type_val = 0; + private SensorLocation(int type_val) + { + this.type_val = type_val; + } + + public int getValue() + { + return this.type_val; + } + + public void setValue(int update) { + type_val = update; + } + +}; \ No newline at end of file diff --git a/src/algorithms/SocketClient.java b/src/algorithms/SocketClient.java new file mode 100644 index 0000000..a5d6be1 --- /dev/null +++ b/src/algorithms/SocketClient.java @@ -0,0 +1,123 @@ +package algorithms; + +import java.io.DataInputStream; +import java.io.IOException; +import java.io.PrintStream; +import java.net.InetAddress; +import java.net.InetSocketAddress; +import java.net.Socket; +import java.net.UnknownHostException; +import java.util.TimerTask; + +public class SocketClient{ + InetAddress host; + Socket socket = null; + DataInputStream input = null; + PrintStream out = null; + String IP_Addr; + int Port; + boolean firstflag = false; + public SocketClient(String IP_Addr, int Port){ + this.IP_Addr = IP_Addr; + this.Port = Port; + } + + public boolean connectToDevice() { + int timeout = 6000; + try + { + if(socket != null) { + if(socket.isConnected()) { + return true; + } + } + InetSocketAddress ISA = new InetSocketAddress(IP_Addr, Port); + socket = new Socket(); + socket.connect(ISA, timeout); + System.out.println("Connected"); + // takes input from terminal + + input = new DataInputStream(socket.getInputStream()); + // sends output to the socket + out = new PrintStream(socket.getOutputStream()); + return true; + } + catch(UnknownHostException u) + { + System.out.println(u); + } + catch(IOException i) + { + System.out.println(i); + } + return false; + + } + + + //sends packet data to rpi to be relayed or processed + public int sendPacket(String packetData){ + try { + System.out.println("Sending packetData..."); + System.out.println(packetData); + out.print(packetData); + System.out.println("Packet sent."); + out.flush(); + return 0; + + }catch(Exception e) { + // TODO Auto-generated catch block + System.out.println("Sending Error: " + e); + e.printStackTrace(); + while(socket.isClosed()) { + System.out.println("socket is not connected... reconnecting.."); + connectToDevice(); + } + System.out.println("resending packet."); + sendPacket(packetData); + + } + return 0; + } + + public String receivePacket(boolean resentflag, String Data){ + String instruction = null; + try { + + //need to rethink this. + do { + + long timestart = System.currentTimeMillis(); + while(input.available() == 0) { + Thread.sleep(10); + } + instruction = input.readLine(); + }while(instruction == null ||instruction.equalsIgnoreCase("")); + } + + + catch (IOException e) { + // TODO Auto-generated catch block + System.out.println("Receiving Error: " + e); + e.printStackTrace(); + connectToDevice(); + } catch (InterruptedException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + + return instruction; + } + + public void closeConnection() { + try { + socket.close(); + } catch (IOException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + } + + + +} \ No newline at end of file diff --git a/src/algorithms/Visualization.java b/src/algorithms/Visualization.java new file mode 100644 index 0000000..037a9ae --- /dev/null +++ b/src/algorithms/Visualization.java @@ -0,0 +1,51 @@ +package algorithms; + +import javax.swing.*; +import javax.swing.Timer; +import java.awt.*; +import java.awt.event.ActionEvent; +import java.awt.event.ActionListener; +import java.util.*; + + +public class Visualization extends JComponent{ + /** + * + */ + private static final long serialVersionUID = 1L; + + public RobotInterface robot; + + + public RobotInterface getRobot() { + return robot; + } + + public void setRobot(RobotInterface robot) { + this.robot = robot; + } + + + public Visualization(){ + } + + + + + public Visualization(RobotInterface robot){ + this.robot = robot; + } + + + protected void paintComponent(Graphics g){ + robot.map.paintMap(g); + robot.paintRobot(g); + PathDrawer.drawGrid(g); + PathDrawer.drawPath(g); + super.paintComponent(g); + + } + + + +} \ No newline at end of file diff --git a/src/algorithms/exploration1.java b/src/algorithms/exploration1.java new file mode 100644 index 0000000..089c301 --- /dev/null +++ b/src/algorithms/exploration1.java @@ -0,0 +1,1017 @@ +package algorithms; + +import java.util.Stack; + + +public class exploration1 { + private static final int listOfActionsSize = 4; + + public enum Action + { + NO_ACTION, + TURN_LEFT, + TURN_RIGHT, + TURN_UP, + TURN_DOWN, + MOVE_FORWARD, + } + + public enum ExplorationState + { + INITIAL_EXPLORATION, + CLEARING_UNKNOWN, + } + ExplorationState state; + ////////////////////////////////////////import variable!!!//////////////////////////////////////////// + boolean exploreUnexplored = true; + ////////////////////////////////////////////////////////////////////////////////////////////////////// + + + Visualization viz; + SocketClient sc=null; + boolean simulator; + RobotInterface robot; + Map map; + + //to check whats the previous move that the robot did + Facing previousFacing; + + //to store a list of actions for the robot to take + Action listOfActions[]; + int actionsIterator; + + //stack to store the previous steps taking(for trace back) + Stack traceBackSteps; + + //stack to store the previous steps taking(for trace back) + Stack traceBackFacing; + + //track num of steps to backtrack + int stepsToBacktrack; + boolean backTracking; + + //track the number of consecutive moveForward() and call the robot to calibrate when a certain number is reached + int numTimesMoveForward; + int timeToSideCalibrate; + + //the starting position of the robot + int startX; + int startY; + + //bool to check weather the robot has moved, to prevent the exploration ending immediately cause since + //robot starts at the start position and i use it to check if exploration is done + boolean robotMoved; + + //prevent robot from front calibrating twice + boolean hasJustFrontCalibrated; + //the stack to contain the unexplored areas of the map, arrays will contain {x,y} + Stack unexploredAreas; + + //the Astar class to the next unexplored area + Astar pathToNextUnexploredArea; + + //get the next unexplored area to go to, coordinates will be {x of grid,y of grid, x of grid that i will go to "look" at target block, y of looking grid} + int[] nextUnexploredArea; + + //true when the robot is currently on its way to an unexplored area + boolean goingToBlock; + + boolean hasUnexplored=false; + + //the values of the offset for the block + Stack storedOffsetValues; + + //number of steps per second(user selected) + float stepsPerSecond; + boolean goingBackToStart = false; + + //the %that map has to be explored before to terminate + float percentageToStop; + + //the time that the program has to run before terminating + int timeToStop; + long timeSinceLastUpdate; + long timeSinceStart; + long timeLastupdate; + + int minute; + int second; + public exploration1(SocketClient sc, boolean simulator, RobotInterface robot,Visualization viz, Map inMap) { + this.simulator = simulator; + if(simulator) + this.sc = sc; //can be null if simulation + this.robot = robot; + this.map = inMap; + this.viz = viz; + + state = ExplorationState.INITIAL_EXPLORATION; + + //init the list to all have "NO.ACTION" + listOfActions = new Action[listOfActionsSize]; + for(int i = 0 ; i < listOfActionsSize; i++) + listOfActions[i] = Action.NO_ACTION; + + actionsIterator = -1; + + traceBackSteps = new Stack(); + traceBackFacing = new Stack(); + + stepsToBacktrack = 0; + backTracking = false; + + + numTimesMoveForward = 0; + timeToSideCalibrate = 3; + + int timeToTurnRight = 3; + int timeToTurnLeft = 3; + //init to false to prevent exploration phase ending immediately + robotMoved = false; + + //init to false to prevent robot from calibrating twice in a row + hasJustFrontCalibrated = false; + + //init the stack + unexploredAreas = new Stack(); + + //the values stored for the checking of offset + storedOffsetValues = new Stack(); + initStoredOffsetValues(); + + + goingToBlock = false; + + //variables to control the flow of exploration, mainly for checklist + + stepsPerSecond = 30f; + + + //% of map explored before stopping + + //time to stop simulationaa + minute = 20; + second = 5; + + timeToStop = minute*60000 +second*1000; + + timeSinceLastUpdate = 0; + timeSinceStart = 0; + timeLastupdate = System.currentTimeMillis(); + } + public void initStoredOffsetValues() + { + + int[] offset; + + //down side + offset = new int[] {-1,1}; + storedOffsetValues.push(offset); + offset = new int[] {0,1}; + storedOffsetValues.push(offset); + offset = new int[] {1,1}; + storedOffsetValues.push(offset); + + //right side + offset = new int[] {1,1}; + storedOffsetValues.push(offset); + offset = new int[] {1,0}; + storedOffsetValues.push(offset); + offset = new int[] {1,-1}; + storedOffsetValues.push(offset); + + //up side + offset = new int[] {1,-1}; + storedOffsetValues.push(offset); + offset = new int[] {0,-1}; + storedOffsetValues.push(offset); + offset = new int[] {-1,-1}; + storedOffsetValues.push(offset); + + //left side + offset = new int[] {-1,-1}; + storedOffsetValues.push(offset); + offset = new int[] {-1,0}; + storedOffsetValues.push(offset); + offset = new int[] {-1,1}; + storedOffsetValues.push(offset); + + //down side + offset = new int[] {-1,2}; + storedOffsetValues.push(offset); + offset = new int[] {0,2}; + storedOffsetValues.push(offset); + offset = new int[] {1,2}; + storedOffsetValues.push(offset); + + //right side + offset = new int[] {2,1}; + storedOffsetValues.push(offset); + offset = new int[] {2,0}; + storedOffsetValues.push(offset); + offset = new int[] {2,-1}; + storedOffsetValues.push(offset); + + //up side + offset = new int[] {1,-2}; + storedOffsetValues.push(offset); + offset = new int[] {0,-2}; + storedOffsetValues.push(offset); + offset = new int[] {-1,-2}; + storedOffsetValues.push(offset); + + //left side + offset = new int[] {-2,-1}; + storedOffsetValues.push(offset); + offset = new int[] {-2,0}; + storedOffsetValues.push(offset); + offset = new int[] {-2,1}; + storedOffsetValues.push(offset); + + //down side + offset = new int[] {-1,3}; + storedOffsetValues.push(offset); + offset = new int[] {0,3}; + storedOffsetValues.push(offset); + offset = new int[] {1,3}; + storedOffsetValues.push(offset); + + //right side + offset = new int[] {3,1}; + storedOffsetValues.push(offset); + offset = new int[] {3,0}; + storedOffsetValues.push(offset); + offset = new int[] {3,-1}; + storedOffsetValues.push(offset); + + //up side + offset = new int[] {1,-3}; + storedOffsetValues.push(offset); + offset = new int[] {0,-3}; + storedOffsetValues.push(offset); + offset = new int[] {-1,-3}; + storedOffsetValues.push(offset); + + //left side + offset = new int[] {-3,-1}; + storedOffsetValues.push(offset); + offset = new int[] {-3,0}; + storedOffsetValues.push(offset); + offset = new int[] {-3,1}; + storedOffsetValues.push(offset); + } + public void initStartPoint(int x, int y) + { + startX = x; + startY = y; + } + int getMapExplored() + { + //counts the number of map that is explored, aka is not UNDISCOVERED + int explored = 0; + for (int i = 0; i < Map.HEIGHT; i++) + for (int j = 0; j < Map.WIDTH; j++) + if (map.getMapArray()[i][j] == ExplorationTypes.toInt("EMPTY") || map.getMapArray()[i][j] == ExplorationTypes.toInt("OBSTACLE")) + explored++; + + return explored; + } + int[] getNearestCoordinateToUnExploredArea(int x, int y) + { + int[] mostEfficientPath = null; + if(x == 4 && y == 10) + mostEfficientPath = null; + + for(int i = 0; i < storedOffsetValues.size(); i++) + { + //if the robot can move to that spot, then calculate the cost and store it + if(robot.canRobotMoveHere(x + storedOffsetValues.get(i)[0], y + storedOffsetValues.get(i)[1])) + { + return new int[] {x, y, x + storedOffsetValues.get(i)[0], y + + storedOffsetValues.get(i)[1]}; + } + } + + //if a path was found, return the path + //if(mostEfficientPath != null) + // return mostEfficientPath; + + + //return {-1,-1} to singify an error + //if {-1,-1} is returned, it means that the block cannot be explored at first, need to explore other path first + return new int[] {-1,-1,-1,-1}; + } + boolean ismapCoordinateInStack(int[] mapCoordinate) + { + //temp variable + int[] unexplored; + for(int i = 0; i < unexploredAreas.size(); i++) + { + + unexplored = unexploredAreas.get(i); + + if(unexplored[0] == mapCoordinate[0] && + unexplored[1] == mapCoordinate[1] && + unexplored[2] == mapCoordinate[2] && + unexplored[3] == mapCoordinate[3]) + return true; + } + return false; + } + void inputAllUnexploredAreas() + { + //get the map array to work on + int[][] mapArray = map.getMapArray(); + + int[] mapCoordinate; + for(int y = 0; y < Map.HEIGHT; y++) + { + for (int x = 0; x < Map.WIDTH; x++) + { + if(mapArray[y][x] == ExplorationTypes.toInt("UNEXPLORED_EMPTY") || mapArray[y][x] == ExplorationTypes.toInt("UNEXPLORED_OBSTACLE")) + { + //need to input an area that the robot has explored and is next to the unexplored area, and that the robot can "fit/go" there + mapCoordinate = getNearestCoordinateToUnExploredArea(x,y); + + //if the values are -1, do not push into the stack + //or if the values are inside the stack already, dont push too + if(mapCoordinate[0] != -1 && mapCoordinate[1] != -1 && !ismapCoordinateInStack(mapCoordinate)) + unexploredAreas.push(mapCoordinate); + } + } + } + + } + void updateUnexploredAreas() + { + //get the map array to work on + int[][] mapArray = map.getMapArray(); + + //run thru the whole list to remove those that were explored halfway + for(int i = 0; i < unexploredAreas.size(); i++) + { + //if the area is now not a "unexplored", means it was explored on the way, then remove it from the stack + if(mapArray[unexploredAreas.get(i)[1]][unexploredAreas.get(i)[0]] == ExplorationTypes.toInt("EMPTY") || + mapArray[unexploredAreas.get(i)[1]][unexploredAreas.get(i)[0]] == ExplorationTypes.toInt("OBSTACLE")) + { + //System.out.print("removed" + unexploredAreas.get(i)[0] + " , " + unexploredAreas.get(i)[1] + " with index " + i + "\n"); + unexploredAreas.remove(i); + i--; + } + } + + //input any new areas that becomes available to explore + inputAllUnexploredAreas(); + } + public boolean haveObstaclesAroundRobot(int tempX, int tempY) + { + + if(robot.isAbleToMove(Direction.UP, tempX, tempY) || robot.isAbleToMove(Direction.DOWN, tempX, tempY) || + robot.isAbleToMove(Direction.LEFT, tempX, tempY) || robot.isAbleToMove(Direction.RIGHT, tempX, tempY)) + { + System.out.print("found a block!\n"); + return true; + } + + return false; + } + public int calculateNumBackTrackSteps() + { + int numSteps = 0; + + int iterator = traceBackSteps.size()-1; + + Action curAction; + int tempX = robot.x; + int tempY = robot.y; + Direction tempFacing = robot.facing; + + while(iterator >= 0) + { + curAction = traceBackSteps.get(iterator); + + switch(curAction) + { + //go backwards + case MOVE_FORWARD: + int movementDistance = 1; + + if(tempFacing == Direction.UP) + tempY += movementDistance; + else if(tempFacing == Direction.DOWN) + tempY -= movementDistance; + else if(tempFacing == Direction.RIGHT) + tempX -= movementDistance; + else if(tempFacing == Direction.LEFT) + tempX += movementDistance; + + numSteps++; + + //if theres a obstacle now, then break out and return the steps + if(haveObstaclesAroundRobot(tempX, tempY)) + return numSteps; + break; + case TURN_LEFT: + tempFacing = robot.simulateTurnRight(tempFacing); + + numSteps++; + break; + case TURN_RIGHT: + tempFacing = robot.simulateTurnLeft(tempFacing); + + numSteps++; + break; + default: + break; + + } + + iterator--; + } + + //return the number of steps to take before being next to a wall to continue the algo + return numSteps; + } + void doStoredActions() + { + //robot will do the actions that have been stored here instead of doing computation in StartExploration() + switch (listOfActions[actionsIterator]) + { + case TURN_LEFT: + //System.out.print("turning left at the stored actions\n"); + robot.turnLeft(); + System.out.print("Robot turn left\n"); + traceBackSteps.push(Action.TURN_LEFT); + break; + + case TURN_RIGHT: + robot.turnRight(); + System.out.print("Robot turn right\n"); + traceBackSteps.push(Action.TURN_RIGHT); + break; + + case MOVE_FORWARD: + //DoIEMoveForward(); + robot.moveRobot(); + System.out.print("Robot move forward\n"); + traceBackSteps.push(Action.MOVE_FORWARD); + numTimesMoveForward++; + break; + default: + break; + } + + actionsIterator -= 1; + } + public void DoIETurnRight() + { + System.out.print("Robot turn right\n"); + hasJustFrontCalibrated = false; + actionsIterator = 0; + //Check if can front calibrate and can left calibrate(has 3 blocks) + if(robot.canFront_Calibrate()&&robot.canLeft_Calibrate()&&!hasJustFrontCalibrated){ + robot.front_Calibrate(); + hasJustFrontCalibrated = true; + numTimesMoveForward=0; + } + //Check is can only left calibrate after a certain number of steps + else if(robot.canLeft_Calibrate()&&!hasJustFrontCalibrated){ + robot.left_Calibrate(); + hasJustFrontCalibrated = true; + numTimesMoveForward=0; + } + //Turn right after all calibrations are done + robot.turnRight(); + System.out.print("Robot turn right\n"); + traceBackSteps.push(Action.TURN_RIGHT); + + //listOfActions[1] = Action.TURN_RIGHT; + listOfActions[0] = Action.MOVE_FORWARD; + + } + public void DoIEMoveForward(Facing facing) + { + hasJustFrontCalibrated = false; + boolean hasCalibrated = false; + System.out.print("Robot move forward\n"); + + //once timeToSideCalibrate + if(numTimesMoveForward >= timeToSideCalibrate) + { + //check if the robot side sensors have the blocks next to them to + if(robot.canSide_Calibrate()) + { + System.out.print("align right\n"); + robot.side_Calibrate(); + + //reset the counter + numTimesMoveForward = 0; + hasCalibrated = true; + } + else if (robot.canLeft_Calibrate()){ + System.out.println("align left\n"); + robot.left_Calibrate(); + numTimesMoveForward = 0; + hasCalibrated = true; + } + } + + if(!hasCalibrated) + { + //simply move forward + robot.moveRobot(); + previousFacing = facing; + traceBackFacing.push(facing); + robotMoved = true; + numTimesMoveForward++; + + traceBackSteps.push(Action.MOVE_FORWARD); + } + + } + public void DoIETurnLeft() + { + actionsIterator = 0; + listOfActions[0] = Action.TURN_LEFT; + System.out.print("Robot turn left\n"); + + //check if the front can be calibrated(if theres 3 blocks right infront of the robot + if(robot.canFront_Calibrate() && robot.canSide_Calibrate() && !hasJustFrontCalibrated) + { + System.out.print("align front\n"); + robot.front_Calibrate(); + hasJustFrontCalibrated = true; + //reset the counter + numTimesMoveForward = 0; + } + else if(robot.canSide_Calibrate() && !hasJustFrontCalibrated) + { + System.out.print("align right\n"); + robot.side_Calibrate(); + hasJustFrontCalibrated = true; + numTimesMoveForward = 0; + } + + } + public void DoIEBackTrack() + { + if(!backTracking) + { + backTracking = true; + stepsToBacktrack = calculateNumBackTrackSteps(); + } + + + System.out.print("doing trace back\n"); + if(traceBackSteps.empty()) + { + System.out.print("trace back step is empty\n"); + return; + } + Action prevAction = traceBackSteps.pop(); + + switch(prevAction) + { + case TURN_LEFT: + robot.turnRight(); + break; + + case TURN_RIGHT: + robot.turnLeft(); + break; + + case MOVE_FORWARD: + robot.reverse(); + break; + default: + System.out.print("back track error\n"); + break; + } + + stepsToBacktrack--; + + if(stepsToBacktrack <= 0) + backTracking = false; + } + + // returns 1 for true + // returns 0 for false + // returns -1 for reset + public int DoInitialExploration() + { + //make sure there isnt a stored action before continuing, if have den do stored actions + if (actionsIterator != -1) + { + System.out.print("doing stored actions\n"); + //System.out.print("doing stored actions\n"); + doStoredActions(); +// return false; + return 0; + } + + //when backtracking, do not do other actions + if(backTracking) + { + DoIEBackTrack(); +// return false; + return 0; + } + + switch (robot.facing) + { + case LEFT: + //if can move up and it was facing left previously, execute stored actions + if (robot.isAbleToMove(Direction.UP) && previousFacing == Facing.LEFT) + DoIETurnRight(); + + //if above is a wall and left is clear, move left + else if (!robot.isAbleToMove(Direction.UP) && robot.isAbleToMove(Direction.LEFT)) + DoIEMoveForward(Facing.LEFT); + + //if cannot move up or left, turn left to face down + else if(!robot.isAbleToMove(Direction.UP) && !robot.isAbleToMove(Direction.LEFT)) + DoIETurnLeft(); + + //no wall next to robot, do back track + else + DoIEBackTrack(); + + break; + + case RIGHT: + //if can move down and it was facing right previously, execute stored actions + if (robot.isAbleToMove(Direction.DOWN) && previousFacing == Facing.RIGHT) + DoIETurnRight(); + + //if down is a wall and right is clear, move right + else if (!robot.isAbleToMove(Direction.DOWN) && robot.isAbleToMove(Direction.RIGHT)) + DoIEMoveForward(Facing.RIGHT); + + //if cannot move down or right, turn left to face up + else if (!robot.isAbleToMove(Direction.DOWN) && !robot.isAbleToMove(Direction.RIGHT)) + DoIETurnLeft(); + + //no wall next to robot, do trace back + else + DoIEBackTrack(); + break; + + case UP: + //if can move right and it was facing up previously, execute stored actions + if (robot.isAbleToMove(Direction.RIGHT) && previousFacing == Facing.UP) + DoIETurnRight(); + + //if right is a wall and up is clear, move up + else if (!robot.isAbleToMove(Direction.RIGHT) && robot.isAbleToMove(Direction.UP)) + DoIEMoveForward(Facing.UP); + + //if cannot move right or up, turn left to face left + else if (!robot.isAbleToMove(Direction.RIGHT) && !robot.isAbleToMove(Direction.UP)) + DoIETurnLeft(); + + //no wall next to robot, do trace back + else + DoIEBackTrack(); + + break; + + case DOWN: + if (robot.isAbleToMove(Direction.LEFT) && previousFacing == Facing.DOWN) + DoIETurnRight(); + + //if left is a wall and down is clear, move right + else if (!robot.isAbleToMove(Direction.LEFT) && robot.isAbleToMove(Direction.DOWN)) + DoIEMoveForward(Facing.DOWN); + + else if (!robot.isAbleToMove(Direction.LEFT) && !robot.isAbleToMove(Direction.DOWN)) + DoIETurnLeft(); + + //no wall next to robot, do trace back + else + DoIEBackTrack(); + + break; + + default: +// return false; + return 0; + } + + /* + debugging stuff + + System.out.print(traceBackSteps.size() + "\n"); + + if(traceBackSteps.size() > 75) + { + //if(map.mapScoreArray[13][12] > 22) + map.turnoffgrid = true; + map.turnoffgrid3 = true; + map.turnoffgrid2 = true; + map.updateMapWithScore(); + }*/ + + // TODO: @JARRETT added new method to check if robot has asked for reset + // If yes, then create new Robot + if (robot.getWantToReset()) { + System.out.println("JARRETT: ROBOT WANTS TO RESET"); + return -1; + } + + //once the robot moves, check if its at the start position to end the exploration + if(robotMoved && robot.getX() == startX && robot.getY() == startY) + { + System.out.println("finished exploration"); +// return true; + return 1; + } +// return false; + return 0; + } + + //optimized to reduce the number of turns the robot takes during clearing unknown. + public boolean DoClearingUnknown() + { + System.out.println("doing clear unknown"); + //while(true) { + //if the robot is going to a block, iterate step by step + if(goingToBlock) + { + System.out.println("goingToBlock"); + //if function returns true, means it has reached its current destination(an unexplored area) + + if(robot.doStepFastestPath()) + { + System.out.println("robot.doStepFastestPath()"); + //make sure the robot is facing the unexplored area before finishing the current path + //if(robot.isFacingArea(nextUnexploredArea[0], nextUnexploredArea[1])) + goingToBlock = false; + } + + } + else + { + System.out.println("else"); + //if the array is empty then check if robot is back at start position, if not then go back start + if(unexploredAreas.empty()) + { + System.out.println("unexploredAreas.empty()\n++++++++++++++++++++++++++++++++++"); + System.out.println("unexploredAreas.empty()"); + if(robot.x == startX && robot.y == startY) + { + System.out.println("robot.x == startX && robot.y == startY"); + if(goingBackToStart)PathDrawer.removePath(); + adjustMapForFastestPath(); + return true; + } + //if the grid is not fully explored, then continue exploring + else if (getMapExplored() <290)//!= map.HEIGHT*map.WIDTH) + { + System.out.print("checking if map got properly explored aka 90% of map"); + inputAllUnexploredAreas(); + goingBackToStart = false; + } + else + { + System.out.print("finished exploring part 2, going back to start"); + + pathToNextUnexploredArea = new Astar(map.getNodeXY(robot.x, robot.y), map.getNodeXY(startX, startY)); + + //send it to the robot to store the instruction + Stack fast = pathToNextUnexploredArea.getFastestPath(); + robot.setFastestInstruction(fast, startX, startY); + + //updates the pathdrawer for graphics + PathDrawer.update(robot.x, robot.y, pathToNextUnexploredArea.getFastestPath()); + goingBackToStart = true; + + goingToBlock = true; + } + } + else + { + hasUnexplored = true; + System.out.print("update map nodes for A star\n"); + //update map nodes for A star + map.updateMap(); + + //update the unexploredAreas stack, to clear the blocks that were discovered on the way to other blocks + updateUnexploredAreas(); + + int indexOfClosestArea = 0; + int costOfClosest = 999; + + if(!unexploredAreas.empty()) + { + System.out.print("unexploreaAreas.size = " + unexploredAreas.size() + "\n"); + for(int i = 0; i < unexploredAreas.size(); i++) + { + //put the x and y value of the unexplored area + //[0]=x value of unexplored area + //[1]=y value of unexplored area + //[2]=x value of coordinate of point next to unexplored area that the robot can access + //[3]=y value of coordinate of point next to unexplored area that the robot can access + System.out.println("unexploredAreas.get("+i+")[0]="+unexploredAreas.get(i)[0]); + System.out.println("unexploredAreas.get("+i+")[1]="+unexploredAreas.get(i)[1]); + System.out.println("unexploredAreas.get("+i+")[2]="+unexploredAreas.get(i)[2]); + System.out.println("unexploredAreas.get("+i+")[3]="+unexploredAreas.get(i)[3]); + pathToNextUnexploredArea = new Astar(map.getNodeXY(robot.x, robot.y), map.getNodeXY(unexploredAreas.get(i)[2], unexploredAreas.get(i)[3])); + pathToNextUnexploredArea.getFastestPath(); + int cost = pathToNextUnexploredArea.getCost(); + System.out.println("Costs:"+cost); + + //if the cost is lesser than the current lowest than update the indexOfClosestArea + if(cost < costOfClosest) + { + costOfClosest = cost; + indexOfClosestArea = i; + } + } + + pathToNextUnexploredArea = new Astar(map.getNodeXY(robot.x, robot.y), map.getNodeXY( + unexploredAreas.get(indexOfClosestArea)[2], + unexploredAreas.get(indexOfClosestArea)[3])); + + System.out.print("next area to explore " + unexploredAreas.get(indexOfClosestArea)[0] +" and "+ unexploredAreas.get(indexOfClosestArea)[1] + + " but going to " + unexploredAreas.get(indexOfClosestArea)[2] + " and " + unexploredAreas.get(indexOfClosestArea)[3] + '\n'); + + + nextUnexploredArea = unexploredAreas.remove(indexOfClosestArea); + + //send it to the robot to store the instruction + Stack fast = pathToNextUnexploredArea.getFastestPath(); + System.out.print("setting fastest instructions\n++++++++++++++++++++++++++++++"); + robot.setFastestInstruction(fast, nextUnexploredArea[0], nextUnexploredArea[1]); + + //updates the pathdrawer for graphics + PathDrawer.update(robot.x, robot.y, pathToNextUnexploredArea.getFastestPath()); + } + goingToBlock = true; + goingBackToStart = false; + } + } + return false; + //} + } + //returns 1 when robot reaches the start point + //returns 0 in place of false + //returns -1 if want to reset + public int DoSimulatorExploration() { + try { + while(true) { + + //////////////////////////////////variables for the control of exploration(checklist)/////////////////////////// + /*timeSinceLastUpdate = System.currentTimeMillis() - timeLastupdate; + timeSinceStart += timeSinceLastUpdate; + timeLastupdate = System.currentTimeMillis(); + if(timeSinceStart > timeToStop) + return true; + float percentageExplored = (float)getMapExplored()/(float)(Map.HEIGHT*Map.WIDTH); + System.out.print("Percentage Explored: "+percentageExplored+"\n"); + System.out.print("Percentage to stop: "+percentageToStop+"\n"); + if(percentageExplored >percentageToStop) + return true;*/ + //////////////////////////////////end of variables for the control of exploration(checklist)/////////////////////////// + + //only do sleeps when in simulation + if(simulator) + Thread.sleep((long) (1000/stepsPerSecond)); + + switch(state) + { + case INITIAL_EXPLORATION: + + + //once it reaches the start point, DoInitialExploration() will return 1. + int DoInitialExplorationResult = DoInitialExploration(); + if(DoInitialExplorationResult == 1) + { + robot.sendMapDescriptor(); + if(exploreUnexplored) { + System.out.println("Doing explore Unexplored\n\n\n\n\n"); + state = ExplorationState.CLEARING_UNKNOWN; + inputAllUnexploredAreas(); + break; + } + else{ + System.out.println("NOT!!! doing explore Unexplored\n\n\n\n\n"); + adjustMapForFastestPath(); + //map.updateMap(); + viz.repaint(); + return 1; + } + + } else if (DoInitialExplorationResult == -1) { + System.out.println("Reset ordered by robot!"); + return -1; + } + + map.updateMap(); + break; + + case CLEARING_UNKNOWN: + //System.out.println("doing clear unknown"); + //once it finishes clearing the map and returning to the start point, function will return true + //stepsPerSecond set to 2f for debugging purposes + stepsPerSecond = 2f; + if(DoClearingUnknown()) { + return 1; + } + map.updateMap(); + PathDrawer.updateUnexploredAreas(unexploredAreas); + + break; + } + + //update the graphics after each loop + viz.repaint(); + } + + } + catch(Exception e) + { + System.out.println(e); + System.out.println("error in exploration"); + } + + //return false by default +// return false; + return 0; + + } + + public void adjustMapForFastestPath() + { + //count how many obstacles there are, if there are above 30, remove the ones with the lowest score + //also change all the unexplored areas to explored with score of 1 + + //get the map array to work on + int[][] mapArray = map.getMapArray(); + + Stack obstacleAndScore = new Stack(); + + + for(int y = 0; y < Map.HEIGHT; y++) + { + for (int x = 0; x < Map.WIDTH; x++) + { + if(mapArray[y][x] == ExplorationTypes.toInt("OBSTACLE")) + { + Integer [] obstacle = new Integer[3]; + obstacle[0] = x; + obstacle[1] = y; + obstacle[2] = map.mapScoreArray[y][x]; + obstacleAndScore.push(obstacle); + } + + //make the unexplore areas explored + if(mapArray[y][x] == ExplorationTypes.toInt("UNEXPLORED_EMPTY") || mapArray[y][x] == ExplorationTypes.toInt("UNEXPLORED_OBSTACLE")) + { + map.mapScoreArray[y][x] = -1; + } + } + } + + int lowestScoreIndex = 0; + int lowestScore = 999; + + //remove the obstacles if there are more than 30 blocks + while(obstacleAndScore.size() > 30) + { + lowestScoreIndex = 0; + lowestScore = 999; + //iterate the list to find out the one with the lowest score + for(int i = 0; i < obstacleAndScore.size(); i++) + { + //if the current score is less than the recorded lowestScore, then replace it + if(obstacleAndScore.get(i)[2] < lowestScore) + { + lowestScoreIndex = i; + lowestScore = obstacleAndScore.get(i)[2]; + } + } + + //set the obstacle with the lowest score to a empty block + map.mapScoreArray[obstacleAndScore.get(lowestScoreIndex)[1]][obstacleAndScore.get(lowestScoreIndex)[0]] = -1; + //remove the obstacle with the lowest score + obstacleAndScore.remove(lowestScoreIndex); + } + + //update the score map + map.updateMapWithScore(); + for(int i=0;i