For a robotics project at college we made sumo wrestling robots using the Lego Mindstorms RCX 2.0 microcontroller. I ended up using a 3rd party firmware called LeJOS, which is essentially a JVM and supporting robotics packages for the RCX. My submission won the final contest, but not without a struggle. Full video of the event is available here: http://www.youtube.com/view_play_list?p ... DD793F6A82. My bot is the big square one with the flashing lights.
It has a couple of complex features:
- Lidar object detection
- Bidirectional synchronization between two cooperating RCX microtontrollers, using the built-in serial IR ports and a system of mirrors.
The code follows, it's fully synchronized; the Action class is a subclass of java.lang.Thread.
Main RCX unit:
Code: Select all
import josx.platform.rcx.*;
import josx.robotics.*;
public class Gobot {
// Auxiliary Motor Pattern Constants:
private final byte mp_forward = (byte) 0x10;
private final byte mp_backward = (byte) 0x20;
private final byte mp_cw = (byte) 0x30;
private final byte mp_ccw = (byte) 0x40;
private final byte mp_left = (byte) 0x50;
private final byte mp_right = (byte) 0x60;
private final byte mp_nowhere = (byte) 0x70;
private int rightEye=0, leftEye=0;
private VisualCortex vis;
private StayOnRing sor;
private Attack att;
private Explore exp;
private boolean sawObjectRecently = false;
private boolean alignedWithDiameter = false;
public static void main(String[] args) throws Exception {
new Gobot();
}
private Gobot() throws Exception {
vis = new VisualCortex(this);
sor = new StayOnRing(this);
att = new Attack(this);
exp = new Explore(this);
// sor must respond to Serial events
Serial.addSerialListener(sor);
// maximum motor output
Motor.A.setPower(7);
Motor.B.setPower(7);
// set thread priorities
initializePriorities();
// mandatory pause at match start
Sound.beep();
Thread.sleep(1000);
Sound.beep();
Thread.sleep(1000);
Sound.beep();
Thread.sleep(1000);
Sound.beep();
Thread.sleep(1000);
Sound.beep();
Thread.sleep(999);
// wrassle!
vis.start();
sor.start();
att.start();
exp.start();
// hang out here for a while
Button.RUN.waitForPressAndRelease();
powerSave();
System.exit(0);
}
private void initializePriorities() {
int basePriority = Thread.currentThread().getPriority();
vis.setPriority(basePriority);
sor.setPriority(basePriority + 1);
att.setPriority(basePriority);
exp.setPriority(basePriority);
}
public synchronized void goForward() {
sendAuxMotorPattern(mp_forward);
Motor.A.forward();
Motor.B.forward();
}
public synchronized void goBackward() {
sendAuxMotorPattern(mp_backward);
Motor.A.backward();
Motor.B.backward();
}
public synchronized void goCW() {
sendAuxMotorPattern(mp_cw);
Motor.A.forward();
Motor.B.backward();
}
public synchronized void goCCW() {
sendAuxMotorPattern(mp_ccw);
Motor.A.backward();
Motor.B.forward();
}
public synchronized void goLeft() {
sendAuxMotorPattern(mp_left);
Motor.A.forward();
Motor.B.stop();
}
public synchronized void goRight() {
sendAuxMotorPattern(mp_right);
Motor.A.stop();
Motor.B.forward();
}
public synchronized void goNowhere() {
sendAuxMotorPattern(mp_nowhere);
Motor.A.stop();
Motor.A.stop();
}
private byte[] packet = {(byte)0xf7, (byte)0x00};
private synchronized void sendAuxMotorPattern(byte mp) {
packet[1] = mp;
Serial.sendPacket(packet, 0, 2);
Sound.beep();
LCD.showNumber((int) packet[1]);
try {
Serial.waitTillSent();
}
catch(InterruptedException e) {
// pause() interrupted
}
}
public synchronized void setEyes(int l, int r) {
leftEye = l;
rightEye = r;
if (getLeftEye() > 1 || getRightEye() > 1) {
att.doIt();
sawObjectRecently = true;
}
else {
exp.doIt();
sawObjectRecently = false;
}
}
public synchronized int getLeftEye() {
return leftEye;
}
public synchronized int getRightEye() {
return rightEye;
}
public boolean getSawObjectRecently () {
return sawObjectRecently;
}
public void setAlignedWithDiameter(boolean awd) {
alignedWithDiameter = awd;
}
public boolean getAlignedWithDiameter() {
return alignedWithDiameter;
}
protected void powerSave() {
goNowhere();
Sensor.S1.passivate();
Sensor.S2.passivate();
Sensor.S3.passivate();
}
/**
VisualCortex
Is a high-priority background task.
Mostly consists of a non-terminating loop doing the following:
1. Take a light measurement for each of the "eyes"
2. Turn the brights on
3. Sleep for 50ms
4. Take a second light measurement for each "eye"
5. Give the resistance differences back to Gobot
6. Sleep for 50ms
*/
private class VisualCortex extends Activity implements SensorConstants {
Gobot gobot;
public VisualCortex(Gobot g) {
gobot = g;
// set light rack to full power
Motor.C.setPower(7);
// set forward light sensors' mode
Sensor.S1.setTypeAndMode(SENSOR_TYPE_LIGHT, SENSOR_MODE_RAW);
Sensor.S2.setTypeAndMode(SENSOR_TYPE_LIGHT, SENSOR_MODE_RAW);
}
protected void action() throws StopException {
try {
Sensor.S1.activate();
Sensor.S2.activate();
pause(50);
int l1, l2, r1, r2;
for (;;) {
r1 = Sensor.S1.readRawValue();
l1 = Sensor.S2.readRawValue();
Motor.C.forward();
pause(50);
r2 = Sensor.S1.readRawValue();
l2 = Sensor.S2.readRawValue();
Motor.C.stop();
gobot.setEyes(l1-l2, r1-r2);
pause(50);
}
}
catch (InterruptedException e) {
// pause() was interrupted, restart
}
}
// this is a background activity;
// we want to do our thing and pause()
// out of the way.
protected void resetRunnable() {
this.iWantToRun();
}
}
private class StayOnRing extends Activity implements SerialListener {
Gobot gobot;
private byte edgeState = (byte) 0x00;
public StayOnRing(Gobot g) {
gobot = g;
}
public void packetAvailable(byte[] packet, int length) {
edgeState = packet[1];
if (edgeState < (byte)0x10) {
this.iWantToRun();
}
}
protected void action() throws StopException {
// RIGHT REAR EDGE = 1
// LEFT REAR EDGE = 2
// FRONT CENTER EDGE = 4
// 3 bits with two discrete states. no need to consider
// the 0 case. yields 7 possible values for edgeState
try {
if (edgeState == (byte) 0x01) {
gobot.goLeft();
pause(50);
} else if (edgeState == (byte) 0x02) {
gobot.goRight();
pause(50);
} else if (edgeState == (byte) 0x03) {
gobot.goForward();
gobot.setAlignedWithDiameter(true);
pause(50);
} else if (edgeState == (byte) 0x04) {
gobot.goBackward();
pause(50);
} else if (edgeState == (byte) 0x05) {
gobot.goCCW();
pause(50);
} else if (edgeState == (byte) 0x06) {
gobot.goCW();
pause(50);
} else if (edgeState == (byte) 0x07) {
gobot.goNowhere();
pause(50);
}
}
catch (InterruptedException e) {
// pause() was interrupted...
// should not happen because of the high
// priority of the StayOnRing thread.
}
}
}
private class Attack extends Activity {
Gobot gobot;
public Attack(Gobot g) {
gobot = g;
}
protected void action() {
// must see something reflective,
// get it!
gobot.goForward();
}
public void doIt() {
this.iWantToRun();
}
}
private class Explore extends Activity {
Gobot gobot;
private final short num_cycles = 45;
private short cycleCounter = 0;
private final short num_aligned_cycles = 8;
private short alignedCycleCounter = 0;
private boolean CW = true;
public Explore(Gobot g) {
gobot = g;
}
protected void action() throws StopException {
if (gobot.getAlignedWithDiameter()) {
// "explore" in the forward direction for a while.
gobot.goForward();
alignedCycleCounter++;
if (alignedCycleCounter > num_aligned_cycles) {
gobot.setAlignedWithDiameter(false);
alignedCycleCounter = 0;
}
}
else if (gobot.getSawObjectRecently()) {
// "explore" in the forward direction this time.
// supresses mistracking due to sensor jitter
gobot.goForward();
}
// spin CW, or spin CCW
else if (CW) {
gobot.goCW();
}
else {
gobot.goCCW();
}
// count this exploration cycle
++cycleCounter;
if (cycleCounter > num_cycles) {
// toggle CW/CCW
CW = !CW;
cycleCounter = 0;
}
}
public void doIt() {
this.iWantToRun();
}
}
}
Code: Select all
import josx.platform.rcx.*;
import josx.robotics.*;
public class GobotAux implements SensorConstants {
// Auxiliary Motor Pattern Constants:
private final byte mp_forward = (byte) 0x10;
private final byte mp_backward = (byte) 0x20;
private final byte mp_cw = (byte) 0x30;
private final byte mp_ccw = (byte) 0x40;
private final byte mp_left = (byte) 0x50;
private final byte mp_right = (byte) 0x60;
private final byte mp_nowhere = (byte) 0x70;
/*
mandatory 5 second waiting period
read initial values, compare average of S1 && S2 with S3.
(S1 && S2 begin on white border, S3 begins on black).
from this, calculate percentage threshold.
forever:
poll the downward-facing light sensors
if S1 == SEEING_WHITE ||
S2 == SEEING_WHITE ||
S3 == SEEING_WHITE
then sendUnsafeState();
wait(25 millis)
meanwhile, listen for incoming bytes from serial IR port.
when byte received, decode it and set auxilliary motor
power accordingly.
*/
public static void main(String[] args) throws Exception {
GobotAux ga = new GobotAux();
}
private GobotAux() throws Exception {
// maximum motor output
Motor.A.setPower(7);
Motor.B.setPower(7);
// setup motor aux control listener
AuxiliaryMotorControl amc = new AuxiliaryMotorControl(this);
amc.setPriority(Thread.currentThread().getPriority() + 1);
// amc must respond to Serial events
Serial.addSerialListener(amc);
// setup edge monitor
EdgeMonitor em = new EdgeMonitor(this);
em.setPriority(Thread.currentThread().getPriority());
// set sensor mode.
Sensor.S1.setTypeAndMode(SENSOR_TYPE_LIGHT, SENSOR_MODE_PCT);
Sensor.S2.setTypeAndMode(SENSOR_TYPE_LIGHT, SENSOR_MODE_PCT);
Sensor.S3.setTypeAndMode(SENSOR_TYPE_LIGHT, SENSOR_MODE_PCT);
// set to active sensing.
Sensor.S1.activate();
Sensor.S2.activate();
Sensor.S3.activate();
Thread.sleep(4999);
// start auxiliary motor control listener
amc.start();
// get initial readings.
int S1_0 = Sensor.S1.readValue();
int S2_0 = Sensor.S2.readValue();
int S3_0 = Sensor.S3.readValue();
int w_avg = (S1_0 + S2_0) / 2;
int thresh = w_avg - ((w_avg - S3_0) / 2);
// start edge monitor
em.setThreshVals(w_avg, thresh);
em.start();
// hang out here for a while:
Button.RUN.waitForPressAndRelease();
powerSave();
System.exit(0);
}
protected void powerSave() {
Motor.A.stop();
Motor.B.stop();
Motor.C.stop();
Sensor.S1.passivate();
Sensor.S2.passivate();
Sensor.S3.passivate();
}
private byte[] packet = {(byte)0xf7, (byte)0x00};
private void sendUnsafeState(byte edgeState) {
packet[1] = edgeState;
Serial.sendPacket(packet, 0, 2);
try {
Serial.waitTillSent();
}
catch(InterruptedException e) {
// was interrupted
}
}
private void goForward() {
Motor.A.forward();
Motor.B.forward();
}
private void goBackward() {
Motor.A.backward();
Motor.B.backward();
}
private void goCW() {
Motor.A.forward();
Motor.B.backward();
}
private void goCCW() {
Motor.A.backward();
Motor.B.forward();
}
private void goLeft() {
Motor.A.forward();
Motor.B.stop();
}
private void goRight() {
Motor.A.stop();
Motor.B.forward();
}
private void goNowhere() {
Motor.A.stop();
Motor.B.stop();
}
private class EdgeMonitor extends Activity {
private int w_avg=0, thresh=0;
private GobotAux gobot_aux;
public EdgeMonitor(GobotAux ga) {
gobot_aux = ga;
}
public void setThreshVals(int w, int t) {
w_avg = w;
thresh = t;
}
protected void action() throws StopException {
for(;;) {
byte edgeState = 0x00;
if (Sensor.S1.readValue() > thresh) {
edgeState += 0x01;
}
if (Sensor.S2.readValue() > thresh) {
edgeState += 0x02;
}
if (Sensor.S3.readValue() > thresh) {
edgeState += 0x04;
}
if (edgeState != 0x00) {
gobot_aux.sendUnsafeState(edgeState);
}
try {
pause(50);
}
catch (InterruptedException e) {
// pause() was interrupted
}
}
}
protected void resetRunnable() {
this.iWantToRun();
}
}
private class AuxiliaryMotorControl extends Activity implements SerialListener {
private GobotAux gobot_aux;
private byte mp;
public AuxiliaryMotorControl(GobotAux ga) {
gobot_aux = ga;
}
public void packetAvailable(byte[] packet, int length) {
if ((mp = packet[1]) > (byte) 0x0F) {
Sound.beep();
LCD.showNumber((int) mp);
this.iWantToRun();
}
}
protected void action() throws StopException {
// for testing
LCD.showNumber((int) mp);
if (mp == mp_forward) {
gobot_aux.goForward();
}
else if (mp == mp_backward) {
gobot_aux.goBackward();
}
else if (mp == mp_cw) {
gobot_aux.goCW();
}
else if (mp == mp_ccw) {
gobot_aux.goCCW();
}
else if (mp == mp_left) {
gobot_aux.goLeft();
}
else if (mp == mp_right) {
gobot_aux.goRight();
}
else if (mp == mp_nowhere) {
gobot_aux.goNowhere();
}
}
}
}