diff options
| author | Ido Hadanny <ido.hadanny@gmail.com> | 2011-09-22 21:30:13 +0300 |
|---|---|---|
| committer | Ido Hadanny <ido.hadanny@gmail.com> | 2011-09-22 21:30:13 +0300 |
| commit | 53b947df484159f5934898ba6c0aa69a9c869007 (patch) | |
| tree | 189d5903a67765c390de8f1e3423e6b80cde57d1 /roombacomm-client/src/com/hackingroomba/roombacomm/RoombaCommCLI.java | |
| parent | 654ca6d59e99b5e72b05dfb917bc5888c4ba6b1b (diff) | |
added stuff
Diffstat (limited to 'roombacomm-client/src/com/hackingroomba/roombacomm/RoombaCommCLI.java')
| -rw-r--r-- | roombacomm-client/src/com/hackingroomba/roombacomm/RoombaCommCLI.java | 1571 |
1 files changed, 1571 insertions, 0 deletions
diff --git a/roombacomm-client/src/com/hackingroomba/roombacomm/RoombaCommCLI.java b/roombacomm-client/src/com/hackingroomba/roombacomm/RoombaCommCLI.java new file mode 100644 index 0000000..8e121ac --- /dev/null +++ b/roombacomm-client/src/com/hackingroomba/roombacomm/RoombaCommCLI.java @@ -0,0 +1,1571 @@ +/*
+ * roombacomm.RoombaCommCLI -- test out RoombaComm library by issuing commands to it from the command line
+ *
+ * Copyright (c) 2009 Paul Bouchier, bouchier@at@classicnet.net
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General
+ * Public License along with this library; if not, write to the
+ * Free Software Foundation, Inc., 59 Temple Place, Suite 330,
+ * Boston, MA 02111-1307 USA
+ *
+ */
+package com.hackingroomba.roombacomm;
+
+import java.io.*;
+import com.hackingroomba.roombacomm.RobotConnection.ReadTerminator;
+
+public class RoombaCommCLI {
+ // robot communications variables
+ private String curLine = ""; // Line read from standard in
+ private String [] args;
+ private String protocol;
+ private String portName;
+ private RobotConnection robotConnection = null;
+ private RobotConnection arduinoConnection = null;
+ private RobotConnection localizerConnection = null;
+ private RoombaComm robot; // use ArduinoBot as the robot, because it's the highest class
+ private ArduinoBot arduino;
+ private RobotType robotType;
+
+ // encoder variables
+ int speed = 50;
+ int angle;
+ int distance = 0;
+ int lEncoder = 0;
+ int rEncoder = 0;
+ int lastLEncoder = 0;
+ int lastREncoder = 0;
+ int lEncoderOverflow = 0;
+ int rEncoderOverflow = 0;
+
+ // navigation variables
+ private double x = 0;
+ private double y = 0;
+ private double targetX, targetY;
+ private boolean targetInitialized = false;
+ private int localizerCompassOffset = 0; // angle between localizer Y axis and compass North, add to vectorToStart angle to get compass to target
+ private boolean localizerOffsetInitialized = false;
+ private double distanceToTarget;
+ private int courseToTarget;
+
+ //RoboRealm variables
+ private watchVideo wv;
+ private FrameProcessor fp;
+ private int rrShapeX; // X location in image frame of detected shape from RoboRealm
+ private int rrConfidence;
+ private int rrSize;
+ private int rrRelativeHeading;
+ private boolean roborealmConnected = false;
+
+ // I/O variables
+ InputStreamReader isr = new InputStreamReader(System.in);
+ BufferedReader in = new BufferedReader(isr);
+
+ private long startTime;
+ private long elapsedTime;
+
+ public RoombaCommCLI()
+ {
+ // do nothing
+ }
+ // main - it all starts here
+ public static void main(String[] args) {
+ System.out.println("Enter a command (type 'help' for command listing or 'quit' to exit): ");
+ RoombaCommCLI cli = new RoombaCommCLI();
+ cli.runCommands();
+ }
+
+ private void usage() {
+ System.out.print(
+ "Available Commands\n------------------\n" +
+ "help\n" +
+ "quit\n" +
+ "connect {ip:portnum | serialPort} protocol; protocol is OI or SCI or AR (tankbot) or FR (FrankenRoomba) or MO (Mo'bot)\n" +
+ "\tUse AR or MO for Arduino connection - skips roombaInit and does arduinoInit\n" +
+ "disconnect Disconnect robot\n" +
+ "safe Enter safe mode\n" +
+ "full Enter full mode\n" +
+ "speed {defaultSpeed} Set the default speed\n" +
+ "spin {angleToSpin} Spins based on timer - YMMV\n" +
+ "spinTo (compass heading}\n" +
+ "drive {distance in inches} Drives based on timer - YMMV - literally\n" +
+ "driveFor {distance in inches} [compass heading] Drives based on wheel encoders and compass\n" +
+ "driveToTarget Drive from current position to target\n" +
+ "rectangle {length, width} [heading] Drive a rectangle, length is 1st, 3rd leg, width is 2nd, 4th leg, heading is initial direction\n" +
+ "squaredance {length, heading} Drive square with side 'length', initial heading 'heading', use localizer to seek to target which should be set before run with setTarget" +
+ "squaredance2 {length, heading} Drive square with side 'length', initial heading 'heading'" +
+ "stop - Stop robot\n" +
+ "compass Print the current compass reading\n" +
+ "encoders Print the current wheel encoders reading\n" +
+ "currentHeading {heading} Set the offset between compass-reported heading and actual heading\n" +
+ "sensors\n" +
+ "localizer {IP:Port} Connect to localizer\n" +
+ "disconnectLocalizer\n" +
+ "localize - get and print localizer fix\n" +
+ "setTarget {x y} - save away the target location ((x,y) coordinate in feet) in the target variables\n" +
+ "vectorToTarget - compute distance, direction to target\n" +
+ "printTarget - Print the current target" +
+ "vectorToTarget - compute the angle (in localizer coordinate system) and distance to start point\n" +
+ "localizerXBearing {bearing of +X axis} - bearing represents compass bearing of +X axis , i.e. +ve clockwise from north to X axis\n" +
+ "roborealm - connect to RoboRealm\n" +
+ "rrsnap - snap a frame, send it to RoboRealm, and get SHAPES data from RoboRealm\n" +
+ "pointToVisualTarget - rotate to find and point at a visual target\n" +
+ "seekToVisualTarget - rotate then travel incrementally to a visual target\n" +
+ "test Run tests\n" +
+ "Connect notes: Frankenroomba: connect 192.168.11.2 FR; should have sdr.sh and sdar1.sh running on chumby\n" +
+ "Mobot: with USB serial cable to Serial3, and arduino on USB1, run sdar1.sh on chumby and connect 192.168.11.2:5002 MO"
+ );
+ }
+
+ private void runCommands()
+ {
+ while (true){
+ // get the command line
+ try {
+ curLine = in.readLine();
+ } catch (IOException e) {
+ // Print out the exception that occurred
+ System.out.println("Error reading line: " + e.getMessage());
+ System.exit(0);
+ }
+
+ // split the command line
+ args = curLine.split("\\s+");
+/* System.out.println("You typed: " + curLine);
+ System.out.println("Args found: " + args.length);
+ for (String s: args) {
+ System.out.println(s);
+ }
+*/
+ // parse the command & execute it
+ if (args[0].equalsIgnoreCase("quit")) {
+ robotConnection.disconnect();
+ if (arduinoConnection != null)
+ arduinoConnection.disconnect();
+ System.exit(0);
+ } else if (args[0].equalsIgnoreCase("help")) {
+ usage();
+/*
+ * Connect Commands
+ */
+ } else if (args[0].equalsIgnoreCase("connect")) {
+ connect();
+ } else if (args[0].equalsIgnoreCase("disconnect")) {
+ robotConnection.disconnect();
+ robot = null;
+ if (arduinoConnection != null)
+ arduinoConnection.disconnect();
+ arduino = null;
+/*
+ * Roborama commands
+ */
+ } else if (args[0].equalsIgnoreCase("squareDance")) {
+ squareDance();
+ } else if (args[0].equalsIgnoreCase("squareDance2")) {
+ squareDance2();
+ } else if (args[0].equalsIgnoreCase("squareDance3")) {
+ squareDance3();
+ } else if (args[0].equalsIgnoreCase("tabletrip")) {
+ tabletrip();
+ } else if (args[0].equalsIgnoreCase("robocolumbus")) {
+ robocolumbus();
+/*
+ * Mode Commands
+ */
+ } else if (args[0].equalsIgnoreCase("safe")) {
+ robot.safe();
+ } else if (args[0].equalsIgnoreCase("full")) {
+ robot.full();
+ } else if (args[0].equalsIgnoreCase("speed")) {
+ if (args.length != 2)
+ System.out.println("Error: must specify speed");
+ else {
+ speed = Integer.parseInt(args[1]);
+ }
+/*
+ * Spin Commands
+ */
+ } else if (args[0].equalsIgnoreCase("spin")) {
+ if (args.length != 2)
+ System.out.println("Error: must specify angle to spin");
+ else {
+ // get heading
+// if (protocol.equalsIgnoreCase("FR")) {
+// System.out.print("Heading before turn: ");
+// arduino.printCompass();
+// }
+ angle = Integer.parseInt(args[1]);
+ int rv = spinByCompass(angle);
+ if (rv < 0)
+ System.out.println("Error spinning");
+ // get heading
+ if (protocol.equalsIgnoreCase("FR")) {
+ System.out.printf("\nHeading after turn: ");
+ arduino.printCompass();
+ }
+
+ }
+ } else if (args[0].equalsIgnoreCase("spinto")) {
+ if (args.length != 2)
+ System.out.println("Error: must specify angle to spin");
+ else {
+ // get heading
+ if (protocol.equalsIgnoreCase("FR")) {
+ System.out.print("Heading before turn: ");
+ arduino.printCompass();
+ }
+ angle = Integer.parseInt(args[1]);
+ spinToHeading(angle);
+ // getheading
+ if (protocol.equalsIgnoreCase("FR")) {
+ System.out.print(" Heading after turn: ");
+ arduino.printCompass();
+ }
+
+ }
+ } else if (args[0].equalsIgnoreCase("randomSpin")) {
+ while (true) {
+ angle = (int)(Math.random() * 358.0);
+ System.out.println("spinning to " + angle);
+ spinToHeading(angle);
+ try {
+ if (System.in.available() != 0){
+ robot.stop();
+ break;
+ }
+ } catch (Exception e) {
+ System.out.println("Exception: in System.in.available");
+ System.exit(-1);
+ }
+ }
+/*
+ * Drive Commands
+ */
+ } else if (args[0].equalsIgnoreCase("stop")) {
+ System.out.println("Stopping robot");
+ robot.stop();
+ } else if (args[0].equalsIgnoreCase("drive")) {
+ if (args.length == 2) {
+ distance = Integer.parseInt(args[1]);
+ robot.setSpeed(speed);
+ robot.goStraight(distance * 25); // 25mm per inch, goStraight takes mm
+ } else if (args.length == 3) {
+ distance = Integer.parseInt(args[1]);
+ angle = Integer.parseInt(args[2]);
+ float pausetime = Math.abs((distance * 25) / speed); // mm/(mm/sec) = sec
+ System.out.println("driving speed " + speed + " angle " + angle + " pausetime " + pausetime);
+ robot.drive( speed, angle );
+ robot.pause( (int)(pausetime*1000) );
+ robot.stop();
+ }
+ } else if (args[0].equalsIgnoreCase("drivefor")) {
+ if (args.length > 1) {
+ distance = Integer.parseInt(args[1]);
+ if (args.length > 2)
+ angle = Integer.parseInt(args[2]);
+ else
+ angle = -1; // drive in current direction
+ try {
+ driveByCompass(distance, angle);
+ } catch (Exception e) {
+ robot.stop();
+ System.out.println("Error: driveFor took exception, robot stopped");
+ e.printStackTrace();
+ }
+ }
+ // drive in a rectangle, by compass
+ } else if (args[0].equalsIgnoreCase("rectangle")) {
+ rectangle();
+ } else if (args[0].equalsIgnoreCase("driveToTarget")) {
+ seekToTarget();
+/*
+ * Sensor Commands
+ */
+ } else if (args[0].equalsIgnoreCase("compass")) {
+ arduino.printCompass();
+ } else if (args[0].equalsIgnoreCase("currentHeading")) {
+ if (args.length != 2) {
+ System.out.println("Error - must specify current heading");
+ continue;
+ }
+ int currentMagHeading = Integer.parseInt(args[1]);
+ arduino.setCompassOffset(currentMagHeading);
+ } else if (args[0].equalsIgnoreCase("sensors")) {
+ if (!robot.updateSensors())
+ System.err.println("Error attempting to read valid data from robot.UpdateSensors()");
+ else
+ System.out.println(robot.getSensorsAsString());
+ } else if (args[0].equalsIgnoreCase("initEncoders")) {
+ try {
+ readEncoderDistance(true);
+ } catch (Exception e) {
+ System.err.println("Exception in initEncoders" + e.getMessage());
+ }
+ } else if (args[0].equalsIgnoreCase("encoders")) {
+ double readEncDistance = 0;
+
+ startTime = System.currentTimeMillis();
+ try {
+ readEncDistance = readEncoderDistance(false);
+ } catch (Exception e) {
+ System.err.println("Exception reading robot encoders: " + e.getMessage());
+ }
+ long elapsedTime = System.currentTimeMillis() - startTime;
+ System.out.println("Distance: " + readEncDistance + " in " + elapsedTime + " ms");
+
+/*
+ * Localizer commands
+ */
+ } else if (args[0].equalsIgnoreCase("localizer")) {
+ connectLocalizer();
+ } else if (args[0].equalsIgnoreCase("disconnectLocalizer")) {
+ localizerConnection.disconnect();
+ } else if (args[0].equalsIgnoreCase("localize")) {
+ if (!localize()) {
+ System.out.println("Error reading localizer");
+ continue;
+ }
+ } else if (args[0].equalsIgnoreCase("setTarget")) {
+ if (args.length < 3) {
+ System.out.println("Must provide x & y location of target (in feet)");
+ continue;
+ }
+ saveTarget(args[1], args[2]);
+ } else if (args[0].equalsIgnoreCase("vectorToTarget")) {
+ if (!localize()) {
+ System.out.println("Error reading localizer");
+ continue;
+ }
+ try {
+ vectorToTarget();
+ } catch (Exception e) {
+ System.err.println("Exception computing vector to target - is target initialized?");
+ continue;
+ }
+ } else if (args[0].equalsIgnoreCase("printTarget")) {
+ System.out.println("targetX: " + targetX + " targetY: " + targetY);
+ } else if (args[0].equalsIgnoreCase("localizerXBearing")) {
+ if (args.length != 2) {
+ System.out.println("Error - must specify bearing of +X axis");
+ continue;
+ }
+ try {
+ // Subtract 90 from localizer X axis (measured by hand-compass)
+ localizerCompassOffset = (Integer.parseInt(args[1]) + 270) % 360;
+ arduino.writeConfigInt("localizerCompassOffset", localizerCompassOffset);
+ System.out.println("Wrote localizerCompassOffset to " + localizerCompassOffset);
+ localizerOffsetInitialized = true;
+ } catch (Exception e) {
+ System.err.println("Error saving localizerCompassOffset\n" + e.getMessage());
+ }
+/*
+ * RoboRealm commands
+ */
+ } else if (args[0].equalsIgnoreCase("roborealm")) {
+ connectRoborealm();
+ } else if (args[0].equalsIgnoreCase("rrsnap")) {
+ int rv;
+ try {
+ rv = rrsnap();
+ } catch (Exception e) {
+ System.out.println(e.getMessage());
+ continue;
+ }
+ System.out.print("Found shape confidence " + rrConfidence + " at X:" + rrShapeX + " ("
+ + rrRelativeHeading + " degrees), size " + rrSize);
+ } else if (args[0].equalsIgnoreCase("seekToVisualTarget")) {
+ seekToVisualTarget(0);
+ } else if (args[0].equalsIgnoreCase("pointToVisualTarget")) {
+ pointToVisualTarget();
+/*
+ * Uncategorized commands
+ */
+ } else if (args[0].equalsIgnoreCase("test")) {
+ testHeadingChange();
+ } else {
+ System.out.println("Invalid command");
+ }
+ }
+
+ }
+ /**
+ * Connect to robot using args
+ */
+ private void connect() {
+ if (args.length > 1) {
+ portName = args[1];
+ }
+ if (args.length > 2) {
+ protocol = args[2];
+ }
+ if ((protocol == null) || (portName == null)) {
+ System.out.println("port name and protocol must be specified\n");
+ return;
+ }
+
+ try {
+ if (!connect(portName)) {
+ System.out.println("Error connecting to service");
+ return;
+ }
+ if (protocol.equalsIgnoreCase("OI") || protocol.equalsIgnoreCase("SCI") ||
+ protocol.equalsIgnoreCase("FR")) {
+ robotType = new RobotType(RobotType.robotTypes.roomba);
+ initRoomba();
+ } else if (protocol.equalsIgnoreCase("FR")) { // FrankenRoomba
+ robotType = new RobotType(RobotType.robotTypes.frankenRoomba);
+ arduino.initArduinoBot(robotType);
+ } else if (protocol.equalsIgnoreCase("AR")) { // Arduino
+ robotType = new RobotType(RobotType.robotTypes.tankbot);
+ arduino.initArduinoBot(robotType);
+ } else if (protocol.equalsIgnoreCase("MO")) {
+ robotType = new RobotType(RobotType.robotTypes.mobot);
+ arduino.initArduinoBot(robotType);
+ } else {
+ System.out.println("Invalid protocol");
+ return;
+ }
+ } catch (Exception e) {
+ e.printStackTrace();
+ }
+ }
+ /**
+ * Connect to a TCP or serial service, which can have roomba
+ * behind it.
+ * @param portName A string containing a serial port name (e.g. /dev/ttyUSB0, or COM12:) or an IP address
+ * with optional port number after it (e.g. 192.168.1.33:5001).
+ */
+ public boolean connect(String portName)
+ {
+ int preferredPortNum = 5001;
+
+ robotConnection = new RobotConnection(portName, preferredPortNum);
+ if (!robotConnection.connect()) {
+ System.out.println("Couldn't connect to " + portName);
+ return false;
+ }
+ // init robot, which is used for the standard roombacomm commands
+ if ((protocol.equalsIgnoreCase("AR")) || (protocol.equalsIgnoreCase("MO"))) {
+ robot = new ArduinoBot(robotConnection); // superclass of roombacomm
+ } else {
+ robot = new RoombaCommTCPClient(robotConnection); // superclass of roombacomm
+ }
+ robot.setConnected(true);
+ // if FrankenRoomba, set up a 2nd robot and connection
+ if (protocol.equalsIgnoreCase("FR")) {
+ arduinoConnection = new RobotConnection(portName, 5002);
+ if (!arduinoConnection.connect()) {
+ System.out.println("Couldn't connect to port 5002 on " + portName);
+ return false;
+ }
+ arduino = new ArduinoBot(arduinoConnection); // it's actually a superclass of roombacomm
+ arduino.setConnected(true);
+ System.out.println("Connected to arduino socket");
+ } else {
+ // this is an uggly hack to cover up that lower level methods use 2 robots.
+ // it should be done by creating a new robot type: Frankenroomba, which sends the right commands to
+ // the right place
+ arduino = (ArduinoBot)robot;
+ }
+ return true;
+ }
+
+ /**
+ * Initialize a Roomba
+ */
+ public boolean initRoomba()
+ {
+ System.out.println("Roomba startup");
+ robot.startup();
+ robot.control();
+ robot.pause(1000);
+
+ System.out.println("Checking for Roomba... \n");
+ // roombaCommTCPClient.setDebug(true);
+ //robot.updateSensors(); // do it once, which doesn't seem to return anything
+ //robot.pause(1000);
+ if (robot.updateSensors()) {
+ System.out.println("Roomba found!\n");
+ System.out.println(robot.getSensorsAsString());
+ } else {
+ System.out.println("No Roomba. :( Is it turned on?\n");
+ return false;
+ }
+ // roombacomm.updateSensors();
+ System.out.println("connected (" + robot.connected() + ")\n");
+ if (robot.connected()) {
+ System.out.println("Playing some notes\n");
+ robot.playNote(72, 10); // C
+ robot.pause(200);
+ robot.playNote(79, 10); // G
+ robot.pause(200);
+ robot.playNote(76, 10); // E
+ robot.pause(200);
+ }
+ robot.setSpeed(speed);
+
+ return true;
+ }
+ /**
+ * Connect to robot using args
+ */
+ private boolean connectLocalizer() {
+ String localizerPort = null;
+
+ if (args.length > 1) {
+ localizerPort = args[1];
+ }
+ localizerConnection = new RobotConnection(localizerPort, 5010);
+ if (!localizerConnection.connect()) {
+ System.out.println("Couldn't connect to localizer at: " + portName);
+ return false;
+ }
+ localizerConnection.setReadTimeout(30000);
+ System.out.println("Connected to localizer");
+ return true;
+ }
+
+ /*
+ * Spin an angle
+ * @param angle Angle to spin, positive is clockwise (like a compass)
+ */
+ public int spinByCompass(int angle)
+ {
+ int rv;
+ int targetHeading;
+
+ targetHeading = arduino.getCompass();
+ if (targetHeading < 0)
+ return targetHeading;
+ targetHeading += angle;
+ targetHeading %= 360;
+ rv = spinToHeading(targetHeading);
+ return rv;
+ }
+
+ /*
+ * Spin to a heading using the simple algorithm for roomba, tankbot, and PID for Mobot. Mo'bot has
+ * lots of inertia, so needs derivative to prevent overshoot, and operates in high-load environment
+ * like grass, so needs integral to get it moving.
+ * @param heading Desired direction
+ * @return 0 = success
+ */
+ public int spinToHeading(int heading)
+ {
+ if (robotType.robotType == RobotType.robotTypes.mobot) {
+ return(spinToHeadingPID(heading));
+ } else {
+ return (spinToHeadingSimple(heading));
+ }
+ }
+ /*
+ * Spin to a heading
+ * @param target Compass direction to turn to
+ */
+ public int spinToHeadingSimple(int target)
+ {
+ int currentHeading;
+ int angleToSpin, absAngleToSpin;
+ int spinSpeed;
+
+ spinSpeed = robotType.fastSpinSpeed;
+
+ if ((target > 359) || (target < 0)) {
+ System.out.println("Invalid heading: " + target);
+ return -1;
+ }
+ System.out.print("Heading: ");
+ while (true) {
+ try{
+ currentHeading = arduino.getCompass();
+ System.out.print(currentHeading + " ");
+ angleToSpin = headingChange(target, currentHeading);
+ absAngleToSpin = Math.abs(angleToSpin);
+
+ if (currentHeading < 0) {
+ System.out.println("Read invalid heading: " + currentHeading);
+ robot.stop();
+ return -2;
+ }
+
+ // spin slow when we get close
+ if (absAngleToSpin < 10) {
+ spinSpeed = robotType.slowSpinSpeed;
+ } else {
+ spinSpeed = robotType.fastSpinSpeed;
+ }
+
+ if ((absAngleToSpin < robotType.tolerance) || (System.in.available() > 0)) {
+ robot.stop();
+ System.out.println();
+ robot.pause(500); // pause to let motion stop, then recheck heading
+ currentHeading = arduino.getCompass();
+ angleToSpin = headingChange(target, currentHeading);
+ absAngleToSpin = Math.abs(angleToSpin);
+ if (absAngleToSpin < robotType.tolerance) {
+ return 0;
+ }
+ }
+ if (angleToSpin > 0)
+ robot.spinRightAt(spinSpeed);
+ else
+ robot.spinLeftAt(spinSpeed);
+
+ if (System.in.available() != 0){
+ robot.stop();
+ return -4;
+ }
+ Thread.sleep(100); // spin for 100ms
+ } catch (Exception e) {
+ e.printStackTrace();
+ return -3;
+ }
+ }
+ }
+
+ /*
+ * Spin to a heading with PID control
+ * @param heading Desired direction at end of spin
+ * @return 0 = success, negative for error
+ */
+ public int spinToHeadingPID(int target)
+ {
+ int currentHeading;
+ int headingError, absHeadingError;
+ int spinSpeed;
+ double radiusPidOutput = 0;
+
+ if ((target > 359) || (target < 0)) {
+ System.out.println("Invalid heading: " + target);
+ return -1;
+ }
+
+ System.out.println("Spinning to heading " + target + " using PID");
+ // initialize the direction control system
+ Pid radiusPid = new Pid(robotType.KP, robotType.KI, robotType.KD);
+
+ // spin checking compass every 100ms
+ while (true) {
+ try{
+ startTime = System.currentTimeMillis();
+
+ currentHeading = arduino.getCompass();
+ System.out.print(currentHeading + " ");
+ headingError = headingChange(target, currentHeading);
+ absHeadingError = Math.abs(headingError);
+
+ if (currentHeading < 0) {
+ System.out.println("Read invalid heading: " + currentHeading);
+ robot.stop();
+ return -2;
+ }
+
+ // stop and re-check if angle to spin < tolerance or user interrupt
+ if ((absHeadingError < robotType.tolerance) || (System.in.available() > 0)) {
+ robot.stop();
+ robot.pause(500); // pause to let motion stop, then recheck heading
+ currentHeading = arduino.getCompass();
+ System.out.println(currentHeading);
+ headingError = headingChange(target, currentHeading);
+ absHeadingError = Math.abs(headingError);
+ if (absHeadingError < robotType.tolerance) {
+ return 0;
+ }
+ }
+
+ // compute the PID output given current headingError
+ radiusPidOutput = radiusPid.computePid(0, headingError);
+ spinSpeed = (int)radiusPidOutput;
+ if (spinSpeed > 50) spinSpeed = 50; // cap spinSpeed
+ if (spinSpeed < -50) spinSpeed = -50; // cap spinSpeed
+ System.out.print(" rPidOut " + radiusPidOutput );
+
+ // start spinning
+ robot.spin(spinSpeed);
+
+ // check for user stop input
+ if (System.in.available() != 0){
+ robot.stop();
+ return -4;
+ }
+ elapsedTime = System.currentTimeMillis() - startTime;
+ System.out.println(" in " + elapsedTime + "ms");
+
+ // spin for 100ms
+ // Thread.sleep(100);
+ } catch (Exception e) {
+ e.printStackTrace();
+ return -3;
+ }
+ }
+ }
+
+ public int driveByCompass(double distance) throws Exception
+ {
+ int rv;
+ int direction;
+
+ direction = arduino.getCompass();
+ if (direction < 0) {
+ System.out.println("Read invalid heading: " + direction);
+ robot.stop();
+ return -3;
+ }
+ rv = driveByCompass(distance, direction);
+ return rv;
+ }
+
+ public int driveByCompass(double distance, int direction) throws Exception
+ {
+ // distance control variables
+ double distanceTravelled = 0;
+ double startEncDistance = 0;
+ double readEncDistance = 0;
+ double distanceToGo = 0;
+ int heading = 0;
+ int headingError = 0;
+ // speed control variables: speedRamp is a table of {speed, brakingDistance}
+ final int [][] speedRamp = {{20,2},{60,2},{100,2},{140,3},{180,4},{220,9},{260,12},{300,15},{340,19},{380,23},{420,27},{460,33},{500,38}};
+ final int maxRamp = 12; // number of steps in speed ramp
+ int rampIndex = 0;
+ boolean rampUp = true; // start by ramping up
+ int currentSpeed;
+ // direction control variables
+ int [] radiusTable;
+ int radiusTableMiddle; // define the midpoint around which PID swings us
+ int radiusTableIndx, radius;
+ double radiusPidOutput = 0;
+
+ readEncoderDistance(true); // initialize the encoder reader
+ while (direction < 0) {
+ System.out.println("getting direction to head in");
+ direction = arduino.getCompass();
+ if (direction < 0) {
+ System.err.println("Error reading compass");
+ }
+ }
+ System.out.println("Driving " + distance + " inches on heading " + direction);
+ // initialize the distance control system
+ if (distance <= 0) return -1;
+ distanceToGo = distance;
+ readEncDistance = startEncDistance = readEncoderDistance(false);
+ if (startEncDistance < 0) {
+ System.out.println("FIXME Error reading encoders in driveByCompass()");
+ readEncDistance = startEncDistance = readEncoderDistance(false); // read it a 2nd time
+ if (startEncDistance < 0) {
+ System.out.println("Second error reading encoders in driveByCompass()");
+ return -2;
+ }
+ }
+ currentSpeed = speedRamp[rampIndex][0];
+ //System.out.println("Encoders (L, R): " + lEncoder +" " + rEncoder);
+
+ // initialize the direction control system
+ radiusTable = initRadiusTable();
+ radiusTableMiddle = (radiusTable.length/2); // table is always an odd # of elements, point to middle of table
+ radiusTableIndx = radiusTableMiddle; // initial radius is middle of table (straight)
+ radius = radiusTable[radiusTableIndx];
+ Pid radiusPid = new Pid(robotType.KP, robotType.KI, robotType.KD);
+ System.out.printf("Distance: %4.2f to go: %4.2f Speed: %d, ReadEncDist: %4.2f (%d %d) radiusPidOutput: %4.2f Radius %d\n",
+ distanceTravelled, distanceToGo, currentSpeed, readEncDistance, lEncoder, rEncoder, radiusPidOutput, radius);
+
+ // turn to point in the right direction
+ spinToHeading(direction);
+ robot.pause(500);
+
+ // drive checking sensors every 100ms
+ while (true) {
+ robot.drive(currentSpeed, radius);
+ Thread.sleep(100); // drive for 100ms
+
+ // get encoders & compute distance remaining
+ readEncDistance = readEncoderDistance(false);
+ if (readEncDistance < 0) {
+ System.err.println("FIXME Error reading encoders in driveByCompass()");
+ readEncDistance = readEncoderDistance(false); // read it a 2nd time
+ if (startEncDistance < 0) {
+ System.out.println("Second error reading encoders in driveByCompass()");
+ continue;
+ }
+ }
+ distanceTravelled = readEncDistance - startEncDistance;
+ distanceToGo = distance - distanceTravelled;
+ heading = arduino.getCompass();
+ if (heading < 0) {
+ System.err.println("Error reading compass");
+ continue;
+ }
+ //System.out.println("Encoders (L, R, distance): " + lEncoder +" " + rEncoder + " " + currentDistance);
+
+ // ramp speed up then down
+ if (distanceToGo < speedRamp[rampIndex][1]) { // if closer than braking distance, decelerate to Vmin
+ rampUp = false;
+ if (rampIndex > 0)
+ rampIndex--;
+ } else if (distanceToGo > speedRamp[rampIndex][1]) { // if further away than braking distance, accelerate to Vmax
+ if ((rampIndex < maxRamp) && (rampUp == true))
+ rampIndex++;
+ }
+ currentSpeed = speedRamp[rampIndex][0];
+
+ heading = arduino.getCompass();
+ headingError = headingChange(direction, heading);
+ radiusPidOutput = radiusPid.computePid(0, headingError);
+ radiusTableIndx = (int)radiusPidOutput + radiusTableMiddle; // offset PID output into table
+ if (radiusTableIndx < 0) radiusTableIndx = 0;
+ else if (radiusTableIndx >= radiusTable.length) radiusTableIndx = radiusTable.length -1;
+ radius = radiusTable[radiusTableIndx];
+
+ //System.out.printf("Distance: %4.2f to go: %4.2f Heading: %d Speed: %d, ReadEncDist: %4.2f (%d %d) radiusPidOutput: %4.2f Radius %d\n",
+ // distanceTravelled, distanceToGo, heading, currentSpeed, readEncDistance, lEncoder, rEncoder, radiusPidOutput, radius);
+
+ // stop if we've arrived
+ if ((distance < distanceTravelled) || (System.in.available() > 0) || (readEncDistance < 0)) {
+ robot.stop();
+ System.out.println("Finished trip, travelled: " + distanceTravelled + " in, readEncDistance: " + readEncDistance);
+ robot.pause(500); // pause to let motion stop, then recheck distance
+ break;
+ }
+ }
+ return 0;
+ }
+
+ /**
+ * Drive to a target that has been previously set using the settarget command.
+ * Use the localizer to decide how far & which direction to drive
+ */
+ private void seekToTarget() {
+ int currentHeading;
+ while ((currentHeading = arduino.getCompass()) < 0)
+ ;
+ seekToTarget(currentHeading, 0);
+
+ }
+ private void seekToTarget(int startHeading, long timeLimit) {
+ // measure/compute initial distance & direction to target
+ try {
+ while (!localize())
+ robot.goForward(12 * 25); // go forward a foot
+ vectorToTarget(); // compute the distance and course to target
+ } catch (Exception e) {
+ System.err.println("Exception getting vector to target\n" + e.getMessage());
+ return;
+ }
+
+ // seek to target while further away than tolerance, limited by timeLimit if non-zero
+ while ((distanceToTarget > 2.0) && ((timeLimit == 0) || (timeLimit > System.currentTimeMillis()))) {
+ int rv = spinToHeading(courseToTarget);
+ if (rv < 0) {
+ System.out.println("Error spinning to target: " + rv);
+ return;
+ }
+ try {
+ rv = driveByCompass(distanceToTarget, courseToTarget);
+ if (rv < 0) {
+ System.out.println("Error driving to target: " + rv);
+ }
+ } catch (Exception e) {
+ System.out.println("Error driving to target: " + e.getMessage());
+ e.printStackTrace();
+ return;
+ }
+ // measure/compute current distance & direction to target
+ try {
+ while (!localize())
+ robot.goForward(12 * 25); // go forward a foot
+ vectorToTarget(); // compute the distance and course to target
+ } catch (Exception e) {
+ System.err.println("Exception getting vector to target\n" + e.getMessage());
+ return;
+ }
+ }
+ spinToHeading(startHeading);
+ if (timeLimit > System.currentTimeMillis()) {
+ System.out.println("**** Timed out while seeking target ****");
+ } else {
+ System.out.println("**** Arrived at target ****");
+ }
+ return;
+ }
+
+ /**
+ * Drive in a rectangle specified by length (1st & 3rd legs) & width (2nd & 4th legs) with optional heading
+ */
+ private void rectangle() {
+ int length, width;
+
+ if (args.length < 3) {
+ System.out.println("Error: must specify length, width");
+ return;
+ }
+ length = Integer.parseInt(args[1]);
+ width = Integer.parseInt(args[2]);
+ if (args.length == 4)
+ angle = Integer.parseInt(args[3]);
+ else
+ angle = arduino.getCompass(); // if initial heading not specified, use current heading
+ if (angle < 0) {
+ System.out.println("initial heading must be > 0, was: " + angle);
+ return;
+ }
+ System.out.println("Driving rectangle length: " + length + " width: " + width + "initial heading: " + angle);
+ try {
+ for (int i=0;i<2;i++) {
+ driveByCompass(length, angle);
+ angle += 90;
+ angle %= 360;
+ driveByCompass(width, angle);
+ angle += 90;
+ angle %= 360;
+ }
+ spinToHeading(angle);
+ } catch (Exception e) {
+ System.out.println("Exception driving");
+ e.printStackTrace();
+ }
+ return;
+ }
+
+ /**
+ * Drive in a square specified by length with optional heading
+ */
+ private void squareDance() {
+ int length;
+ int startHeading;
+
+ if (args.length < 2) {
+ System.out.println("Error: must specify length of side");
+ return;
+ }
+ length = Integer.parseInt(args[1]);
+ startHeading = arduino.getCompass(); // record what direction to turn to at finish
+
+ if (args.length == 3)
+ startHeading = angle = Integer.parseInt(args[2]);
+ else
+ angle = startHeading; // if initial heading not specified, use current heading
+ if (angle < 0) {
+ System.out.println("initial heading must be > 0, was: " + angle);
+ return;
+ }
+ System.out.println("Driving square dance length: " + length + "initial heading: " + angle);
+ startTime = System.currentTimeMillis();
+ long timeLimit = System.currentTimeMillis() + (2500 * 60); // limit square dance to 2.5 minutes to avoid running overtime
+
+ try {
+ // drive 4 sides
+ for (int i=0;i<4;i++) {
+ driveByCompass(length, angle);
+ angle += 90;
+ angle %= 360;
+ }
+
+ seekToTarget(startHeading, timeLimit);
+
+ } catch (Exception e) {
+ System.out.println("Exception driving");
+ e.printStackTrace();
+ }
+ spinToHeading(startHeading);
+ return;
+ }
+
+ /*
+ * Drive squaredance just based on time, without seek to target at end.
+ * Don't use compass to control direction, no timeLimit
+ */
+ private void squareDance2() {
+ int length;
+ int startHeading;
+
+ if (args.length < 2) {
+ System.out.println("Error: must specify length of side");
+ return;
+ }
+ length = Integer.parseInt(args[1]);
+ startHeading = arduino.getCompass(); // record what direction to turn to at finish
+
+ if (args.length == 3)
+ startHeading = angle = Integer.parseInt(args[2]);
+ else
+ angle = startHeading; // if initial heading not specified, use current heading
+ if (angle < 0) {
+ System.out.println("initial heading must be > 0, was: " + angle);
+ return;
+ }
+ System.out.println("Driving square dance length: " + length + "initial heading: " + angle);
+ startTime = System.currentTimeMillis();
+
+ try {
+ // drive 4 sides
+ robot.speed =200;
+ for (int i=0;i<4;i++) {
+ robot.goStraight(length * 25);
+ angle += 90;
+ angle %= 360;
+ spinToHeading(angle);
+ }
+ } catch (Exception e) {
+ System.out.println("Exception driving");
+ e.printStackTrace();
+ }
+ spinToHeading(startHeading);
+ return;
+
+ }
+ /*
+ * Drive squaredance just based on time, without seek to target at end.
+ * Don't use compass to control direction, no timeLimit
+ */
+ private void squareDance3() {
+ int length;
+ int startHeading;
+
+ if (args.length < 2) {
+ System.out.println("Error: must specify length of side");
+ return;
+ }
+ length = Integer.parseInt(args[1]);
+ startHeading = arduino.getCompass(); // record what direction to turn to at finish
+
+ if (args.length == 3)
+ startHeading = angle = Integer.parseInt(args[2]);
+ else
+ angle = startHeading; // if initial heading not specified, use current heading
+ if (angle < 0) {
+ System.out.println("initial heading must be > 0, was: " + angle);
+ return;
+ }
+ System.out.println("Driving square dance length: " + length + "initial heading: " + angle);
+ startTime = System.currentTimeMillis();
+ long timeLimit = System.currentTimeMillis() + (2500 * 60); // limit square dance to 2.5 minutes to avoid running overtime
+
+ try {
+ // drive 4 sides
+ robot.speed =200;
+ for (int i=0;i<4;i++) {
+ robot.goStraight(length * 25);
+ angle = arduino.getCompass();
+ angle += 90;
+ angle %= 360;
+ spinToHeading(angle);
+ }
+
+ } catch (Exception e) {
+ System.out.println("Exception driving");
+ e.printStackTrace();
+ }
+ seekToTarget(startHeading, timeLimit);
+
+
+ spinToHeading(startHeading);
+ return;
+
+ }
+
+ /**
+ * Drive to edge of table, back up, turn around, & do it to the other end, but don't fall off
+ */
+ private void tabletrip()
+ {
+ robot.goForwardAt(100);
+ robot.pause(15000);
+ robot.full();
+ robot.pause(500);
+ robot.goBackward(50);
+ robot.safe();
+ robot.pause(500);
+ System.out.println("safe, spinning");
+ spinByCompass(180);
+ robot.goForwardAt(100);
+ robot.pause(15000);
+ robot.full();
+ robot.pause(500);
+ robot.goBackward(50);
+ robot.safe();
+ System.out.println("safe, spinning");
+ robot.pause(500);
+ spinByCompass(180);
+ }
+
+ /**
+ * RoboColumbus: drive a distance, turn on video & snap pic, find target in image using roborealm, & incrementally seek
+ * to target, re-acquiring image every so often
+ */
+ private void robocolumbus()
+ {
+ int initialDriveDistance = 12;
+ int startHeading;
+ boolean rv;
+
+ // set up parameters
+ if (args.length < 1) {
+ System.out.println("Error: must specify drive distance (feet) before seeking target");
+ return;
+ }
+ //initialDriveDistance = Integer.parseInt(args[1]) * 12;
+ startHeading = arduino.getCompass(); // record what direction to face when searching for target
+
+ angle = 83;
+ //angle = ;
+ initialDriveDistance = 95;
+ if (args.length == 2) {
+ initialDriveDistance = 12 * Integer.parseInt(args[1]);
+ }
+ if (args.length == 3)
+ startHeading = angle = Integer.parseInt(args[2]);
+ else
+ angle = startHeading; // if initial heading not specified, use current heading
+ if (angle < 0) {
+ System.out.println("initial heading must be > 0, was: " + angle);
+ return;
+ }
+
+
+ System.out.println("Driving RoboColumbus length: " + initialDriveDistance + "initial heading: " + angle);
+ startTime = System.currentTimeMillis();
+ long timeLimit = System.currentTimeMillis() + (4500 * 60); // limit square dance to 2.5 minutes to avoid running overtime
+
+ // start driving
+ try {
+ // drive to image search point
+ spinToHeading(angle);
+ robot.speed = 200;
+ driveByCompass(initialDriveDistance*12, angle);
+
+ // point to the start direction for the image search
+ spinToHeading(angle);
+
+ rv = seekToVisualTarget(timeLimit);
+ if (!rv) {
+ System.out.println("FAILED: unable to visually acquire target - stopping");
+ System.exit(-1);
+ }
+ } catch (Exception e) {
+ System.out.println("Exception driving");
+ e.printStackTrace();
+ }
+ return;
+
+ }
+
+ private boolean seekToVisualTarget(long timeLimit)
+ {
+ double incrementalDistance = 12.0;
+ boolean arrived = false;
+ int currentSeekHeading;
+
+ currentSeekHeading = arduino.getCompass(); // starting direction for looking for targe
+ // measure/compute initial distance & direction to target
+
+ while (!arrived) {
+ currentSeekHeading = pointToVisualTarget(); // update current heading to target
+ try {
+ if (System.in.available() != 0){
+ robot.stop();
+ return false;
+ }
+ } catch (Exception e) {
+ System.out.println("Exception: in System.in.available");
+ System.exit(-1);
+ }
+
+ int rv = spinToHeading(currentSeekHeading);
+ if (rv < 0) {
+ System.out.println("Error spinning to target: " + rv);
+ return false;
+ }
+ try {
+ rv = driveByCompass(incrementalDistance, currentSeekHeading);
+ if (rv < 0) {
+ System.out.println("Error driving to target: " + rv);
+ }
+ } catch (Exception e) {
+ System.out.println("Error driving to target: " + e.getMessage());
+ e.printStackTrace();
+ return false;
+ }
+ }
+ return true;
+ }
+
+ /*
+ * Point robot at visual target. On exit, provide compass heading to target, and robot is pointed
+ * approximately at target
+ * @param currentHeading The starting direction we're approximately facing in, and the direction
+ * around which the algorithm searches for the target
+ */
+ public int pointToVisualTarget()
+ {
+ int targetHeading;
+ int currentSeekHeading, startHeading;
+ int currentSeekHeadingIncrement = 5;
+ int seekIteration;
+ final int seekLimit = 24; // how many seeks until we give up
+ boolean gotDataFlag;
+
+ currentSeekHeading = arduino.getCompass();
+ startHeading = currentSeekHeading;
+
+ // seek back and forth until we acquire image with good confidence
+ for (seekIteration=1; seekIteration<seekLimit; seekIteration++) {
+ try {
+ if (System.in.available() != 0){
+ robot.stop();
+ System.out.println("Exception: aborted by user keystroke");
+ System.exit(-1);
+ }
+ } catch (Exception e) {
+ System.out.println("Exception: in System.in.available");
+ System.exit(-1);
+ }
+ try {
+ rrsnap();
+ gotDataFlag = true;
+ } catch (Exception e) {
+ System.out.print(e.getMessage());
+ gotDataFlag = false;
+ }
+ if ((gotDataFlag == false) || (rrConfidence < 50)) {
+ System.out.println("; failed to find target on iteration " + seekIteration + "in direction " + currentSeekHeading);
+ //currentSeekHeadingIncrement = 0 - (seekIteration * currentSeekHeadingIncrement); // next direction to look in
+ if (seekIteration == 12) {
+ currentSeekHeading = startHeading; //Go back to initial direction & search the other way
+ currentSeekHeadingIncrement = 0 - currentSeekHeadingIncrement;
+ }
+ currentSeekHeading += currentSeekHeadingIncrement; // failed to find target
+ currentSeekHeading = normalizeCompassHeading(currentSeekHeading); // fixup wraparound
+ spinToHeading(currentSeekHeading); // point in the next direction to search
+ } else {
+ System.out.println("Found target with robot pointed in direction " + currentSeekHeading);
+ break;
+ }
+ }
+
+ if (seekIteration == seekLimit) {
+ System.out.println("Couldn't find target: abandoning search");
+ return -1;
+ }
+
+ // return the target heading
+ targetHeading = arduino.getCompass() + rrRelativeHeading;
+ targetHeading = normalizeCompassHeading(targetHeading);
+ System.out.println("pointToVisualTarget returning target at " + targetHeading + " robot direction " + arduino.getCompass());
+ return targetHeading;
+ }
+
+ private int normalizeCompassHeading(int heading)
+ {
+ if (heading > 359)
+ return (heading % 360);
+ else if (heading < 0)
+ return (heading + 360);
+ return heading;
+ }
+
+ /*
+ * Ask the localizer for robot location. Results returned in x, y class variables
+ * @return boolean which is false if localization failed, true otherwise
+ */
+ private boolean localize()
+ {
+ String locationString = "";
+
+
+ if (localizerConnection == null) {
+ // if no localizer, prompt for a location
+ System.out.println("Enter location in feet (decimals ok), x and y, in form like x 1.2 2.9<enter>");
+ try {
+ locationString = in.readLine();
+ } catch (IOException e) {
+ // Print out the exception that occurred
+ System.out.println("Error reading line: " + e.getMessage());
+ return false;
+ }
+ } else {
+ // we are connected to localizer, ask it for location
+ localizerConnection.send('L'); // send localize command to localizer
+ try {
+ locationString = localizerConnection.readBotToTerminator(ReadTerminator.NULL);
+ } catch (Exception e) {
+ System.out.println("Error: exception reading localizer");
+ return false;
+ }
+ }
+
+ // split the string apart & look for the Invalid keyword
+ System.out.println("received string: " + locationString);
+ String [] splitLocationStrings = locationString.split("\\s");
+ if (splitLocationStrings[0].compareTo("bogus") == 0) {
+ System.out.println("Location is invalid");
+ return false;
+ }
+
+ // parse the location string into x & y and print
+ if (splitLocationStrings.length < 3) {
+ System.out.println("Error: must have at least 3 elements in location string");
+ return false;
+ }
+ try {
+ x = new Double(splitLocationStrings[1]) * 12;
+ y = new Double(splitLocationStrings[2]) * 12;
+ } catch (Exception e) {
+ System.err.println("Error parsing location strings\n" + e.getMessage());
+ return false;
+ }
+ System.out.println("x: " + x + " y: " + y);
+ return true;
+ }
+ private void saveTarget(String x, String y)
+ {
+ // target is entered in decimal feet, usually from localizer reading
+ targetX = Double.parseDouble(x) * 12;
+ targetY = Double.parseDouble(y) * 12;
+ try {
+ arduino.writeConfigDouble(new String("targetX"), targetX);
+ arduino.writeConfigDouble(new String("targetY"), targetY);
+ targetInitialized = true;
+ } catch (Exception e) {
+ System.err.println("Error writing target location");
+ }
+ }
+ public void vectorToTarget () throws Exception
+ {
+ double deltaX, deltaY;
+ double localizerDirection = 0;
+ int angle2t;
+
+ // if we haven't initialized the target location yet, try to read from a config file
+ // targetInitialized ensures we only try this once
+ try {
+ if (!targetInitialized) { // get the compass offset from file if it exists
+ targetX = arduino.readConfigDouble(new String("targetX"));
+ targetY = arduino.readConfigDouble(new String("targetY"));
+ System.out.println("target set by file read to x: " + targetX + " y: " + targetY);
+ targetInitialized = true;
+ }
+ if (!localizerOffsetInitialized) { // get the localizer Y axis offset from North if it exists
+ localizerCompassOffset = arduino.readConfigInt("localizerCompassOffset");
+ localizerOffsetInitialized = true;
+ System.out.println("localizerCompassOffset set by file to " + localizerCompassOffset);
+ }
+ } catch (Exception e) {
+ System.out.println("No targetX or targetY file or localizer compass offset file could be opened\n" + e.getMessage());
+ throw (e);
+ }
+
+ // compute distance from current location to target
+ deltaX = targetX - x;
+ deltaY = targetY - y;
+ distanceToTarget = Math.sqrt(Math.pow(deltaX, 2) + Math.pow(deltaY, 2));
+ if (deltaX == 0) { // make sure we don't try to divide by zero
+ if (deltaY > 0)
+ localizerDirection = Math.PI/2;
+ else
+ localizerDirection = -Math.PI/2;
+ } else { // deltaX is non-zero, compute arctan to find angle to target
+ localizerDirection = Math.atan(deltaY/deltaX); // in radians, with incrementing positive values going counterclockwise
+ if ((deltaX < 0) && (deltaY <=0)) localizerDirection -= Math.PI;
+ if ((deltaX < 0) && (deltaY > 0)) localizerDirection += Math.PI;
+ }
+ angle2t = (int)Math.toDegrees(localizerDirection); // angle to start in localizer frame of reference, +180 to -180
+ courseToTarget = 90 - angle2t + localizerCompassOffset; // convert to +ve Y axis = 0 degrees, increasing angle clockwise
+ courseToTarget %= 360; // make sure course < -360
+ if (courseToTarget < 0) courseToTarget += 360; // turn -90 into 270
+ System.out.println("** Current location: x: " + x + " y: " + y + " Target location x: " + targetX + " y: " + targetY);
+ System.out.println("** Distance to target: " + distanceToTarget + " heading: " + courseToTarget);
+ System.out.println("localizerCompassOffset: " + localizerCompassOffset);
+ }
+
+ private int [] initRadiusTable()
+ {
+ // speed table goes in steps of 100 from -500 to -2000, with 0x8000 instead of 0
+ // then 2000 to 500
+ int stepSize = 100;
+ int minRadius = 500;
+ int maxRadius = 2000;
+ int numEntries = (((maxRadius-minRadius) * 2)/stepSize) + 3; // +1 for straight, +1 on each side for inclusive entries
+ int [] radiusTable = new int[numEntries];
+ int indx = 0;
+ int tableVal = -500;
+
+ do {
+ radiusTable[indx++] = tableVal;
+ tableVal -= stepSize; // -500, -600 ... -2000
+ } while (tableVal >= -maxRadius);
+ tableVal = 0x8000; // mid-value is 0x8000
+ radiusTable[indx++] = tableVal;
+ tableVal = maxRadius;
+ do {
+ radiusTable[indx++] = tableVal;
+ tableVal -= stepSize; // 2000, 1900, ... 500
+ } while (tableVal >= 500);
+
+ return radiusTable;
+ }
+ /**
+ *
+ * @param init If true, init initializes encoder variables, otherwise regular read encoder distance
+ * @return Average of encoders distance, in inches
+ * @throws Exception
+ */
+ public double readEncoderDistance(boolean init) throws Exception
+ {
+ byte [] encoders = {43, 44};
+ byte [] sensor_bytes;
+ double encoderDistance;
+
+ if ((protocol.equalsIgnoreCase("AR")) || (protocol.equalsIgnoreCase("MO"))) {
+ // FIXME need to handle overflow
+ encoderDistance = arduino.getEncoders();
+ lEncoder = rEncoder = arduino.getEncoder();
+ } else {
+ // WARNING - THIS DOESN'T SEEM TO HANDLE OVERFLOW WELL - WON'T WORK WITH TANKBOT
+ // read encoders from roomba
+ robot.queryList(encoders, 4);
+ if (robot.getSensorData(4) == false) // read failed
+ return -1;
+ sensor_bytes = robot.getSensor_bytes();
+
+ // handle overflow in encoders
+ lastLEncoder = lEncoder; // first time through will be 0, then will be last reading;
+ lEncoder = ArduinoBot.toUnsignedShort(sensor_bytes[0], sensor_bytes[1]);
+ lEncoder += lEncoderOverflow;
+ lastREncoder = rEncoder;
+ rEncoder = ArduinoBot.toUnsignedShort(sensor_bytes[2], sensor_bytes[3]);
+ rEncoder += rEncoderOverflow;
+
+ if (init) {
+ lEncoderOverflow = 0;
+ rEncoderOverflow = 0;
+ lEncoder = ArduinoBot.toUnsignedShort(sensor_bytes[0], sensor_bytes[1]);
+ rEncoder = ArduinoBot.toUnsignedShort(sensor_bytes[2], sensor_bytes[3]);
+ lastLEncoder = lEncoder; // initialize last value to current reading
+ lastREncoder = rEncoder;
+ } else {
+ if ((lEncoder - lastLEncoder) < -1000) {
+ lEncoderOverflow += 1<<16; // add 65536 to overflow adjuster if encoder rolled over
+ lEncoder += 1<<16;
+ }
+ if ((rEncoder - lastREncoder) < -1000) {
+ rEncoderOverflow += 1<<16; // add 65536 to overflow adjuster if encoder rolled over
+ rEncoder += 1<<16;
+ }
+ }
+ // average the encoders & turn into inches
+ }
+
+ encoderDistance = ((lEncoder + rEncoder)/2) /robotType.countsPerInch;
+ //System.out.println("encoderDistance: " + encoderDistance + " lEncoder: " + lEncoder + " rEncoder: " + rEncoder + " lastLEncoder: " + lastLEncoder + " lEncoderOverflow: " + lEncoderOverflow
+ // + " lastREncoder: " + lastREncoder + " rEncoderOverflow: " + rEncoderOverflow);
+ return encoderDistance;
+ }
+
+ /**
+ * Calculate the shortest heading change which will move the bot from current heading to target.
+ * See http://www.dreamincode.net/forums/topic/163469-calculating-the-difference-between-two-angles/
+ * for some concepts.
+ *
+ * @param target Desired compass heading: 0 - 359 degrees
+ * @param heading Current compass heading: 0 - 359 degrees
+ * @return direction and angle to turn. Positive is clockwise (like a compass)
+ */
+ public int headingChange(int target, int heading)
+ {
+ int diff = target - heading;
+ if (diff > 180) diff -= 360;
+ else if (diff < -180) diff += 360;
+ return diff;
+ }
+ private void testHeadingChange()
+ {
+ int heading[] = {35, 45, 315, 45, 0, 180, 45};
+ int target[] = {45, 35, 45, 315, 180, 0, 45};
+ int diff;
+ int testCaseCount = 7;
+ for (int i=0; i<testCaseCount; i++) {
+ diff = headingChange(target[i], heading[i]);
+ System.out.println("Target: " + target[i] + " Heading: " + heading[i] + " Heading Change: " + diff);
+ }
+
+ // first test of vectorToStart: x,y = 0,0, ring then sit on top of x,y
+ int xTest[] = {0, 10, 10, 10, 0, -10, -10, -10, 0};
+ int yTest[] = {10, 10, 0, -10, -10, -10, 0, 10, 0};
+ x = 0; y = 0;
+ saveTarget("0", "0");
+ for (int i=0; i<xTest.length; i++) {
+ x = xTest[i]; y = yTest[i];
+ try {
+ vectorToTarget();
+ } catch (Exception e) {
+ System.err.println("Exception in vectorToTarget");
+ System.exit(0);
+ }
+ System.out.println("x: " + x + " y: " + y + " distanceToTarget: " + distanceToTarget + " courseToTarget: " +
+ courseToTarget);
+
+ }
+ }
+ /**
+ * Initialize connection to Roborealm
+ */
+ private void connectRoborealm() {
+ wv = new watchVideo();
+ wv.setHeight(240);
+ wv.setWidth(320);
+ String s[] = portName.split(":"); // in case portnumber was specified, get the IP by itself
+ wv.setVideoServer(s[0]);
+ wv.setColor(true);
+ wv.setRoborealm(true);
+ fp = wv.initVideo();
+ System.out.println("Connected to video source and RoboRealm");
+ roborealmConnected = true;
+ }
+
+ /**
+ * Snap a webcam image and send it to roborealm for analysis and print the results
+ */
+ public int rrsnap() throws Exception {
+ double conf;
+
+ if (!roborealmConnected)
+ connectRoborealm();
+
+ wv.getShowFrame();
+ Boolean rv = fp.frame2Roborealm();
+ if (rv == false) {
+ System.out.println("error sending image to Roborealm");
+ throw new Exception("Error sending image to Roborealm");
+ }
+ String rrString = fp.getShapeData();
+ if (rrString != null) {
+ String s[] = rrString.split(",");
+ int xL, xR;
+ xL = Integer.parseInt(s[3]);
+ xR = Integer.parseInt(s[4]);
+ rrShapeX = (xL + xR)/2;
+ rrRelativeHeading = (rrShapeX - (wv.getWidth()/2))/16; // distance from image center
+ conf = Double.parseDouble("0." + s[0]);
+ conf *= 100; // convert to %
+ rrConfidence = (int)conf;
+ rrSize = Integer.parseInt(s[2]);
+
+ //System.out.print("Found shape confidence " + s[0] + " at " + rrShapeX);
+ return rrRelativeHeading;
+ } else {
+ //System.out.println("No RobotRealm data returned");
+ throw new Exception("Error: no Roborealm data returned");
+ }
+ }
+
+}
|