This program implements 5 selectable behaviours:
- blink – leave P1.1-3 unconnected (pins 3-6)
- zig-zag – ground P1.1 (pin 3)
- distance checker – ground P1.2 (pin 4)
- sumo – ground P1.3 (pin 5)
- avoid – ground P1.4 (pin 6)
Compiled program in two formats:
- Hex format: RoboSlam_testing_program_MSP430G2553.hex
- Out format: RoboSlam_testing_program_MSP430G2553.out
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;
}
RoboSlam 