linefollower.c

/********************************************************************************
 Platform: SPARK V
 Experiment: 4_Motion_Control_Simple
 Written by: Vinod Desai, NEX Robotics Pvt. Ltd.
 Edited By: Sachitanand Malewar, NEX Robotics Pvt. Ltd.
 Last Modification: 22nd September 2010
 AVR Studio Version 4.17, Build 666

 Concepts covered: I/O interfacing, motion control using L293D 

 This experiment demonstrates simple motion control using L293D Motor driver IC.

 There are two components to the motion control:
 1. Direction control using pins PORTB0 to 	PORTB3
 2. Velocity control using Pulse Width Modulation(PWM) on pins PD4 and PD5.

 Pulse width modulation is a process in which duty cycle of constant frequency square wave is modulated to 
 control power delivered to the load i.e. motor. 

 In this experiment for the simplicity PWM pins, PD4 and PD5 are kept at logic 1.
 Use of PWM will be covered in the "5_Velocity_Control_Using_PWM" experiment.
   
 Connection Details:  	L-1---->PB0;		L-2---->PB1;
   						R-1---->PB2;		R-2---->PB3;
   						PD4 (OC1B) ----> Logic 1; 	PD5 (OC1A) ----> Logic 1; 

 Make sure that motors are connected to onboard motor connectors.
 
 For more detail refer to hardware and software manual

 Note: 
 
 1. Make sure that in the configuration options following settings are 
 	done for proper operation of the code

 	Microcontroller: atmega16
 	Frequency: 7372800Hz
 	Optimization: -O0 (For more information read section: Selecting proper 
	              optimization options below figure 4.22 in the hardware manual)

*********************************************************************************/

/********************************************************************************

   Copyright (c) 2010, NEX Robotics Pvt. Ltd.                       -*- c -*-
   All rights reserved.

   Redistribution and use in source and binary forms, with or without
   modification, are permitted provided that the following conditions are met:

   * Redistributions of source code must retain the above copyright
     notice, this list of conditions and the following disclaimer.

   * Redistributions in binary form must reproduce the above copyright
     notice, this list of conditions and the following disclaimer in
     the documentation and/or other materials provided with the
     distribution.

   * Neither the name of the copyright holders nor the names of
     contributors may be used to endorse or promote products derived
     from this software without specific prior written permission.

   * Source code can be used for academic purpose. 
	 For commercial use permission form the author needs to be taken.

  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  POSSIBILITY OF SUCH DAMAGE. 

  Software released under Creative Commence cc by-nc-sa licence.
  For legal information refer to: 
  http://creativecommons.org/licenses/by-nc-sa/3.0/legalcode


********************************************************************************/

#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>


#define RS 0
#define RW 1
#define EN 2
#define lcd_port PORTC
#define white 0
#define black 1


#define sbit(reg,bit)	reg |= (1<<bit)			// Macro defined for Setting a bit of any register.
#define cbit(reg,bit)	reg &= ~(1<<bit)		// Macro defined for Clearing a bit of any register.

int error = 0;
int last_error = 0;
int integral =0;
int difference=0;
int turn=0;
float p =10;
float d =30;
float i =0;
int avgSpeed = 60;
int speedA=0;
int speedB=0;



// Timer 5 initialized in PWM mode for velocity control
// Prescale: 64
// PWM 8bit fast, TOP=0x00FF
// Timer Frequency:450.000Hz
void timer1_init()
{
 TCCR1B = 0x00; //Stop
 TCNT1H = 0xFF; //Counter higher 8-bit value to which OCR1xH value is compared with
 TCNT1L = 0x01; //Counter lower 8-bit value to which OCR1xH value is compared with
 OCR1AH = 0x00; //Output compare register high value for Left Motor
 OCR1AL = 0xFF; //Output compare register low value for Left Motor
 OCR1BH = 0x00; //Output compare register high value for Right Motor
 OCR1BL = 0xFF; //Output compare register low value for Right Motor
 TCCR1A = 0xA1; //COM1A1=1, COM1A0=0; COM1B1=1, COM1B0=0;
//For Overriding normal port functionality to OCR1A outputs. WGM11=0, WGM10=1 Along With GM12
//in TCCR1B for Selecting FAST PWM 8-bit Mode
 TCCR1B = 0x0D; //WGM12=1; CS12=0, CS11=1, CS10=1 (Prescaler=64)
} 

void motion_pin_config (void)
{
 DDRB = DDRB | 0x0F;   //set direction of the PORTB3 to PORTB0 pins as output
 PORTB = PORTB & 0xF0; // set initial value of the PORTB3 to PORTB0 pins to logic 0
 DDRD = DDRD | 0x30;   //Setting PD4 and PD5 pins as output for PWM generation
 PORTD = PORTD | 0x30; //PD4 and PD5 pins are for velocity control using PWM
}

//Function to initialize ports
void port_init()
{
 motion_pin_config();
 adc_pin_config();
 lcd_port_config();

}
//ADC Functions
/********************************************************************************/
void adc_pin_config (void)
{
 DDRA = 0x00; //set PORTA direction as input
 PORTA = 0x00; //sett PORTA pins floating
} 

//Function to Initialize ADC
void adc_init()
{
 ADCSRA = 0x00;
 ADMUX = 0x20; //Vref=5V external --- ADLAR=1 --- MUX4:0 = 0000
 ACSR = 0x80;
 ADCSRA = 0x86; //ADEN=1 --- ADIE=1 --- ADPS2:0 = 1 1 0
} 

//This Function accepts the Channel Number and returns the corresponding Analog Value
unsigned char ADC_Conversion(unsigned char Ch)
{
unsigned char a;
 Ch = Ch & 0x07;
 ADMUX= 0x20| Ch;
 ADCSRA = ADCSRA | 0x40; //Set start conversion bit
while((ADCSRA&0x10)==0); //Wait for ADC conversion to complete
 a=ADCH;
 ADCSRA = ADCSRA|0x10; //clear ADIF (ADC Interrupt Flag) by writing 1 to it
 //ADCSRB = 0x00;
return a;
} 

//LCD Functions
/********************************************************************************/
void lcd_port_config (void)
{
 DDRC = DDRC | 0xF7; //all the LCD pin's direction set as output
 PORTC = PORTC & 0x80; // all the LCD pins are set to logic 0 except PORTC 7
} 

void lcd_home()
{
 lcd_wr_command(0x80);
} 

void lcd_set_4bit()
{
 _delay_ms(1);
 cbit(lcd_port,RS); //RS=0 --- Command Input
 cbit(lcd_port,RW); //RW=0 --- Writing to LCD
 lcd_port = 0x30; //Sending 3 in the upper nibble
 sbit(lcd_port,EN); //Set Enable Pin
 _delay_ms(5); //delay
 cbit(lcd_port,EN); //Clear Enable Pin_delay_ms(1);
 cbit(lcd_port,RS); //RS=0 --- Command Input
 cbit(lcd_port,RW); //RW=0 --- Writing to LCD
 lcd_port = 0x30; //Sending 3 in the upper nibble
 sbit(lcd_port,EN); //Set Enable Pin
 _delay_ms(5); //delay
 cbit(lcd_port,EN); //Clear Enable Pin
 _delay_ms(1);
 cbit(lcd_port,RS); //RS=0 --- Command Input
 cbit(lcd_port,RW); //RW=0 --- Writing to LCD
 lcd_port = 0x30; //Sending 3 in the upper nibble
 sbit(lcd_port,EN); //Set Enable Pin
 _delay_ms(5); //delay
 cbit(lcd_port,EN); //Clear Enable Pin
 _delay_ms(1);
 cbit(lcd_port,RS); //RS=0 --- Command Input
 cbit(lcd_port,RW); //RW=0 --- Writing to LCD
 lcd_port = 0x20; //Sending 2 in the upper nibble to initialize LCD 4-bit mode
 sbit(lcd_port,EN); //Set Enable Pin
 _delay_ms(5); //delay
 cbit(lcd_port,EN); //Clear Enable Pin
}

//Function to Initialize LCD
void lcd_init()
{
 _delay_ms(1);
 lcd_wr_command(0x28); //4-bit mode and 5x8 dot character font
 lcd_wr_command(0x01); //Clear LCD display
 lcd_wr_command(0x06); //Auto increment cursor position
 lcd_wr_command(0x0E); //Turn on LCD and cursor
 lcd_wr_command(0x80); //Set cursor position
} 

//Function to write command on LCD
void lcd_wr_command(unsigned char cmd)
{
 unsigned char temp;
 temp = cmd;
 temp = temp & 0xF0;
 lcd_port &= 0x0F;
 lcd_port |= temp;
 cbit(lcd_port,RS);
 cbit(lcd_port,RW);
 sbit(lcd_port,EN);
 _delay_ms(5);
 cbit(lcd_port,EN);

 cmd = cmd & 0x0F;
 cmd = cmd<<4;
 lcd_port &= 0x0F;
 lcd_port |= cmd;
 cbit(lcd_port,RS);
 cbit(lcd_port,RW);
 sbit(lcd_port,EN);
 _delay_ms(5);
 cbit(lcd_port,EN);
} 	

//Function to write data on LCD
void lcd_wr_char(char letter)
{
char temp;
 temp = letter;
 temp = (temp & 0xF0);
 lcd_port &= 0x0F;
 lcd_port |= temp;
 sbit(lcd_port,RS);
 cbit(lcd_port,RW);
 sbit(lcd_port,EN);
 _delay_ms(5);
 cbit(lcd_port,EN);
 letter = letter & 0x0F;
 letter = letter<<4;
 lcd_port &= 0x0F;
 lcd_port |= letter;
 sbit(lcd_port,RS);
 cbit(lcd_port,RW);
 sbit(lcd_port,EN);
 _delay_ms(5);
 cbit(lcd_port,EN);
} 

// Function to print any input value up to the desired digit on LCD
void lcd_print (char row, char coloumn, unsigned
int value, int digits)
{
unsigned char flag=0;
unsigned int temp=0;
unsigned char million=0;
unsigned char thousand=0;
unsigned char hundred=0;
unsigned char tens=0;
unsigned char unit=0;
if(row==0||coloumn==0)
 {
 lcd_home();
 }
else
 {
 lcd_cursor(row,coloumn);
 }
if(digits==5 || flag==1)
 { million=value/10000+48;
 lcd_wr_char(million);
 flag=1;
 }
if(digits==4 || flag==1)
 {
 temp = value/1000;
 thousand = temp%10 + 48;
 lcd_wr_char(thousand);
 flag=1;
 }
if(digits==3 || flag==1)
 {
 temp = value/100;
 hundred = temp%10 + 48;
 lcd_wr_char(hundred);
 flag=1;
 }
if(digits==2 || flag==1)
 {
 temp = value/10;
 tens = temp%10 + 48;
 lcd_wr_char(tens);
 flag=1;
 }
if(digits==1 || flag==1)
 {
 unit = value%10 + 48;
 lcd_wr_char(unit);
 }
if(digits>5)
 {
 lcd_wr_char('E');
 }
} 

//Position the LCD cursor at "row", "column"
void lcd_cursor (char row, char column)
{
switch (row) {
 case 1: lcd_wr_command (0x80 + column - 1); break;
 case 2: lcd_wr_command (0xc0 + column - 1); break;
 case 3: lcd_wr_command (0x94 + column - 1); break;
 case 4: lcd_wr_command (0xd4 + column - 1); break;
 default: break;
 }
} 
/********************************************************************************/
void display_sensor()
{
lcd_print(1,1,ADC_Conversion('3'),3);
lcd_print(1,7,ADC_Conversion('4'),3);
lcd_print(1,13,ADC_Conversion('5'),3);
lcd_print(2,1,speedB,3);	
lcd_print(2,13,speedA,3);
}


//---------------------------------------------------------------------------------
//Function used for setting motor's direction
void motion_set (unsigned char Direction)
{
 unsigned char PortBRestore = 0;

 Direction &= 0x0F; 			// removing upper nibbel as it is not needed
 PortBRestore = PORTB; 			// reading the PORTB's original status
 PortBRestore &= 0xF0; 			// setting lower direction nibbel to 0
 PortBRestore |= Direction; 	// adding lower nibbel for direction command and restoring the PORTB status
 PORTB = PortBRestore; 			// setting the command to the port
}
//---------------------------------------------------------------------------------
void forward (void)         //both wheels forward
{
  motion_set(0x06);
}

void back (void)            //both wheels backward
{
  motion_set(0x09);
}

void left (void)            //Left wheel backward, Right wheel forward
{
  motion_set(0x05);
}

void right (void)           //Left wheel forward, Right wheel backward
{   
  motion_set(0x0A);
}

void soft_left (void)       //Left wheel stationary, Right wheel forward
{
 motion_set(0x04);
}

void soft_right (void)      //Left wheel forward, Right wheel is stationary
{ 
 motion_set(0x02);
}

void soft_left_2 (void)     //Left wheel backward, right wheel stationary
{
 motion_set(0x01);
}

void soft_right_2 (void)    //Left wheel stationary, Right wheel backward
{
 motion_set(0x08);
}

void hard_stop (void)       //hard stop(stop suddenly)
{
  motion_set(0x00);
}

void soft_stop (void)       //soft stop(stops solowly)
{
  motion_set(0x0F);
}
//---------------------------------------------------------------------------------
void pid()
{

error = ADC_Conversion('3')-ADC_Conversion('5') ;
integral +=error;

//if(integral > 255){
 //integral=255;}
//if(integral < -255){
 //integral=-255;}

difference = error-last_error;
last_error = error;
turn = error*p + i*integral + d*difference;
if(turn >avgSpeed) turn = avgSpeed;
if(turn <-1*avgSpeed) turn = -1*avgSpeed;

}
//---------------------------------------------------------------------------------
void velocity (unsigned char left_motor, unsigned char right_motor)
{
OCR1AL = left_motor;
OCR1BL = right_motor;
} 
//---------------------------------------------------------------------------------
void init_devices (void)
{
 cli(); //Clears the global interrupts
 port_init();
 adc_init(); 
 lcd_set_4bit();
 lcd_init();
 timer1_init();
 sei(); //Enables the global interrupts
}
//---------------------------------------------------------------------------------

//Main Function
int main()
{
	init_devices();
	forward();
	
	while(1)
	{
	display_sensor();
	//_delay_ms(50);
	pid();


 	if(turn>=0){
 	speedA=avgSpeed;
 	speedB=avgSpeed-turn;
 	}
 	else{
 	speedA=avgSpeed+turn;
 	speedB=avgSpeed;
 	}
	velocity(speedB,speedA);

	}
}