RoboSlam Testing Code and hex file

This program implements 5 selectable behaviours:

  1. blink – leave P1.1-3 unconnected (pins 3-6)
  2. zig-zag – ground P1.1 (pin 3)
  3. distance checker – ground P1.2 (pin 4)
  4. sumo – ground P1.3 (pin 5)
  5. avoid – ground P1.4 (pin 6)

Compiled program in two formats:

Here’s the full C code for MSPGCC:

//
// CAO Open Day 2015 - Pre-programmed behaviour for mini RoboSlam
// Code is for MSP430G2553 or MSP430G2452
// Written by Ted Burke - Last updated 28-3-2015
//
// There are 5 behaviours, each of which comprises several states:
// 
//    blink, zig-zag, distance checker, sumo, avoid
//
// States:
//
//   behaviour: blink
//      01. green LED on
//      02. red LED on
//
//   behaviour: zigzag
//      11. zig forward
//      12. right turn
//      13. zag forward
//      14. turn left
//
//   behaviour: distance checker
//      21. near
//      22. far
//
//   behaviour: sumo
//      31. search
//      32. charge
//
//   behaviour: avoid
//      41. forward
//      42. reverse
//      43. spin
//

#include <msp430.h>

// function prototype
int distance();

// LED and motor control function definitions
void stop()      {P2OUT = 0b00000000;}
void forward()   {P2OUT = 0b00000101;}
void reverse()   {P2OUT = 0b00001010;}
void spinleft()  {P2OUT = 0b00001001;}
void spinright() {P2OUT = 0b00000110;}
void leds(int n) {P1OUT = 0b00011110 + n;}

// define pin constants
#define GREEN_LED 0b00000001
#define RED_LED   0b10000000
#define TRIG_PIN  0b01000000
#define ECHO_PIN  0b00100000

// main function
int main( void )
{
    // variables for distance and distance counter
    int d, dc = 0;
    
    // disable watchdog timer to prevent time out reset
    WDTCTL = WDTPW + WDTHOLD;
 
    // configure digital inputs and outputs
    P1OUT = 0b00000000;
    P1REN = 0b00011110; // enable pull-downs on P1.1-4
    P1DIR = GREEN_LED + RED_LED + TRIG_PIN;
    P2DIR = 0b00001111; // P2.0-1 left motor, P2.2-3 right motor
    
    // main loop implements state machine
    int state = 1;
    while(1)
    {
        // update distance variable and counter
        d = distance();
        if (d < 50) dc = 8;
        else if (dc > 0) dc--;
        
        // blink behaviour
        if (state == 1) // green LED on
        {
            if      ((P1IN & 0b00011110) == 0b00011100) state = 11;
            else if ((P1IN & 0b00011110) == 0b00011010) state = 21;
            else if ((P1IN & 0b00011110) == 0b00010110) state = 31;
            else if ((P1IN & 0b00011110) == 0b00001110) state = 41;
            else
            {
                leds(GREEN_LED);
                stop();
                __delay_cycles(500000);
                state = 2;
            }
        }
        else if (state == 2) // red LED on
        {
            leds(RED_LED);
            stop();
            __delay_cycles(500000);
            state = 1;
        }
        
        // zigzag behaviour
        else if (state == 11) // zig forward
        {
            if ((P1IN & 0b00011110) != 0b00011100) state = 1;
            else
            {
                leds(GREEN_LED + RED_LED);
                forward();
                __delay_cycles(2000000);
                state = 12;
            }
        }
        else if (state == 12) // turn left
        {
            if ((P1IN & 0b00011110) != 0b00011100) state = 1;
            else
            {
                leds(GREEN_LED);
                spinleft();
                __delay_cycles(1000000);
                state = 13;
            }
        }
        else if (state == 13) // zag forward
        {
            if ((P1IN & 0b00011110) != 0b00011100) state = 1;
            else
            {
                leds(GREEN_LED + RED_LED);
                forward();
                __delay_cycles(2000000);
                state = 14;
            }
        }
        else if (state == 14) // turn right
        {
            if ((P1IN & 0b00011110) != 0b00011100) state = 1;
            else
            {
                leds(RED_LED);
                spinright();
                __delay_cycles(1000000);
                state = 11;
            }
        }
        
        // distance behaviour
        else if (state == 21) // far reading
        {
            if ((P1IN & 0b00011110) != 0b00011010) state = 1;
            else if (dc > 0) state = 22;
            else
            {
                leds(RED_LED);
                stop();
            }
        }
        else if (state == 22) // near reading
        {
            if ((P1IN & 0b00011110) != 0b00011010) state = 1;
            else if (dc == 0) state = 21;
            else
            {
                leds(GREEN_LED);
                stop();
            }
        }
        
        // sumo behaviour
        else if (state == 31) // searching
        {
            if ((P1IN & 0b00011110) != 0b00010110) state = 1;
            else if (dc > 0) state = 32;
            else
            {
                leds(RED_LED);
                spinleft();
            }
        }
        else if (state == 32) // charging
        {
            if ((P1IN & 0b00011110) != 0b00010110) state = 1;
            else if (dc == 0) state = 31;
            else
            {
                leds(GREEN_LED);
                forward();
            }
        }
        
        // avoid behaviour
        else if (state == 41) // forward
        {
            if ((P1IN & 0b00011110) != 0b00001110) state = 1;
            else if (d < 20) state = 42;
            else if (d < 40) state = 43;
            else
            {
                leds(GREEN_LED);
                forward();
            }
        }
        else if (state == 42) // reverse
        {
            if ((P1IN & 0b00011110) != 0b00001110) state = 1;
            else if (d > 20 && d < 40) state = 43;
            else if (dc == 0) state = 41;
            else
            {
                leds(RED_LED);
                reverse();
            }
        }
        else if (state == 43) // spin
        {
            if ((P1IN & 0b00011110) != 0b00001110) state = 1;
            else
            {
                leds(GREEN_LED + RED_LED);
                spinleft();
                __delay_cycles(1000000);
                state = 41;
            }
        }
    }
}

//
// This function measures distance in cm using the rangefinder.
// It assumes TRIG is P1.3 and ECHO is P1.2.
//
int distance()
{
    // Send 20us trigger pulse
    P1OUT |= TRIG_PIN;
    __delay_cycles(20);
    P1OUT &= ~TRIG_PIN;
      
    // Wait for start of echo pulse
    while((P1IN & ECHO_PIN) == 0);
      
    // Measure how long the pulse is
    int d = 0;
    while((P1IN & ECHO_PIN) > 0)
    {
        // The following delay was worked out by trial and error
        // so that d counts up in steps corresponding to 1cm
        __delay_cycles(30);
        d = d + 1;
        if (d >= 400) break;
    }
     
    // Leave 100ms for any ultrasound reflections to die away
    __delay_cycles(100000);
     
    // Pass the measured distance back to the calling function
    return d;
}
Advertisements
This entry was posted in Uncategorized. Bookmark the permalink.

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s