Page 1 of 1

Reactive Sumo Robot

Posted: Sat Mar 14, 2009 1:33 pm
by nor0101
Hey everyone,

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();
        }
    }
 
}
 
Auxiliary RCX Unit:

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();
            }
        }
    }
}
Thanks for looking!

Re: Reactive Sumo Robot

Posted: Mon Mar 16, 2009 12:57 am
by omniuni
Cool! Good job! I'll also keep that alternative in mind for my current robotics course.