Rangefinder code from today’s class

//
// Rangefinder example for MSP430G2452
//
// LED control is on P1.0
// Left motor control is P2.0 and P2.1
// Right motor control is P2.4 and P2.5
// Rangefinder trigger is P1.1
// Rangefinder echo is P1.2
//

#include <msp430.h>

// function prototypes
int object_detect();
void led(int);
void forward();
void stop();
void spin_left();

void main()
{
    // Stop watchdog timer to prevent timeout reset
    WDTCTL = WDTPW + WDTHOLD;
      
    P1DIR = 0b00000011; // LED is P1.0, Trig is P1.1
    P2DIR = 0b00110011; // LM is P2.4,P2.5 and RM is P2.0,P2.1
    
    // This while loop repeats the four statements it contains
    // over and over again to blink the LED on and off forever
    while(1)
    {
        if (object_detect() == 1)
        {
            led(1);
            forward();
        }
        else
        {
            led(0);
            spin_left();
        }
        
        __delay_cycles(50000); // 50 ms delay to let echoes die down
    }
}

// When on=1 LED turns on, when on=0 LED turns off 
void led(int on)
{
    if (on == 1) P1OUT |= BIT0; // set P1.0 high
    else P1OUT &= ~BIT0; // set P1.0 low
}

// Returns 1 when object detected and 0 otherwise
int object_detect()
{
    // Send trigger pulse
    P1OUT |= BIT1;      // Set P1.1 high
    __delay_cycles(20); // 20us delay
    P1OUT &= ~BIT1;     // Set P1.1 low

    // Check echo pin after 4.2ms to see if an object is detected
    __delay_cycles(4200);
    if ((P1IN & BIT2)>0) return 0;
    else return 1;
}

void forward()
{
    P2OUT = 0b00010001;
}

void stop()
{
    P2OUT = 0b00000000;
}

void spin_left()
{
    P2OUT = 0b00010010;
}

Advertisements
This entry was posted in Uncategorized. Bookmark the permalink.

One Response to Rangefinder code from today’s class

  1. Pingback: Week 14 – Robosumo |

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