From 53b947df484159f5934898ba6c0aa69a9c869007 Mon Sep 17 00:00:00 2001 From: Ido Hadanny Date: Thu, 22 Sep 2011 21:30:13 +0300 Subject: added stuff --- .../src/com/hackingroomba/roombacomm/Roborama.java | 910 +++++++++++++++++++++ 1 file changed, 910 insertions(+) create mode 100644 roombacomm-client/src/com/hackingroomba/roombacomm/Roborama.java (limited to 'roombacomm-client/src/com/hackingroomba/roombacomm/Roborama.java') diff --git a/roombacomm-client/src/com/hackingroomba/roombacomm/Roborama.java b/roombacomm-client/src/com/hackingroomba/roombacomm/Roborama.java new file mode 100644 index 0000000..5fd4a40 --- /dev/null +++ b/roombacomm-client/src/com/hackingroomba/roombacomm/Roborama.java @@ -0,0 +1,910 @@ +/* + * roombacomm.Bsquare -- test out the Bsquare command + * + * 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 jargs.gnu.CmdLineParser; +import com.hackingroomba.roombacomm.RoombaCommTCPClient; +import java.io.*; + + +/** + Drive the Roomba forward, back, or CW or CCW square
+

+ Run it with something like: 

+	    java roombacomm.Bsquare /dev/cu.KeySerial1 [protocol] command velocity distance

+     Usage:
+       roombacomm.Bsquare  [protocol]    [options]

+     where 
+     protocol (optional) is SCI or OI
+     command is one of:
+      f -- forward or back; direction controlled by +/- speed
+	  bcw or bccw -- clockwise or counter-clockwise b-square
+	  onb -- out and back
+     [options] can be one or more of:
+      -debug       -- turn on debug output
+      -hwhandshake -- use hardware-handshaking, for Windows Bluetooth
+      -nohwhandshake -- don't use hardware-handshaking
+     velocity and distance are in inches\n";
+
+*/ + public class Roborama { + + private int maxLineWidth = 30; + private int minLineWidth = 5; + private int height = 120; + private int width = 160; + String usage = + "Usage: \n"+ + " roombacomm.Roborama [protocol] [options]\n" + + "where is one of:\n" + + " f: go forward or back based on velocity for the specified distance\n" + + " bcw: do a clockwise Borenstein square with leg size specified\n" + + " bccw: do a counter-clockwise Borenstein square with leg size specified\n" + + " onb: do out'n'back for distance specified\n" + + " fig8: do figure 8 with length specified (width is hard-coded to 3 feet)\n" + + " followline: Follow a line using camera" + + " followLine takes arguments like: -v 100 -p 192.168.15.150:5001 -c followLine --videoServer 192.168.15.150 --videoPortNum 5005 -x 320 -y 240 --debug" + + " to match roombasrvr string ./roombasrvr -v -x320 -y240 -m " + + "and where [options] can be one or more of:\n"+ + " --debug -- turn on debug output\n"+ + " -hwhandshake -- use hardware-handshaking, for Windows Bluetooth\n"+ + "velocity and distance are in mm/s and mm\n"; + boolean debug = false; + boolean hwhandshake = false; + byte [] sensorBytes; // The last bytes read by roombacomm library + byte bumpSensor; + short odoDistance; + int leftEncoder, rightEncoder; + int leftMm, rightMm; + int beginLeft, beginRight; + private final static double countsPerMm = 2.57; + private int minVelocity = 50; + RoombaComm roombacomm; + private String portname; + private String cmd; + private int velocity; + private int waittime; + private String protocol; + private int distance = 0 ; + private int angle; + private int radius; + private int thresholdOverride = 0; + private String videoServer = ""; + private int videoPortNum = 0 ; + private int videoRowStart = 50; + private int videoRowEnd = 54; + + public Roborama() { + // constructor, mustn't throw exceptions. Do nothing for now + } + + // main - it all starts here + public static void main(String[] args) { + Roborama b = new Roborama(); + b.doCommand(args); + } + + public void f() + { + /* + // get sensor values before starting + roombacomm.queryList(sensorList, 6); + boolean sensorStatus = roombacomm.wait4Sensors(); + if (sensorStatus == false) { + System.out.println("Failed to read sensors"); // wait until sensorsValid true, when 6 bytes ready + System.exit(-1); + } + sensorBytes = roombacomm.getSensor_bytes(); + odoDistance = roombacomm.toShort(sensorBytes[0], sensorBytes[1]); + leftEncoder = roombacomm.toShort(sensorBytes[2], sensorBytes[3]); + rightEncoder = roombacomm.toShort(sensorBytes[4], sensorBytes[5]); + System.out.println("Odometry sensors: distance: " + odoDistance + " left encoder: " + + leftEncoder + " rightEncoder " + rightEncoder); +*/ + // now drive + roombacomm.drive( getVelocity(), 0x8000 ); + roombacomm.pause(getWaittime()); + roombacomm.stop(); + roombacomm.pause(250); // wait for robot to stop moving +/* + // get sensor values after the run + roombacomm.queryList(sensorList, 6); + sensorStatus = roombacomm.wait4Sensors(); + if (sensorStatus == false) { + System.out.println("Failed to read sensors"); // wait until sensorsValid true, when 6 bytes ready + System.exit(-1); + } + sensorBytes = roombacomm.getSensor_bytes(); + odoDistance = roombacomm.toShort(sensorBytes[0], sensorBytes[1]); + leftEncoder = roombacomm.toShort(sensorBytes[2], sensorBytes[3]); + rightEncoder = roombacomm.toShort(sensorBytes[4], sensorBytes[5]); + System.out.println("Odometry sensors: distance: " + odoDistance + " left encoder: " + + leftEncoder + " rightEncoder " + rightEncoder); +*/ + } + + void initOdometry() + { + beginLeft = beginRight = 0; + // get sensor values before starting + getOdometry(); // get 5 bytes of return data + beginLeft = leftEncoder; + beginRight = rightEncoder; + getOdometry(); // update leftMm & rightMm (should be 0) + printOdometry(); + } + boolean getOdometry() + { + byte[] sensorList = {7, 43, 44}; // bump/wheeldrop, and encoders + int returnLength = 5; + + roombacomm.queryList(sensorList, returnLength); + roombacomm.logmsg("updateSensors: pausing."); + + for(int i=0; i < 20; i++) { + if( roombacomm.sensorsValid() ) { + break; + } + roombacomm.pause( 50 ); + } + if (!roombacomm.sensorsValid()) { + System.out.println("ERROR: unable to read sensors"); + return(false); + } + sensorBytes = roombacomm.getSensor_bytes(); + bumpSensor = sensorBytes[0]; + leftEncoder = RoombaComm.toUnsignedShort(sensorBytes[1], sensorBytes[2]); + leftMm = (int)((leftEncoder - beginLeft) / countsPerMm); + rightEncoder = RoombaComm.toUnsignedShort(sensorBytes[3], sensorBytes[4]); + rightMm = (int)((rightEncoder - beginRight) / countsPerMm); + return(true); + } + void printOdometry() + { + System.out.println("Odometry sensors: left encoder: " + + leftEncoder + " (" + leftMm + "mm), rightEncoder " + rightEncoder + " (" + rightMm + "mm) bump: " + bumpSensor); + + } + public void fod() // forward using odometry + { + int rampDownDistance; + int currentVelocity = minVelocity; // starting velocity + long nextUpdateTime; + boolean rampUp, rampDown; + int currentRadius = radius; + int initialDifference; + int distanceTravelled; + + initOdometry(); + initialDifference = leftEncoder - rightEncoder; + rampDownDistance = (distance - 150); // - (int)(Math.pow(velocity, 2)/ 200); + System.out.println("target: " + distance + " rampdown Distance: " + rampDownDistance); + rampUp = true; + rampDown = false; + + // every 100ms calculate new speed & correct course + roombacomm.logmsg("velocity: " + velocity + " rampUp: " + rampUp + " rampDown: " + rampDown); + do { + nextUpdateTime = System.currentTimeMillis() + 100; + if (rampUp == true) { + currentVelocity += 20; + if (currentVelocity >= velocity) { + currentVelocity = velocity; + rampUp = false; + } + } + if (rampDown == true) { + currentVelocity -= 20; + if (currentVelocity < minVelocity) { + currentVelocity = minVelocity; + } + } + getOdometry(); + printOdometry(); + + // check whether we hit anything + if (bumpSensor != 0) { + System.out.println("Hit bumper - stopping"); + roombacomm.stop(); + roombacomm.powerOff(); + System.exit(-1); + } + + // Check which wheel to track for distance forward, or track average if not spinning + if (radius == 1) { + distanceTravelled = rightMm; + } else if (radius == -1) { + distanceTravelled = leftMm; + } else { + distanceTravelled = leftMm; + } + + if (distanceTravelled > rampDownDistance) { + rampUp = false; + rampDown = true; + } + + if (distanceTravelled > distance) + break; + + System.out.println("encoder difference: " + (leftEncoder - rightEncoder - initialDifference)); + //System.out.println("vel " + currentVelocity + " rampUp " + rampUp + " rampDown " + rampDown); + roombacomm.logmsg("driving at: " + currentVelocity + " radius: " + radius + " leftMm: " + leftMm); + roombacomm.drive( currentVelocity, radius ); + + // Sleep till next tick + if (System.currentTimeMillis() < nextUpdateTime) { + try { + Thread.sleep(nextUpdateTime - System.currentTimeMillis()); + } catch(Exception e) { + e.printStackTrace(); + } + } else { + System.out.println("WARNING: missed frame by " + (System.currentTimeMillis() - nextUpdateTime) + "ms"); + } + } while (distanceTravelled < distance); + + roombacomm.stop(); + roombacomm.pause(500); // wait for roomba to settle + + getOdometry(); // get 5 bytes of return data + printOdometry(); + + System.out.println("Ldistance: " + (leftEncoder - beginLeft) + " Rdistance: " + (rightEncoder - beginRight)); + + + } + + public void bcwod() + { + int tempDistance = distance; + int tempRadius = radius; + int turnDistance = angle; // use --angle to set turnDistance + + // left leg + distance = tempDistance; + radius = tempRadius; + fod(); + + radius = -1; + distance = turnDistance; + fod(); + + // top leg + distance = tempDistance; + radius = tempRadius; + fod(); + + radius = -1; + distance = turnDistance; + fod(); + + // right leg + distance = tempDistance; + radius = tempRadius; + fod(); + + radius = -1; + distance = turnDistance; + fod(); + + // bottom leg + distance = tempDistance; + radius = tempRadius; + fod(); + + radius = -1; + distance = turnDistance; + fod(); + + } + public void bcw() + { + roombacomm.drive( velocity, radius ); + roombacomm.pause(waittime); + roombacomm.stop(); + roombacomm.pause(1000); + roombacomm.spin(-angle); + roombacomm.pause(1000); + + roombacomm.drive( velocity, radius ); + roombacomm.pause(waittime); + roombacomm.stop(); + roombacomm.pause(1000); + roombacomm.spin(-angle); + roombacomm.pause(1000); + + roombacomm.drive( velocity, radius ); + roombacomm.pause(waittime); + roombacomm.stop(); + roombacomm.pause(1000); + roombacomm.spin(-angle); + roombacomm.pause(1000); + + roombacomm.drive( velocity, radius ); + roombacomm.pause(waittime); + roombacomm.stop(); + roombacomm.pause(1000); + roombacomm.spin(-angle); + } + + public void bccw() + { + roombacomm.drive( velocity, radius ); + roombacomm.pause(waittime); + roombacomm.stop(); + roombacomm.pause(1000); + roombacomm.spin(angle); + roombacomm.pause(1000); + + roombacomm.drive( velocity, radius ); + roombacomm.pause(waittime); + roombacomm.stop(); + roombacomm.pause(1000); + roombacomm.spin(angle); + roombacomm.pause(1000); + + roombacomm.drive( velocity, radius ); + roombacomm.pause(waittime); + roombacomm.stop(); + roombacomm.pause(1000); + roombacomm.spin(angle); + roombacomm.pause(1000); + + roombacomm.drive( velocity, radius ); + roombacomm.pause(waittime); + roombacomm.stop(); + roombacomm.pause(1000); + roombacomm.spin(angle); + } + + public void onbod() + { + fod(); + roombacomm.drive(-200, radius); + roombacomm.pause(500); + roombacomm.drive(-400, radius); + roombacomm.pause(500); + roombacomm.drive( 0-velocity, radius ); + roombacomm.pause(waittime+5000); + roombacomm.stop(); + roombacomm.pause(1500); + + } + public void onb() + { + roombacomm.drive(200, radius); + roombacomm.pause(500); + roombacomm.drive(400, radius); + roombacomm.pause(500); + roombacomm.drive( velocity, radius ); + roombacomm.pause(waittime); + roombacomm.stop(); + roombacomm.pause(500); + roombacomm.drive(-200, radius); + roombacomm.pause(500); + roombacomm.drive(-400, radius); + roombacomm.pause(500); + roombacomm.drive( 0-velocity, radius ); + roombacomm.pause(waittime); + roombacomm.stop(); + roombacomm.pause(1000); + //roombacomm.spin(360); + } + + public void fig8() + { + // calculate delay when crossing from one side of figure 8 to the other + int crosstime = Math.abs((1000 / velocity) * 900); // waittime in ms (correction .9) - 3' cross + + // lower left leg + System.out.println("lower left leg"); + fod(); + roombacomm.spin(-angle); + roombacomm.pause(500); + + // 1st middle crossing + System.out.println("1st middle crossing"); + fod(); + roombacomm.spin(angle); + roombacomm.pause(500); + + // upper right leg + System.out.println("upper right leg"); + fod(); + roombacomm.spin(angle); + roombacomm.pause(500); + + // upper crossing + System.out.println("upper crossing"); + fod(); + roombacomm.spin(angle); + roombacomm.pause(500); + + // upper left leg + System.out.println("upper left leg"); + fod(); + roombacomm.spin(angle); + roombacomm.pause(500); + + // 2nd middle crossing + System.out.println("2nd middle crossing"); + fod(); + roombacomm.spin(-angle); + roombacomm.pause(500); + + // lower right leg + System.out.println("lower right leg"); + fod(); + roombacomm.spin(-angle); + roombacomm.pause(500); + + // bottom crossing + System.out.println("bottom crossing"); + fod(); + roombacomm.spin(-angle); + roombacomm.pause(500); + } + + public void followLine() + { + int frameSize; + int trackError, trackErrorPrev1, trackErrorPrev2; + double loopGain = 5; + + if (getVideoServer() == null || getVideoServer().length() == 0) { + System.err.println(" you must supply a --videoServer value to use the command \"getVideo\""); + if (getVideoPortNum() <= 0) { + System.err.println(" you must supply a --videoPortNum value to use the command \"getVideo\""); + } + System.exit(6); + } + if (getVideoPortNum() <= 0) { + System.err.println(" you must supply a --videoPortNum value to use the command \"getVideo\""); + System.exit(7); + } + trackErrorPrev1 = 0; + trackErrorPrev2 = 0; + + FrameProcessor fp = new FrameProcessor(getVideoServer(),getVideoPortNum(), getWidth(), getHeight(), getMinLineWidth(), getMaxLineWidth(), getThresholdOverride()); + //fp.testQuantization(); + fp.createAndShowGUI(); + fp.connect(false); + + while (true) { + frameSize = fp.readFrame(getWidth(), getHeight(), 0); + if (frameSize != (getWidth() * getHeight())) { + System.out.println("getVideo read " + frameSize + " bytes - abandoning frame"); + continue; + } + if (fp.quantizeRows(videoRowStart, videoRowEnd) == 0) + fp.segmentImage(); + fp.displayFrame(); + + trackError = 0 - fp.trackLine(); // change error sign to correspond to desired turn direction + + // calculate drive radius - lots of magic numbers here + if (Math.abs(trackError)<5){ + radius = 0x8000; + } else if ((trackError >= 5) && (trackError < 74)){ + radius = (int)(-loopGain * trackError + 321.0); + if (radius < 1) + radius = 1; + } else if ((trackError <= -5) && trackError > -74) { + radius = (int)(-loopGain * trackError - 321); + if (radius > -1) + radius = -1; + } else if ((trackError == -100) || (trackError == 100)) { + roombacomm.stop(); + roombacomm.pause(1000); + roombacomm.drive(50, 0x8000); + roombacomm.pause(3240); // go forward 1/2 a roomba length + roombacomm.stop(); + roombacomm.pause(200); + roombacomm.spin((trackError < 0) ? -angle : angle); // spin 90 degrees + roombacomm.stop(); + roombacomm.pause(200); + roombacomm.drive(-50, 0x8000); // back up 1/2 a roomba length + roombacomm.pause(3240); + roombacomm.stop(); // camera should be on same point we lost the line + continue; + } else if (trackError == 2001) { + roombacomm.stop(); + roombacomm.pause(1000); + roombacomm.drive(-50, 0x8000); + roombacomm.pause(500); // go forward 1/2 a roomba length + roombacomm.stop(); + roombacomm.pause(200); + continue; + } else { + System.out.println("HELP: TRACKERROR OUT OF BOUNDS: " + trackError); + roombacomm.drive(0, 8000); // stop + continue; + } + roombacomm.logmsg(" Driving radius: " + radius); + roombacomm.drive(velocity, radius); + + } + //frameSize = fp.readFrame(); + //System.out.println("getVideo read " + frameSize + " bytes"); + //fp.disconnect(); + } + + public void normalize() + { + int frameSize; + FileOutputStream fos; + DataOutputStream dos; + + if (getVideoServer() == null || getVideoServer().length() == 0) { + System.err.println(" you must supply a --videoServer value to use the command \"getVideo\""); + if (getVideoPortNum() <= 0) { + System.err.println(" you must supply a --videoPortNum value to use the command \"getVideo\""); + } + System.exit(6); + } + if (getVideoPortNum() <= 0) { + System.err.println(" you must supply a --videoPortNum value to use the command \"getVideo\""); + System.exit(7); + } + + FrameProcessor fp = new FrameProcessor(getVideoServer(),getVideoPortNum(), getWidth(), getHeight(), getMinLineWidth(), getMaxLineWidth(), getThresholdOverride()); + fp.connect(false); + + frameSize = fp.readFrame(getWidth(), getHeight(), 0); + System.out.println("getVideo read " + frameSize + " bytes"); + double [] normalizeArray = fp.normalizeRows(videoRowStart, videoRowEnd); + + try { + File file= new File("normalizeArray"); + fos = new FileOutputStream(file); + dos=new DataOutputStream(fos); + for (int i=0; i=4\n"); + System.out.println( usage ); + System.exit(0); + } + parseCmd(args); + + // open a connection to Roomba (net or serial) + char portNameChar1 = getPortname().charAt(0); + if (portNameChar1 >= '0' && portNameChar1 <='9') { // portname begins with number, assume it's an IP + System.out.println("Using network IP " + getPortname()); + RoombaCommTCPClient roombacommTCPClient = new RoombaCommTCPClient(); +// roombacommTCPClient.setProtocol(args[1]); + roombacommTCPClient.setProtocol(getProtocol()); + if( ! roombacommTCPClient.connect( getPortname() ) ) { + System.out.println("Couldn't connect to "+getPortname()); + System.exit(1); + } + roombacomm = roombacommTCPClient; + } else { + System.out.println("using serial port " + getPortname()); + RoombaCommSerial roombacommSerial = new RoombaCommSerial(); + roombacommSerial.setProtocol(getProtocol()); + roombacommSerial.setWaitForDSR(isHwhandshake()); + if( ! roombacommSerial.connect( getPortname() ) ) { + System.out.println("Couldn't connect to "+getPortname()); + System.exit(1); + } + roombacomm = roombacommSerial; + } + + // set up ^C handling + MyShutdown sh = new MyShutdown(roombacomm); + Runtime.getRuntime().addShutdownHook(sh); + roombacomm.debug = isDebug(); + System.out.println("Roomba startup"); + roombacomm.startup(); + roombacomm.control(); +//over ride + // roombacomm.full(); + roombacomm.pause(100); + + // run the requested command + System.out.println("running command " + getCmd() + "\n"); + if (cmd.equals("f") ) { + f(); + } else if (cmd.equals("bccw")) { + bccw(); + } else if (cmd.equals("bcw")){ + bcw(); + } else if (cmd.equals("onb")) { + onb(); + } else if (cmd.equals("onbod")) { + onbod(); + } else if (cmd.equals("spin")) { + for (int i=0; i<4; i++) { + roombacomm.spin(-angle); + roombacomm.pause(1000); + } + } else if (cmd.equals("fig8")) { + fig8(); + } else if (cmd.equals("followLine")) { + followLine(); + } else if (cmd.equals("fod")){ // forward using odometry + fod(); + } else if (cmd.equals("bcwod")) { + bcwod(); + } else { + System.out.println("Invalid Command"); + } + roombacomm.stop(); + + System.out.println("Disconnecting"); + roombacomm.disconnect(); + + System.out.println("Done"); + } + + public void parseCmd(String[] args){ + System.out.println("*** start of parseCmd"); + + CmdLineParser parser = new CmdLineParser(); + CmdLineParser.Option debugOption = parser.addBooleanOption('X', "debug"); +// CmdLineParser.Option verboseOption = parser.addBooleanOption('W', "Verbose"); + CmdLineParser.Option portNameOption = parser.addStringOption('p', "portname"); + CmdLineParser.Option protocalOption = parser.addStringOption('a', "api"); + CmdLineParser.Option angleOption = parser.addIntegerOption("angle"); + CmdLineParser.Option velocityOption = parser.addIntegerOption('v', "velocity"); + CmdLineParser.Option distanceOption = parser.addIntegerOption('d', "distance"); + CmdLineParser.Option commandOption = parser.addStringOption('c', "command"); + CmdLineParser.Option radiusOption = parser.addIntegerOption('r', "radius"); + CmdLineParser.Option widthOption = parser.addIntegerOption('x', "width"); + CmdLineParser.Option heightOption = parser.addIntegerOption('y', "height"); + CmdLineParser.Option minlineOption = parser.addIntegerOption("min"); + CmdLineParser.Option maxlineOption = parser.addIntegerOption("max"); + CmdLineParser.Option videoServerOption = parser.addStringOption("videoServer"); + CmdLineParser.Option videoPortNumOption = parser.addIntegerOption("videoPortNum"); + CmdLineParser.Option thresholdOption = parser.addIntegerOption('t', "threshold"); + CmdLineParser.Option hwHandShakeOption = parser.addBooleanOption("nohwhandshake"); + try { + parser.parse(args); + } + catch ( CmdLineParser.OptionException e ) { + System.err.println(e.getMessage()); + System.out.println("parseCmd had an error\n"+ usage ); + System.exit(2); + } + + // String portname = args[0]; // e.g. "/dev/cu.KeySerial1", or "COM5" or "192.168.1.1" + setPortname((String)parser.getOptionValue(portNameOption)); + System.out.println("portname is ("+getPortname()+")"); + + setProtocol((String)parser.getOptionValue(protocalOption,"SCI")); + System.out.println("protocal is ("+getProtocol()+")"); + setAngle((Integer)parser.getOptionValue(angleOption,new Integer(83))); + System.out.println("angle is ("+getAngle()+")"); + setRadius((Integer)parser.getOptionValue(radiusOption,new Integer(0x8000))); + System.out.println("radius is ("+getRadius()+")"); + if (args[1].equals("SCI") || (args[1].equals("OI"))) { + } else { + } + setThresholdOverride(((Integer)parser.getOptionValue(thresholdOption, getThresholdOverride())).intValue()); + setWidth(((Integer)parser.getOptionValue(widthOption,getWidth())).intValue()); + setHeight(((Integer)parser.getOptionValue(heightOption,getHeight())).intValue()); + setMinLineWidth(((Integer)parser.getOptionValue(minlineOption,getMinLineWidth())).intValue()); + setMaxLineWidth(((Integer)parser.getOptionValue(maxlineOption,getMaxLineWidth())).intValue()); + setVideoServer(((String)parser.getOptionValue(videoServerOption))); + setVideoPortNum(((Integer)parser.getOptionValue(videoPortNumOption, getVideoPortNum())).intValue()); + // String cmd = args[1+argOffset]; + setCmd((String)parser.getOptionValue(commandOption,"fig8")); + if (getCmd().equalsIgnoreCase("followLine")){ + System.out.println("videoServer is ("+getVideoServer()+")"); + System.out.println("videoPortNum is ("+getVideoPortNum()+")"); + } + Integer velocityInt = (Integer)parser.getOptionValue(velocityOption,new Integer(0)); + Integer distanceInt = (Integer)parser.getOptionValue(distanceOption,new Integer(0)); + try { +// velocity = (int)(Integer.parseInt( args[2+argOffset])); +// distance = (int)(Integer.parseInt( args[3+argOffset] )); + setVelocity(velocityInt.intValue()); + System.out.println("velocity is ("+getVelocity()+")"); + setDistance(distanceInt.intValue()); + System.out.println("distance is ("+getDistance()+")"); + setWaittime(Math.abs((getDistance()/getVelocity()) * 900)); // waittime in ms (correction .9) + System.out.println("waittime is ("+getWaittime()+")"); + if (getWaittime() == 0) { + System.out.println("Invalid waittime "+getWaittime()); + } + if (getMinLineWidth() == 0) { + System.out.println("Invalid MinLineWidth "+getMinLineWidth()); + } else { + System.out.println("MinLineWidth is ("+getMinLineWidth()+")"); + } + if (getMaxLineWidth() == 0) { + System.out.println("Invalid MaxLineWidth "+getMaxLineWidth()); + } else { + System.out.println("MaxLineWidth is ("+getMaxLineWidth()+")"); + } + System.out.println("velocity: " + getVelocity() + " distance: " + getDistance() + " waittime: " + getWaittime()); + System.out.println("width: " + getWidth() + "height: " + getHeight() + "\n"); + } catch( Exception e ) { + System.err.println(e.getMessage()); + System.err.println("Couldn't parse velocity or distance2"); + System.exit(1); + } +// for( int i=4+argOffset; i < args.length; i++ ) { +// if( args[i].endsWith("debug") ) +// debug = true; +// } + Boolean debugBool = (Boolean)parser.getOptionValue(debugOption,new Boolean(false)); + setDebug(debugBool.booleanValue()); + System.out.println("debug is ("+isDebug()+")"); + Boolean hwHandShakeBool = (Boolean)parser.getOptionValue(hwHandShakeOption, new Boolean(false)); + setHwhandshake(hwHandShakeBool.booleanValue()); + setThresholdOverride((Integer)parser.getOptionValue(thresholdOption, new Integer(0))); + System.out.println("thresholdOverride is " + getThresholdOverride()); + System.out.println("hwHandShake is ("+isHwhandshake()+")"); + System.out.println("*** end of parseCmd"); + } + public int getRadius() { + return radius; + } + public void setRadius(int rad) { + radius = rad; + } + public int getDistance() { + return distance; + } + public void setDistance(int dist) { + distance = dist; + } + public boolean isHwhandshake() { + return hwhandshake; + } + public void setHwhandshake(boolean hwhandshake_) { + hwhandshake = hwhandshake_; + } + public int getAngle() { + return angle; + } + public void setAngle(int ang) { + angle = ang; + } + public boolean isDebug() { + return debug; + } + public void setDebug(boolean debug_) { + debug = debug_; + } + public String getPortname() { + return portname; + } + public void setPortname(String portname_) { + portname = portname_; + } + public String getCmd() { + return cmd; + } + public void setCmd(String cmd_) { + cmd = cmd_; + } + public int getVelocity() { + return velocity; + } + public void setVelocity(int velocity_) { + velocity = velocity_; + } + public int getWaittime() { + return waittime; + } + public void setWaittime(int waittime_) { + waittime = waittime_; + } + public String getProtocol() { + return protocol; + } + public void setProtocol(String protocol_) { + protocol = protocol_; + } + public class MyShutdown extends Thread { + RoombaComm roomba = null; + public MyShutdown(RoombaComm roomba){ + this.roomba = roomba; + } + public void run() { + System.out.println("MyShutdown hook called"); + if (roomba != null && roomba.isConnected()){ + System.out.println("roomba not null trying to stop and disconenct"); + roomba.stop(); + System.out.println("roomba stop issued"); + roomba.disconnect(); + System.out.println("roomba disconenct issued"); + }else{ + System.out.println("roomba was null or not connected"); + } + } + } + public int getThresholdOverride() { + return thresholdOverride; + } + public void setThresholdOverride(int thresholdOverride) { + this.thresholdOverride = thresholdOverride; + } + protected int getMaxLineWidth() { + return maxLineWidth; + } + protected void setMaxLineWidth(int maxLineWidth) { + this.maxLineWidth = maxLineWidth; + } + protected int getMinLineWidth() { + return minLineWidth; + } + protected void setMinLineWidth(int minLineWidth) { + this.minLineWidth = minLineWidth; + } + protected int getHeight() { + return height; + } + protected void setHeight(int height) { + this.height = height; + } + protected int getWidth() { + return width; + } + protected void setWidth(int width) { + this.width = width; + } + /** + * @return the videoServer + */ + protected String getVideoServer() { + return videoServer; + } + /** + * @param videoServer the videoServer to set + */ + protected void setVideoServer(String videoServer) { + this.videoServer = videoServer; + } + /** + * @return the videoPortNum + */ + protected int getVideoPortNum() { + return videoPortNum; + } + /** + * @param videoPortNum the videoPortNum to set + */ + protected void setVideoPortNum(int videoPortNum) { + this.videoPortNum = videoPortNum; + } + + } + + \ No newline at end of file -- cgit v1.3.1