Colour Sensor Example Code

//
// RoboSlam Example: Colour sensor navigation
// In this program the robot is guided by a colour sensor
// connected to P1.1. When P1.1 is high, the robot turns
// right on the spot. When P1.1 is low, the robot drives
// forward.
//
// Code is for MSP430G2553
// Last updated 19-10-2015
//
 
#include <msp430.h>
 
// function prototypes
void leds(int green, int red);
void motors(int left, int right);
 
// main function
int main( void )
{
    // stop watchdog timer to prevent time out reset
    WDTCTL = WDTPW + WDTHOLD;
 
    // configure digital inputs and outputs
    P1DIR = 0b10000001;   // Port 1 outputs: P1.0 green LED, P1.7 red LED
    P2DIR = 0b00001111;   // Port 2 outputs: P2.0-1 left motor, P2.2-3 right motor
 
    while(1)
    {
        if ((P1IN & BIT1) == 0)
        {
            leds(1,0);
            motors(1,1);  // both motors forward on black
        }
        else
        {
            leds(0,1);
            motors(1,-1); // turn right on white
        }
    }
 
    return 0;
}
 
//
// This function sets the direction of both motors.
// The first argument controls the left motor.
// The second argument controls the right motor.
// For each motor: positive number for forward,
// negative number for reverse, zero for stop.
//
void motors(int left, int right)
{
    // work out P2OUT value, then write to register
    int p2out_value = 0;
    if (left > 0)  p2out_value += 0b00001000;
    if (left < 0)  p2out_value += 0b00000100;
    if (right > 0) p2out_value += 0b00000010;
    if (right < 0) p2out_value += 0b00000001;
    P2OUT = p2out_value;
}
 
//
// This function controls the green and red LEDs.
// For each LED: 1 is for on, 0 is for off.
//
void leds(int green, int red)
{
    // work out P1OUT value, then write to register
    int p1out_value = 0;
    if (green == 1) p1out_value += 0b00000001;
    if (red == 1)   p1out_value += 0b10000000;
    P1OUT = p1out_value;
}

Advertisement