Daniel’s code

//
// Daniel's robot
//
 
#include <msp430.h>
 
// function prototype
int distance();
 
// LED and motor control function definitions
void stop()      {P2OUT = 0b00000000;}
void forward()   {P2OUT = 0b00001010;}
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, n;
     
    // 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
    while(1)
    {
        // searching
        while(1)
        {
            leds(RED_LED);
            spinleft();
            __delay_cycles(150000);
            stop();
            __delay_cycles(50000);
            d = distance();
            if (d < 50) break;
        }
        
        // charging at opponent
        n = 5;
        while(n > 0)
        {
            leds(GREEN_LED);
            forward();
            d = distance();
            if (d < 50) n = 5;
            else n = n - 1;
        }
    }
    
    return 0;
}
 
//
// 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;
}

Advertisement