From 53b947df484159f5934898ba6c0aa69a9c869007 Mon Sep 17 00:00:00 2001 From: Ido Hadanny Date: Thu, 22 Sep 2011 21:30:13 +0300 Subject: added stuff --- .../com/hackingroomba/roombacomm/ArduinoBot.java | 379 +++++++++++++++++++++ 1 file changed, 379 insertions(+) create mode 100644 roombacomm-client/src/com/hackingroomba/roombacomm/ArduinoBot.java (limited to 'roombacomm-client/src/com/hackingroomba/roombacomm/ArduinoBot.java') diff --git a/roombacomm-client/src/com/hackingroomba/roombacomm/ArduinoBot.java b/roombacomm-client/src/com/hackingroomba/roombacomm/ArduinoBot.java new file mode 100644 index 0000000..d029413 --- /dev/null +++ b/roombacomm-client/src/com/hackingroomba/roombacomm/ArduinoBot.java @@ -0,0 +1,379 @@ +/* + * roombacomm.ArduinoClient -- test out the Arduino subsystem without robot motion + * + * 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 com.hackingroomba.roombacomm.RobotConnection.ReadTerminator; +import com.hackingroomba.roombacomm.RobotType.robotTypes; + +import java.io.*; + +/** + * The extensions to roombacomm for ArduinoBot operations. + * + *

Overview

+ * This class contains the operations available from an ArduinoBot +**/ + +public class ArduinoBot extends RoombaComm { + + // connection variables + private Integer compassHeading; + private String arduinoString; + private RobotConnection robotConnection; + private RobotType robotType; + + // compass variables + private int compassOffset = 0; + private boolean compassOffsetInitialized = false; + + // encoder variables + private int encoder; + + private long startTime; + public long elapsedTime; + + public ArduinoBot() { + super(); + } + + public ArduinoBot(RobotConnection robotConnection) { + super(robotConnection); + this.robotConnection = robotConnection; + } + + /** + * Initialize an Arduino robot controller + */ + public boolean initArduinoBot(RobotType rt) + { + byte[] robotTypeMsg = "robot m\r".getBytes(); // message to Arduino to tell it the robot type + + robotType = rt; // record robotType for later use + System.out.println("checking for ArduinoBot... \n"); + try { + Thread.sleep(2000); + } catch (Exception e) {System.out.println("Exception in Thread.sleep()"); } + + // send the robot type message to Arduino, so it knows how to read the encoders, and any other robot-specific things + if (robotType.robotType == RobotType.robotTypes.frankenRoomba) { + System.out.println("Sending robot r to Arduino"); + robotTypeMsg = "robot r\r".getBytes(); + } else if (robotType.robotType == robotTypes.tankbot) { + System.out.println("Sending robot t to Arduino"); + robotTypeMsg = "robot t\r".getBytes(); + } + send(robotTypeMsg); + + printCompass(); + printEncoders(); + return true; + } + + + /** + * Send a single byte to the ArduinoBot + * (defined as int because of stupid java signed bytes). SendToArduino method of RobotConnection + * is used because this is an ardunobot, which requires a leading m and trailing to make + * the parser work right + * @param b byte of an Arduino command to send + * @return true on successful send + */ + public boolean send(int b) { + return(robotConnection.sendToArduino(b)); + } + + /** + * Send a byte array to the ArduinoBot. If the first byte is 128 or higher, preface the byte string with m + * so it's treated by Arduino as a roomba command + */ + public boolean send(byte[] bytes) { + if (bytes[0] < 0) // must be a roomba command + return(robotConnection.sendToArduino(bytes)); + else + return(robotConnection.send(bytes)); + } + + /** + * Read a string from Arduino up to a + * @return array of bytes read from ArduinoBot + */ + public String readArduinoBot() throws Exception { + return(robotConnection.readBotToTerminator(ReadTerminator.LF)); + } + + public void printCompass() { + int compassReading; + startTime = System.currentTimeMillis(); + compassReading = getCompass(); + if (compassReading < 0) return; + elapsedTime = System.currentTimeMillis() - startTime; + System.out.println("Heading: " + compassReading + " in " + elapsedTime + " ms"); + } + + public int getCompass() { + // read the uncorrected heading + try { + compassHeading = getUncorrectedCompass(); + } catch (Exception e) { + System.out.println("Exception reading uncorrected compass\n" + e.getMessage() + "\n" ); + e.printStackTrace(); + return -1; + } + // if we haven't initialized the compass offset yet, try to read an offset from a config file + // compassOffsetInitialized ensures we only try this once + try { + if (!compassOffsetInitialized) { // get the compass offset from file if it exists + compassOffset = readConfigInt(new String("compass_offset")); + compassOffsetInitialized = true; + } + } catch (Exception e) { + System.out.println("No compass_offset file could be opened\n" + e.getMessage()); + compassOffsetInitialized = true; + } + + // offset the uncorrected heading by the compassOffset, which depends on mounting position + compassHeading -= compassOffset; + compassHeading %= 360; // compensate for offset which can make it exceed 360 + if (compassHeading < 0) compassHeading += 360; + return compassHeading; + } + + private int getUncorrectedCompass() throws Exception { + byte [] compassCmd = {'c', '\r'}; + int heading; + + //System.out.println("getUncorrectedCompass"); + startTime = System.currentTimeMillis(); + if (!send(compassCmd)) { + System.out.println("Error sending c command"); + } + try { + arduinoString = robotConnection.readBotToTerminator(ReadTerminator.LF); + if (arduinoString == null) { + System.out.println("Error: getUncorrectedCompass failed to read heading - null string"); + } + } catch (Exception e) { + elapsedTime = System.currentTimeMillis() - startTime; + System.err.println("Exception reading compass in readBotToTerminator in " + elapsedTime + " ms"); + e.printStackTrace(); + throw e; + } + elapsedTime = System.currentTimeMillis() - startTime; + + //System.out.println("received string: " + arduinoString); + String headingString = arduinoString.split("\\s")[0]; + heading = new Integer(headingString); + return heading; + } + + public boolean setCompassOffset(int currentMagHeading) + { + int heading; + + if ((currentMagHeading < 0) || (currentMagHeading > 360)) { + System.out.println("currentMagHeading must be between 0 and 360"); + return false; + } + try { + heading = getUncorrectedCompass(); + if (heading < 0) return false; + compassOffset = heading - currentMagHeading; + if (compassOffset < 0) compassOffset += 360; + writeConfigInt(new String("compass_offset"), compassOffset); + } catch (Exception e) { + System.err.println("Error getting compass or writing compass offset"); + e.printStackTrace(); + return false; + } + compassOffsetInitialized = true; + return true; + } + + /* + * Print, Get the current wheel encoders reading + */ + public void printEncoders() + { + double readEncDistance = 0; + + startTime = System.currentTimeMillis(); + try { + readEncDistance = getEncoders(); + } catch (Exception e) { + System.err.println("Exception reading robot encoders: " + e.getMessage()); + } + long elapsedTime = System.currentTimeMillis() - startTime; + System.out.println("Wheel encoders reading: " + readEncDistance + " in " + elapsedTime + " ms"); + + } + + public double getEncoders() throws Exception { + byte [] cmd = {'e', '\r'}; + + //System.out.println("read encoders"); + startTime = System.currentTimeMillis(); + if (!send(cmd)) { + System.out.println("Error sending e command"); + } + try { + arduinoString = robotConnection.readBotToTerminator(ReadTerminator.LF); + elapsedTime = System.currentTimeMillis() - startTime; + if (arduinoString == null) { + System.err.println("Error: getEncoders failed to read encoders, null returned in " + elapsedTime + " ms"); + } + } catch (Exception e) { + elapsedTime = System.currentTimeMillis() - startTime; + System.err.println("Exception reading encoders from readBotToTerminator in " + elapsedTime + " ms"); + e.printStackTrace(); + throw e; + } + + //System.out.println("received string: " + arduinoString); + String [] encoderStrings = arduinoString.split("\\s"); + if (encoderStrings.length == 1) { + encoder = new Integer(encoderStrings[0]); + } else { + System.out.println("Error reading encoders - expected 1 string, found:" + encoderStrings.length + + " in: " + arduinoString + " in " + elapsedTime + " ms"); + return -1; + } + + double encoderDistance = encoder /robotType.countsPerInch; + return encoderDistance; + } + + + /** + * Read roomba 26-byte sensor record using robotConnection. Tries once to read valid data, allowing 100ms + * timeout on each attempt. + * @return true if read 26 bytes of valid data. Data has been stored in sensor_bytes. False otherwise + */ + public boolean updateSensors() + { + return updateSensors(SENSORS_ALL); + } + + public boolean updateSensors(int sensorGroup) + { + int sensorGroupSize; + + if (robotConnection == null) { + System.out.println("Error at ArduinoBot.updateSensors(): no connection object for robot"); + return false; + } + + switch(sensorGroup) { + case SENSORS_ALL: sensorGroupSize = 26; break; + case 100: sensorGroupSize = 80; break; + default: + System.err.println("Invalid sensor group in updateSensors(): " + sensorGroup); + return false; + } + sensors(sensorGroup); + return getSensorData(sensorGroupSize); + } + + /** + * Writes an int to the named file + * @throws FileNotFoundException If it can't open the file + * @throws IOException If it can't write to the file + */ + public void writeConfigInt(String filename, int value) throws FileNotFoundException, IOException { + DataOutputStream dos = new DataOutputStream(new FileOutputStream(new File(filename))); + dos.writeInt(value); + } + + /** + * Read a configuration file containing an int. + * @param filename The file to open & read an int from + * @return Int read from file + * @throws Exception related to attempting to open & read int from file + */ + public int readConfigInt(String filename) throws Exception { + int configInt = -1; + + try { + DataInputStream dis = new DataInputStream(new FileInputStream(new File(filename))); + configInt = dis.readInt(); + + } catch (Exception e) { + System.out.println("No config file could be read\n" + e.getMessage()); + } + return configInt; + } + + + /** + * Writes a double to the named file + * @throws FileNotFoundException If it can't open the file + * @throws IOException If it can't write to the file + */ + public void writeConfigDouble(String filename, double value) throws FileNotFoundException, IOException { + DataOutputStream dos = new DataOutputStream(new FileOutputStream(new File(filename))); + dos.writeDouble(value); + } + + /** + * Read a configuration file containing a double. + * @param filename The file to open & read a double from + * @return double read from file + * @throws Exception related to attempting to open & read double from file + */ + public double readConfigDouble(String filename) throws Exception { + double configDouble = -1; + + try { + DataInputStream dis = new DataInputStream(new FileInputStream(new File(filename))); + configDouble = dis.readDouble(); + + } catch (Exception e) { + System.out.println("No config file could be read\n" + e.getMessage()); + throw(e); + } + return configDouble; + } + + public int getEncoder() { + return encoder; + } + + /** + * Bogus methods which should never be called on ArduinoClient + */ + public String [] listPorts() + { + System.out.println("Error at ArduinoBot.listPorts(): should never get here"); + return (new String[] {"Error: listports calledon ArdunoBot; not supported"}); + } + public boolean connect(String s) + { + System.out.println("Error at ArduinoBot.connect(): should never get here"); + return false; + } + public void disconnect() + { + System.out.println("Error at ArduinoBot.disconnect(): should never get here"); + return; + } + +} \ No newline at end of file -- cgit v1.3.1