import josx.platform.rcx.*; import josx.robotics.*; import josx.util.*; import java.lang.System; import josx.rcxcomm.RCXPort; import java.io.InputStream; import java.io.OutputStream; import java.io.IOException; /* + Thesis Design Project + RCX Robot Code + George A. Papayiannis + +------------------------------------------------------------ + Start Revision History (final_rcx.java) +------------------------------------------------------------ + + 1. + George A. Papayiannis (G.A.P.) + 25/02/2006 + @ v 1.0 + Most of the code is here, but lots of tweaking is needed. + This version does not have Paul's code to transfer commands + from the UI. This will be added later. + + 2. + G.A.P. + 27/02/2006 + @ v. 1.1 + Solid update to keep straight algo. Its now amazing, the robot + stays centered perfectly. A few more changes here and there + definitly not the last. + + 3. + G.A.P + 28/02/2006 + @ v. 1.2 + A lot of changes to the sensors + *** Sensor 1 *** + --- Rotation sensor @ 1 + *** Sensor 2 *** + --- Left light sensor @ 2 + --- Front touch 1 sensor @ 2 + --- Back touch 1 sensor @ 2 + *** Sensor 3 *** + --- Right light sensor @ 3 + --- Front touch 2 sensor @ 3 + --- Back touch 2 sensor @ 3 + The big change is the addition of a rotation sensor + This will be used to count ticks for turns, and to + find the egg. + + A turn takes 12 clicks on the rotation sensor. We have a 1-1 + gear ration on the rotation sensor. + + 4. + G.A.P. + 03/03/2006 + @ v. 1.3 + - Added Pauls POC3 code to transfer intructions + - Depending on the surface, we may need to calibrate TICKS to turn + Imagine the surface is now carpet, we may want to force-calibrate + the number if ticks needed to turn. See View Button. + + 5. G.A.P. + 12/03/2006 + @ v. 1.4 + The testing demo gave me an idea. Do away with reverse all + together. This worked well. No more reverse. We turn by + insturctions 4,9,4. + +------------------------------------------------------------ + End Revision History (final_rcx.java) +------------------------------------------------------------ */ public class final_rcx_ibm implements SensorConstants { // private static int HATCH_ACTION_TIME = 200; // private static int TASK_NUM = 0; private static int CURR_TASK = -1; private static int PREV_TASK = -1; private static int TASKS_SIZE_MAX = 150; private static int[] TASKS = new int[TASKS_SIZE_MAX]; // private boolean HATCH_STATE = false; // private static int DEFAULT_LEFT_WALL = 30; private static int DEFAULT_RIGHT_WALL = 30; // private static int POLL_LEFT_WALL = 30; private static int POLL_RIGHT_WALL = 30; // private static int PREV_DIR[] = {0,0,0,0,0,0,0,0,0,0}; private static int COUNTER_FORWARD = 0; private static int COUNTER_BACKWARD = 9; // // private static int MOTOR_LEFT_POWER = 7; private static int MOTOR_RIGHT_POWER = 7; // private static final int MOTOR_MAX_POWER = 7; // private static boolean DEBUG_STAY_CENTERED = false; private static boolean DEBUG_TASKS = false; private static boolean DEBUG_GET_EGG = false; private static boolean DEBUG_ROTATION = false; // private static int TURN_TICKS_RIGHT = 13; private static int TURN_TICKS_LEFT = 12; // private static int TICKS = 0; // private static boolean SECND_RUN = false; // // private static int TASK_TICK_START = 0; private static int TASK_TICK_POLL = 0; // private boolean TURN_STOP_MOTOR = true; private static final int STOP_MOTOR_TIME = 100; // // private static int T_INTERSECT_READ_DIFF = 5; // // private static int LOG_SIZE_MAX = 100; private static int[] LOG = new int[LOG_SIZE_MAX]; private static int LOG_INDEX = 0; // // private static boolean PRODUCTION = true; // // private static boolean POWER_UP_REVERSE = true; // private static RCXPort port; // public static void main (String[] aArg) throws InterruptedException { // clearLog(); if (PRODUCTION) { // puts commands retrieved into tasks[] declared a member variable, for now retrieveTaskList(); // play descending arpeggio Sound.systemSound(false,2); } else { manualList(); } // Sensor.S1.setTypeAndMode(SENSOR_TYPE_ROT, SENSOR_MODE_ANGLE); Sensor.S1.setPreviousValue(0); Sensor.S1.activate(); // Sensor.S2.setTypeAndMode(SENSOR_TYPE_LIGHT, SENSOR_MODE_PCT); Sensor.S2.activate(); // Sensor.S3.setTypeAndMode(SENSOR_TYPE_LIGHT, SENSOR_MODE_PCT); Sensor.S3.activate(); // Button.PRGM.addButtonListener( new ButtonListener() { public void buttonPressed( Button button) { } public void buttonReleased( Button button) { DEFAULT_LEFT_WALL = Sensor.S2.readValue(); DEFAULT_RIGHT_WALL = Sensor.S3.readValue(); } } ); // // Button.VIEW.addButtonListener( new ButtonListener() { public void buttonPressed( Button button) { } public void buttonReleased( Button button) { } } ); // // Button.RUN.addButtonListener( new ButtonListener() { public void buttonPressed( Button button) { } public void buttonReleased( Button button){ (new final_rcx_ibm()).run(); } } ); Button.RUN.waitForPressAndRelease(); } // private void run() { int WALL_LEFT_DIFF = 0; int WALL_RIGHT_DIFF = 0; // setNextTask(); // log first action logEvent(); // begin control loop while(true) { // // +-------------------------------------------------- // // begin stay centered // // +-------------------------------------------------- if (COUNTER_FORWARD == 9) { COUNTER_FORWARD = 0; } COUNTER_FORWARD++; // POLL_LEFT_WALL = Sensor.S2.readValue(); POLL_RIGHT_WALL = Sensor.S3.readValue(); // WALL_LEFT_DIFF = POLL_LEFT_WALL - DEFAULT_LEFT_WALL; WALL_RIGHT_DIFF = POLL_RIGHT_WALL - DEFAULT_RIGHT_WALL; // // if (CURR_TASK == 3 || CURR_TASK == 4) { // we are turning, power up MOTOR_LEFT_POWER = 7; MOTOR_RIGHT_POWER = 7; // // < -2 , -1 , 0 , 1 , 2 > // // handle the left side // } else if ( WALL_LEFT_DIFF > 2 ) { PREV_DIR[COUNTER_FORWARD] = -2; // hard right -- too close to left wall MOTOR_LEFT_POWER = 7; MOTOR_RIGHT_POWER = 0; if (DEBUG_STAY_CENTERED) { TextLCD.print("3"); } } else if ( WALL_LEFT_DIFF >= 1) { PREV_DIR[COUNTER_FORWARD] = -1; // soft right -- close to left wall MOTOR_LEFT_POWER = 7; MOTOR_RIGHT_POWER = 0; if (DEBUG_STAY_CENTERED) { TextLCD.print("4"); } } // // handle the right side // else if (WALL_RIGHT_DIFF > 2) { PREV_DIR[COUNTER_FORWARD] = 2; // hard left -- too close to right wall MOTOR_LEFT_POWER = 0; MOTOR_RIGHT_POWER = 7; if (DEBUG_STAY_CENTERED) { TextLCD.print("7"); } } else if (WALL_RIGHT_DIFF >= 1) { PREV_DIR[COUNTER_FORWARD] = 1; // soft left -- close to right wall MOTOR_LEFT_POWER = 0; MOTOR_RIGHT_POWER = 7; if (DEBUG_STAY_CENTERED) { TextLCD.print("6"); } // // it appears that we are centered // } else { if (DEBUG_STAY_CENTERED) { TextLCD.print("5"); } // begin going backward in the array // if prev was not center, set reference if (PREV_DIR[COUNTER_FORWARD] != 0) { COUNTER_BACKWARD = COUNTER_FORWARD; } else { // the previous instance was centered // continue to go back if (COUNTER_BACKWARD == 0) { COUNTER_BACKWARD = 9; } COUNTER_BACKWARD--; } // counter adjust based on PREV_DIR if (PREV_DIR[COUNTER_BACKWARD] == -2) { MOTOR_LEFT_POWER = 0; MOTOR_RIGHT_POWER = 7; } else if (PREV_DIR[COUNTER_BACKWARD] == -1) { MOTOR_LEFT_POWER = 3; MOTOR_RIGHT_POWER = 7; } else if (PREV_DIR[COUNTER_BACKWARD] == 2) { MOTOR_LEFT_POWER = 7; MOTOR_RIGHT_POWER = 0; } else if (PREV_DIR[COUNTER_BACKWARD] == 1) { MOTOR_LEFT_POWER = 7; MOTOR_RIGHT_POWER = 3; } else { MOTOR_LEFT_POWER = 7; MOTOR_RIGHT_POWER = 7; } PREV_DIR[COUNTER_BACKWARD] = 0; } // Motor.B.setPower(MOTOR_LEFT_POWER); Motor.C.setPower(MOTOR_RIGHT_POWER); // // +-------------------------------------------------- // // end stay centered // // +-------------------------------------------------- // // +-------------------------------------------------- // // start task actions // // +-------------------------------------------------- // if (CURR_TASK == -1) { // stop everything prepare to send log Motor.A.stop(); Motor.B.stop(); Motor.C.stop(); sendTaskList(); return; } else if (CURR_TASK == 0) { // go straight & expect nothing // power levels set from above Motor.B.forward(); Motor.C.forward(); if (Sensor.S2.readValue() == 100) { // setNextTask(); // } } else if (CURR_TASK == 1) { // go forward & expect a RIGHT T-Intersection turn Motor.B.forward(); Motor.C.forward(); // TASK_TICK_POLL = Sensor.S1.readValue(); if (TASK_TICK_POLL > TASK_TICK_START + TICKS) { setNextTask(); } // if (DEBUG_TASKS) { TextLCD.print("1"); } } else if (CURR_TASK == 2) { // go forward & expect a LEFT T-Intersection turn Motor.B.forward(); Motor.C.forward(); // TASK_TICK_POLL = Sensor.S1.readValue(); if (TASK_TICK_POLL > TASK_TICK_START + TICKS) { setNextTask(); } // if (DEBUG_TASKS) { TextLCD.print("2"); } } else if (CURR_TASK == 3) { // // TURN RIGHT // int start = 0; int end = 0; int turn_right_local_ticks = 0; // // set power back to normal // Motor.B.setPower(7); Motor.C.setPower(7); // start = Sensor.S1.readValue(); end = start; // if (PREV_TASK == 1) { // PREV_TASK == 1 turn_right_local_ticks = TURN_TICKS_RIGHT - 2; } else { // PREV_TASK == 9 turn_right_local_ticks = TURN_TICKS_RIGHT; } // while (end > start - turn_right_local_ticks) { Motor.B.forward(); Motor.C.backward(); end = Sensor.S1.readValue(); } // setNextTask(); // you don't care about what happened before turn clearCache(); // if (DEBUG_TASKS) { TextLCD.print("3"); } } else if (CURR_TASK == 4) { // when turning LEFT the rotation sensor will be going opposite! // int start = 0; int end = 0; int turn_left_local_ticks = 0; // // TURN LEFT // set power back to normal // Motor.B.setPower(7); Motor.C.setPower(7); // start = Sensor.S1.readValue(); end = start; // // if (PREV_TASK == 2) { // PREV_TASK == 2 turn_left_local_ticks = TURN_TICKS_LEFT + 1; } else { // PREV_TASK == 9 turn_left_local_ticks = TURN_TICKS_LEFT + 1; } // // while (end < start + turn_left_local_ticks) { Motor.C.forward(); Motor.B.backward(); end = Sensor.S1.readValue(); } // // grab next task setNextTask(); // you don't care about what happened before turn clearCache(); // if (DEBUG_TASKS) { TextLCD.print("4"); } } else if (CURR_TASK == 5) { // // if (DEBUG_GET_EGG == true) { LCD.showNumber(Sensor.S1.readValue()); } // grab the egg // The hatch will be initially closed, when this task // is called, the hatch opens. Once the rotation // sensor tickcs hit, the hatch closes. // open the hatch, get ready for the egg Motor.B.forward(); Motor.C.forward(); // if (HATCH_STATE == false) { Motor.A.forward(); wait(HATCH_ACTION_TIME); HATCH_STATE = true; } else { Motor.A.stop(); } // TASK_TICK_POLL = Sensor.S1.readValue(); // if (TASK_TICK_POLL > TASK_TICK_START + TICKS) { // Motor.A.backward(); wait(HATCH_ACTION_TIME); setNextTask(); Motor.A.stop(); // reset the hatch to get more eggs HATCH_STATE = false; TASK_TICK_POLL = 0; // Motor.B.stop(); Motor.C.stop(); wait(200); // // double pump the cage to ensure we get egg // Motor.A.forward(); wait(100); Motor.A.backward(); wait(150); Motor.A.stop(); // // } else { Motor.A.stop(); } // // clearCache(); // if (DEBUG_TASKS) { TextLCD.print("5"); } } else if (CURR_TASK == 9) { // go forward & expect an L-Intersection RIGHT or LEFT turn Motor.B.forward(); Motor.C.forward(); // if (Sensor.S2.readValue() == 100) { // setNextTask(); // } else if (Sensor.S3.readValue() == 100) { setNextTask(); } if (DEBUG_TASKS) { TextLCD.print("9"); } } } } // private void wait( int ms ) { try { Thread.sleep( ms ); } catch (Exception e) {} } // private void clearCache() { for (int i=0;i<10;i++){ PREV_DIR[i] = 0; } } // private static void clearLog() { for (int i=0;i> offset) & 0xFF); } return b; } // // private void logEvent() { /* Storing the milliseconds of events */ LOG[LOG_INDEX] = (int)System.currentTimeMillis(); LOG_INDEX++; } // // private static void manualList() { int MANUAL[] = { 9, 4, 5, 12, 4, 9, 4, 9, 3, 0, -1 }; for (int i = 0 ; i < MANUAL.length ; i++) { TASKS[i] = MANUAL[i]; } } // // private void setNextTask() { /* -1 === Do nothing, stop the motors, send the log 0 === Go forward expect nothing - usually to exit the network 1 === Go forward expect right turn (T-Intersection) 2 === Go forward expect left turn (T-Intersection) 3 === Turn right 4 === Turn left 5 === Go forward expect egg -- takes 2 int's, the 2nd is the distance 9 === Go forward expect right OR left turn (L-Intersection) */ // // logEvent(); // PREV_TASK = CURR_TASK; CURR_TASK = TASKS[TASK_NUM]; if ( CURR_TASK == 1 || CURR_TASK == 2 || CURR_TASK == 5 ) { // poll the rotation sensor as a reference TASK_TICK_START = Sensor.S1.readValue(); // get the distance to the egg or T-intersection TASK_NUM++; TICKS = TASKS[TASK_NUM]; } // POWER_UP_REVERSE = true; TASK_NUM++; // // } }