From 53b947df484159f5934898ba6c0aa69a9c869007 Mon Sep 17 00:00:00 2001 From: Ido Hadanny Date: Thu, 22 Sep 2011 21:30:13 +0300 Subject: added stuff --- .../hackingroomba/roombacomm/RoombaCommCLI.java | 1571 ++++++++++++++++++++ 1 file changed, 1571 insertions(+) create mode 100644 roombacomm-client/src/com/hackingroomba/roombacomm/RoombaCommCLI.java (limited to 'roombacomm-client/src/com/hackingroomba/roombacomm/RoombaCommCLI.java') 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 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"); + 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