Page 139 - The Definitive Guide to Building Java Robots

Preston_5564C04.fm Page 120 Wednesday, October 5, 2005 7:22 AM

120 CHAPTER 4 ■ SENSORS

Example 4-2. CompassStamp.java
package com.scottpreston.javarobot.chapter5;

import com.scottpreston.javarobot.chapter2.Controller;
import com.scottpreston.javarobot.chapter2.JSerialPort;
import com.scottpreston.javarobot.chapter2.SingleSerialPort;

public class CompassStamp extends Controller {

// commands set in basic stamp program
public static final int CMD_INIT = 100;
public static final int CMD_DINSMORE = 101;
public static final int CMD_DEVANTECH = 102;
public static final int CMD_VECTOR = 103;

// default reading for compass
private int compass = CMD_DEVANTECH;

// constructor
public CompassStamp(JSerialPort sPort) throws Exception {
super(sPort);
}

public int getHeading(int compass) throws Exception {
setCompass(compass);
return getHeading();
}

// get heading method
public int getHeading() throws Exception {
// calling super execute() method
String heading = execute(new byte[] { CMD_INIT, (byte) compass },
getCompassDelay());
// since returning heading as one, two or three bytes
String[] h2 = heading.split("~");
String heading2 = "";
for (int h = 0; h < h2.length; h++) {
// convert each byte to char which I append to create single number
heading2 = heading2 + (char) new Integer(h2[h]).intValue();
}
// return 3 chars like '123' which is 123 degrees
return new Integer(heading2).intValue();
}

public int getDinsmore() throws Exception{
return getHeading(CMD_DINSMORE);
}