Reactive Sumo Robot

Coding Critique is the place to post source code for peer review by other members of DevNetwork. Any kind of code can be posted. Code posted does not have to be limited to PHP. All members are invited to contribute constructive criticism with the goal of improving the code. Posted code should include some background information about it and what areas you specifically would like help with.

Popular code excerpts may be moved to "Code Snippets" by the moderators.

Moderator: General Moderators

Post Reply
User avatar
nor0101
Forum Commoner
Posts: 53
Joined: Thu Jan 15, 2009 12:06 pm
Location: Wisconsin

Reactive Sumo Robot

Post 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!
User avatar
omniuni
Forum Regular
Posts: 738
Joined: Tue Jul 15, 2008 10:50 pm
Location: Carolina, USA

Re: Reactive Sumo Robot

Post by omniuni »

Cool! Good job! I'll also keep that alternative in mind for my current robotics course.
Post Reply