MSP430 Line Follower

Introduction

For someone familiar with the digitalRead() and digitalWrite() of the Arduino IDE, the port directions and bitwise operations of Texas Instrument's Code Composer Studio might look a bit alien. But trust me, once you get used to it, you'll be making epic projects with cheap MCUs like PIC and MSP430 in no time!

We'll be making a line following robot with a PD controller using the MSP430G2 LaunchPad Development kit ( http://www.ti.com/tool/MSP-EXP430G2) that comes with the MSP430 G2553 value line MCU. If you have a different Texas Instruments MCU the code should work, but you'll have to check its datasheet to be sure that the pins support some special functions assigned to them. (Such as timers and interrupts)

Code Composer Studio

We'll be using CCS in this tutorial. You can download it here. http://processors.wiki.ti.com/index.php/Download_CCS

When installing, make sure you're installing all the components for MSP430 devices. Once done installing, open up the application and create a new CCS project using File > New.

Make sure you select the correct target device. You can let CCS figure out the board using the Identify button after plugging in the board. Give your project a name and hit finish.

You'll be greeted with a main.c file containing the small code snippet as shown above. The gibberish inside the main loop is used to stop the watchdog timer (duh! It says so in the comment right next to it) Hint : Try highlighting one word of that line and hit F3. It'll open up the msp430g2553.h file in a separate file and give you more info about that register.

Setting and Clearing Bits

We'll use the DIR register as an example on how to set and clear bits.

The MSP430 uses the DIR (Direction) register to set pins as inputs or outputs. In addition, the G2553 has two ports, namely PORT1 and 2. Look at your Launchpad. You'll see pins labeled as P1.0, P1.1, P2.0, P2.1. What do they mean? Easy!P1.0 - Port 1, Pin 0P2.3 - Port 2, Pin 3

These ports are usually 8 bits long, which is why you only see 8 pins per port. There are two DIR registers for the two ports, and they follow the same naming convention as the pins.P1DIR - Port 1 direction registerP2DIR - Port 2 direction register

These registers are also 8 bits long, and each bit corresponds to a pin in the related port. BIT1 of P1DIR corresponds to P1.1 (Port 1 Pin 1)

Right. So how do we set a pin as an input or an output? If you set the corresponding bit of a pin in the DIR register to 1, that pin is an output. Similarly, if you set the bit to 0, that pin becomes an input. Same as Arduino's pinMode() function.e.g - In order to set P2.1 as an output, we have to make BIT1 of P2DIR to one.CCS allows us to easily access the bits in a register by using BIT0, BIT1, BIT2...BIT6 keywords.

Try typing 'BIT4' in CCS, highlight it and press the F3 key. You'll be able to see how they're defined in the msp430g2553.h header.

You can use these keywords and the binary OR operator ( ' | ' ) to set individual bits in a register to 1.

P1DIR |= BIT2; //Set BIT2 of P1DIR to 1 (Sets pin 1.2 as an output)

We're using the or operator so that we only make changes to the required bit.

Resetting a single bit is tricky. This involves using the binary AND operator (' & ') and the Binary Ones Complement Operator (' ~ ')

P1DIR &= ~BIT2; //Set BIT2 of P1DIR to 0 (Sets pin 1.2 as an input)

Wiring Components

Now that we know the basics, let's get down to business. First, use the picture below as a guide to assemble your components. (I have also attached the fritzing project, you can download that at the bottom of this page)

Note : I used the IR line following sensor that comes with the TCRT5000 and a comparator. These things are fairly cheap. You can get 5 of them for $2. https://www.ebay.com/itm/5PCS-TCRT5000-IR-Infrared-Line-Track-Follower-Sensor-Obstacle-Avoidanc-Module/311568146310?epid=911730611&hash=item488ae89f86:g:TDUAAOSwlzZbIiiI:rk:3:pf:0

Declarations and Global Variables

We'll first declare the names of the functions and the global variables that we'll be using.

#include <msp430.h> 
void MotorSetup();
void SetLeftMotorSpeed();
void SetRightMotorSpeed();
void SetSpeeds(int lspeed,int rspeed);
void IRSensorSetup();
void SonarSetup();
void ReadSonar();
int readLine();
void lineFollow();
void SetBrakes();
/**
* main.c
*/
unsigned int up_counter;
unsigned int distance_cm;
int val = 0;
int sensorpanelVal = 0;
int lastval = 0;
//PID Values
int error = 0;
int lasterror = 0;
#define BaseSpeed 160
#define Kp 18
#define Kd 30

Pin Setup

Let's first setup the pins to use our sonar. I am using separate functions which I'll call once inside the main function.

void SonarSetup(){
   /* set P1.2 (TRIG)to output direction */
   P1DIR |= BIT2;
   P1OUT &= ~BIT2;                 // keep trigger at low
   /* Set P1.1 to input direction (echo)
   P1.1 is an input for Timer A0 - Compare/Capture input */
   P1DIR &= ~BIT1;
   // Select P1.1 as timer trigger input select (echo from sensor)
   P1SEL = BIT1;
   /* Timer A0 configure to read echo signal:
   Timer A Capture/Compare Control 0 =>
   capture mode: 1 - both edges +
   capture sychronize +
   capture input select 0 => P1.1 (CCI1A) +
   capture mode +
   capture compare interrupt enable */
   CCTL0 |= CM_3 + SCS + CCIS_0 + CAP + CCIE;
   /* Timer A Control configuration =>
   Timer A clock source select: 1 - SMClock +
   Timer A mode control: 2 - Continous up +
   Timer A clock input divider 0 - No divider */
   TA0CTL |= TASSEL_2 + MC_2 + ID_0;
   // Global Interrupt Enable
   _BIS_SR(GIE);
}

If you want to know more about the registers used, you can check out the MSP430 G2553 datasheet here.

Now let's setup the pins required to control the L298N motor driver.

void MotorSetup(){
   P2DIR |= BIT1+BIT5;//Pin 2.1 -> left motor speed Pin 2.5 -> Right Motor Speed
   P2SEL |= BIT1+BIT5;
   P1DIR |= BIT3+BIT4; // Set Pins 1.3 and 1.4 as outputs to control left motor direction
   P1DIR |= BIT5+BIT0; // Set Pins 1.5 and 1.6 as outputs to control right motor direction
   P1OUT &= ~(BIT1+BIT4+BIT5+BIT0); //set all pins to low
   /*** Timer1_A Set-Up ***/
   TA1CCR0 |= 200 - 1;
   TA1CCTL1 |= OUTMOD_7;
   TA1CCTL2 |= OUTMOD_7;
   TA1CCR1 |= 0;
   TA1CCR2 |= 0;
   TA1CTL |= TASSEL_2 + MC_1;
}

And finally, the pins to read the 5 IR sensors.

void IRSensorSetup(){
   //Set IR sensor pins as inputs
   P2DIR &= ~(BIT0+BIT2+BIT3+BIT4); //Set pins 2.0,2.2,2.3,2.4 as inputs
   P1DIR &= ~(BIT6); //1.7 as inputs
}

Helper Functions

Let's create some helper functions that allows us to easily control our components. This is easier than cramming everything inside the main function.

Helper function to read the sonar -

void ReadSonar(){
   P1OUT ^= BIT2;              // assert
   __delay_cycles(10);         // 10us wide
   P1OUT ^= BIT2;              // deassert
   __delay_cycles(60000);      // 60ms measurement cycle
}

Functions to control the motors -

void SetLeftMotorSpeed(int speed){
   if (speed >0){
       P1OUT &= ~BIT3; // Pin 1.3 Low
       P1OUT |= BIT4; //Pin 1.4 High
       if(speed >199){
           speed = 199;//prevent CCR2 from being negative
       }
       TA1CCR1 = speed;
   }else if(speed <0){
       P1OUT |= BIT3; //1.3 High
       P1OUT &= ~BIT4; // Pin 1.4 Low
       speed = -speed;
       if(speed >199){
           speed = 199;//prevent CCR1 from being negative
       }
       TA1CCR1 = speed;
   }
}
void SetRightMotorSpeed(int speed){
   if (speed >0){
       P1OUT &= ~BIT5;//Pin 1.5 Low
       P1OUT |= BIT0; // Pin 1.3 Low
       if(speed >199){
           speed = 199;//prevent CCR2 from being negative
       }
       TA1CCR2 = speed;
   }else if(speed <0){
       P1OUT |= BIT0; //1.5 High
       P1OUT &= ~BIT6;//Pin 1.6 Low
       speed = -speed;
       if(speed >199){
           speed = 199; //prevent CCR2 from being negative
       }
       TA1CCR2 = speed;
   }
}
void SetSpeeds(int lspeed,int rspeed){
   SetLeftMotorSpeed(lspeed);SetRightMotorSpeed(rspeed);
}
void SetBrakes(){
   P1OUT &= ~BIT5;//Pin 1.5 Low
   P1OUT &= ~BIT0; // Pin 1.3 Low
   P1OUT &= ~BIT3; // Pin 1.3 Low
   P1OUT &= ~BIT4; //Pin 1.4 Low
   TA1CCR1 = 199;
   TA1CCR2 = 199;
}

Sonar Interrupt Handler

You will need this to get the time for the Echo pin to go low after triggering the sonar.

#pragma vector=TIMER0_A0_VECTOR
__interrupt void Timer_A (void)
{
   if (CCTL0 & CCI)            // Raising edge
   {
       up_counter = CCR0;      // Copy counter to variable
   }
   else                        // Falling edge
   {
       // Formula: Distance in cm = (Time in uSec)/58
       distance_cm = (CCR0 - up_counter)/58;
   }
   TA0CTL &= ~TAIFG;           // Clear interrupt flag - handled
}

Line Following

First, we'll create a function to return a value according to the position of the line. As an example, the function will return 1 if the black line is below the 1st sensor, 2.5 if the line is below sensor 2 ans sensor 3 and so on.

int readLine()
{
   //from left to right. Sensor output is high when white space is detected. Since we're seeking a black line,inputs are inverted
   int sensor1 = !(P2IN&BIT0);
   int sensor2 = !(P2IN&BIT2);
   int sensor3 = !(P2IN&BIT3);
   int sensor4 =!(P1IN&BIT6);
   int sensor5 =!(P2IN&BIT4);
   int sum = 0;
   sensorpanelVal = (sensor1 * 1)+(sensor2* 2)+(sensor3 * 3)+(sensor4 *4)+(sensor5*5);
   sum = (sensor1+sensor2+sensor3+sensor4+sensor5);
   if (sum ==0){
       return lastval;
   }else{
   lastval = sensorpanelVal/sum;
   return lastval;
   }
}

Now that we have a function that gives us a numerical value according the position of the black line, we can develop a proportional and derivative controller to maintain the position at a specified setpoint.

void lineFollow(){
   val = readLine();
   error = 3-val;
   int delta = error-lasterror;
   int change = Kp*error + Kd*delta;
   lasterror = error;
   int leftMotorPWM = BaseSpeed -change;
   //constrain PWM
   if(leftMotorPWM >199){
       leftMotorPWM = 199;
   }else if(leftMotorPWM <0){
       leftMotorPWM = 0;
   }
   //constrain PWM
   int rightMotorPWM = BaseSpeed + change;
   if(rightMotorPWM >199){
           rightMotorPWM = 199;
   }else if(leftMotorPWM <0){
           rightMotorPWM = 0;
   }
   SetSpeeds(leftMotorPWM,rightMotorPWM);
}

And that's about it! Now all you have to do is set up your main function to run the pin setups, and continuously run the linefollow() function.

int main(void)
{
   WDTCTL = WDTPW | WDTHOLD;   // stop watchdog timer
   MotorSetup();
   IRSensorSetup();
   SonarSetup();
   while(1){
       lineFollow();
       if(sensorpanelVal ==15){
           SetBrakes();
           while(1){
           }
       }
   }
}

Here I'm stopping the robot indefinitely if all 5 of it's sensors are over a black surface, but you can use the sonar reading, or any other combination of IR sensor values to trigger a different function. (can be accessed using the global variable sensorpanelVal)

Conclusion

You can modify the linefollow() and readline() functions in order to fit your robot and your surface. If you need to follow a white line in a black surface, replace the lines in readline() with these,

int sensor1 = (P2IN&BIT0); //For white line on black surface
   int sensor2 = (P2IN&BIT2);
   int sensor3 = (P2IN&BIT3);
   int sensor4 = (P1IN&BIT6);
   int sensor5 = (P2IN&BIT4);

You will also have to play around with the Kp and Kd values to make your robot follow the line without losing it.