public static int readLine(
InputStream in, OutputStream out, Display display, boolean echo, byte[] buf, int maxLen) {
// get a line from the input stream into the buffer buf, of max length maxLen
// line is terminated with a return character;
// echo back to output stream
// also write to line 0 of the display, up to 16 characters
try {
int data;
int len = 0;
maxLen--; // make room for a null byte
while ((data = in.read()) != -1) {
if (data == 0) continue;
if (data == '\n') continue;
if (echo) out.write(data); // echo it back
byte ch = (byte) data;
if (ch == '\r') {
if (echo) out.write('\n');
int dlen = len; // LCD panel display length
if (dlen > 16) dlen = 16;
byte[] screenBuf = new byte[dlen];
for (int i = 0; i < dlen; i++) screenBuf[i] = buf[i];
display.print(0, screenBuf); // echo it to the display
buf[len] = 0;
return len;
} else if (len < maxLen) {
buf[len++] = ch;
}
}
} catch (Exception e) {
display.print(0, "oops!");
}
return -1;
}
public static void main(String args[]) {
Display display = IntelliBrain.getLcdDisplay();
IBB ibb = new IBB();
try {
SerialPort comPort = IntelliBrain.getCom1();
// Serial Parameters
comPort.setSerialPortParams(
38400, SerialPort.DATABITS_8, SerialPort.STOPBITS_1, SerialPort.PARITY_NONE);
InputStream inputStream = comPort.getInputStream();
OutputStream outputStream = comPort.getOutputStream();
// clear screen
display.print(0, "");
display.print(1, "");
// test the readLine routine
byte[] buf = new byte[128];
while (true) {
int len = readLine(inputStream, outputStream, display, true, buf, 128);
if (len == 0) continue;
switch (buf[0]) {
case 'B':
ibb.getVersion(outputStream, buf, len);
break;
case 'D':
ibb.setSpeed(outputStream, buf, len);
break;
case 'H':
ibb.sysBeep(outputStream, buf, len);
break;
case 'L':
ibb.setLedState(outputStream, buf, len);
break;
case 'N':
ibb.readProximitySensors(outputStream, buf, len);
break;
case 'O':
ibb.readLightSensors(outputStream, buf, len);
break;
case 'Z':
ibb.motorsOff(outputStream, buf, len);
break;
}
}
} catch (Exception e) {
e.printStackTrace();
}
}
public static void run(String args[]) {
try {
System.out.println("WheelWatcher WW-01");
// Get the user interface objects
Display display = IntelliBrain.getLcdDisplay();
AnalogInput thumbwheel = IntelliBrain.getThumbWheel();
// Get the servo object and give it a Motor interface by encapsulating it
// in a ContinuousRotationServo object.
Motor motor = new ContinuousRotationServo(IntelliBrain.getServo(3), true);
motor.setPower(1);
motor.setPower(0);
motor.setPower(-1);
Thread.sleep(2000);
// Get a shaft encoder object and initialize it with the two digital inputs it uses.
IntelliBrainShaftEncoder encoder = IntelliBrain.getShaftEncoder(1);
encoder.initialize(IntelliBrain.getDigitalIO(11), IntelliBrain.getDigitalIO(10));
int previousCounts = 0;
int power = 0;
int rpm = 0;
long time = System.currentTimeMillis();
while (true) {
// Calculate RPM (Revolutions Per Minute)
// RPM = countsThisIteration * iterationsPerMinute / countsPerRevolution
// countsThisIteration - current encoder counts minus previous count
// iterationsPerMinute = 600 - ten loops per second
// countsPerRevolution = 128 - fixed by design of sensor/codewheel
int counts = encoder.getCounts();
display.print(1, "RPM: " + ((counts - previousCounts) * 600) / 128);
previousCounts = counts;
// Read the thumbwheel and scale the value to set the motor power.
int velocity = (thumbwheel.sample() - 512) / 31;
while (velocity == 3 || velocity == 4) {
if (velocity == 3) velocity = (thumbwheel.sample() - 512) / 31;
else velocity = -(thumbwheel.sample() - 512) / 31;
// counts and rpm logic
counts = encoder.getCounts();
previousCounts = counts;
rpm = ((counts - previousCounts) * 600) / 128;
display.print(0, "RPM: " + rpm);
// governer logic
power += (velocity - (rpm / 16.25));
// limiter
if (power > 16) power = 16;
if (power < -16) power = -16;
display.print(0, "Power: " + power);
// set power
motor.setPower(power);
time += 100;
Thread.sleep(time - System.currentTimeMillis());
}
motor.setPower(velocity);
display.print(0, "Power: " + velocity);
// Calculate sleep time so the next iteration starts 1/2 second after the
// the previous one.
time += 100;
Thread.sleep(time - System.currentTimeMillis());
}
} catch (Throwable t) {
// Display a stack trace if an exception occurs.
t.printStackTrace();
}
}