Labels

Wednesday, June 23, 2010

Build Your Own Microcontroller Based PID Control Line Follower Robot (LFR) – Second Part

One of the interesting parts in building the Line Follower Robot is; you could start it with a very simple version by using just two transistors with the LED and LDR for sensor (Build Your Own Transistor Based Mobile Line Follower Robot - First Part) and enhance it to the programmable version that use microcontroller as the brain for controlling the robot. The reason of using the microcontroller for the Line Follower Robot is we need to make more robust, reliable and flexible robot which you could not have it from the discrete electronics component robot without changing most of the electronic circuit design.
BRAM II LFR 03
The advantages in building the microcontroller based Line Follower Robot (LFR) is we could take the advantage of microcontroller’s ALU (Arithmetic Logic Unit) to compute mathematics equation to perform the industrial standard Proportional, Integral and Derivative control or known as PID control. On this tutorial we will learn to build the LFR using the powerful Atmel AVR ATMega168 microcontroller and at the same time we will learn to utilize many of the AVR ATMega168 microcontroller sophisticated features to support our Line Follower Robot.
Now let’s check out all the exciting features of this Line Follower Robot that we are going to build:
  • Fully implement the industrial standard Proportional, Integral and Derivative (PID) control with flexible PID tuning parameter using the AVR ATMega168 UART peripheral and store the parameter to the AVR ATMega168 microcontroller build-in 512 Bytes EEPROM
  • Use five infra red reflective object sensor for the black line sensor with Microchip MCP23008 8-bit I2C (read as I square C) I/O expander chip to talk to the AVR ATMega168 Microcontroller I2C peripheral
  • 4.5 Volt to 5 Volt DC to DC Step-Up using Maxim MAX756 for powering both the electronics circuits and the DC motors. This will ensure the electronics circuits and the DC motors keep working properly even though the battery voltage level drops below 4.5 Volt.
  • Use the AVR ATMega168 ADC (Analog to Digital Converter) peripheral to control the maximum speed of the robot.
  • Use the AVR ATMega168 PWM (Pulse Width Modulation) peripheral to drive the SGS-Thomson L293D chip to control the DC motors speed
BRAM II Chassis Construction
The first version of BRAM (Beginner’s Robot Autonomous Mobile), used CD/DVD for the chassis; in this version of BRAM, I use the acrylic as the base for the chassis; which is easy to shape, drill and it came with transparent look and many color to choose too. You could read more information about the first BRAM version on my previous posted blog Building BRAM your first Autonomous Mobile Robot Using Microchip PIC Microcontroller.

BRAM II construction material parts:
  • Acrylic for the BRAM chassis
  • Two geared DC motors with wheels rated 4.5 to 5 volt (25-30mA) with the wheel or you could use the modified servo motor (it’s a servo motor without the electronics’s control board)
  • One 3 x 1.5 volt AA battery holder with on-off switch
  • One plastic bead (usually it use for the neck less) and one paper clip for the caster
  • Enough nuts, bolts and PCB (printed circuit board) standoff.
BRAM construction consists of two decks, where the lower deck is used to hold the battery, DC motors and the sensor circuit while the upper deck is used to hold the motor controller circuit and the AVRJazz Mega168 board.
BRAM II LFR 09
BRAM II LFR 08
Put the 3xAA battery and sensor circuit on the lower deck and the motor control circuit at the bottom of the upper deck.
BRAM II LFR 06
BRAM II LFR 05
Now connect the battery power terminal, sensor circuit power terminal, DC motor power terminal to the Motor Control Circuit and screw the upper deck on the lower deck.
BRAM II LFR 04
Finally put the AVRJazz Mega168 board on top of the upper deck and connect all the connectors from the Motor Control Circuit and Sensor Circuit to the AVRJazz Mega168 board, now you are ready to program the robot.
BRAM II Electronic Circuit and Program
BRAM II use quite sophisticated power mechanism circuit as well as the line sensor circuit design to make it more robust line follower robot.
BRAM II LFR 02
The 3xAA battery is the main power source for both the electronics circuits and the DC motors, because the Line Follower Robot need to accurately control the motor speed and tracking the line; therefore BRAM II design use DC to DC step-up circuit to boost 4.5 volt level up to the 5 volt level using the Maxim MAX756, you could read more about MAX756 in my previous posted blog Powering your Microcontroller’s Based Project. The advantages of using the power boost is all the electronics circuits and DC motors will guarantee to get proper voltage level even though the main power source theoretically drops down to 2 volt level; although I’ve never try this.
For the DC motors controller we simply use the two channels SGS-Thomson L293D H-Bridge motor driver chips, this chip is used to make circuit simpler and robust compared to the standard transistor’s base H-Bridge driver. The L293D chip is able to handle non repetitive current up to 1.2 A for each channels and its already equipped with the internal EMF (Electromotive Force) protection diodes.
For the line sensor circuit I decided to take advantage of the Microchip MCP23008 I2C 8-bit I/O expander to minimized the AVR Mega168 microcontroller I/O ports usage and at the same time it serve as a learning tools of how to expand your microcontroller I/O port using the Microchip MCP23008. Here is the complete PDF schematic for the AVRJazz Mega168 boardMotor Controller & DC to DC Step-Up and theSensor Circuit.
The last is the ATMega168 UART peripheral, which is used to setup our BRAM II PID control parameter; by using the AVRJazz Mega168 board build in RS323 voltage level converter; we could connect the RS232 communication (COM) port directly to the computer COM port or you could use the USB to RS232 converter as I did and use the MS Windows HyperTerminal or puTTY program to enter the PID control setup. You could read more about AVR UART peripheral on my previous posted blog Working with AVR microcontroller Communication Port Project.
The following are the list of hardware and software used in this tutorial:
  • Supporting chips: SGS Thomson L293D, Maxim MAX756, Microchip MCP23008 and 74HC14 Schmitt trigger
  • Five Junye JY209-01 reflective object sensor.
  • For the detail electronics components such as transistors, capacitors and resistors please refer to the schematic above.
  • AVRJazz Mega168 board from ermicro which base on the AVR ATmega168 microcontroller.
  • WinAVR for the GNU’s C compiler v4.3.0 (WinAVR 2009313).
  • Atmel AVR Studio v4.17 for the coding and debugging environment.
  • STK500 programmer from AVR Studio 4, using the AVRJazz Mega168 board STK500 v2.0 bootloader facility.
Ok that’s a brief explanation of the BRAM II Line Follower Robot project electronic circuits that we are going to build; now let’s take a look at the C code that makes the BRAM II brain:


//***************************************************************************
// File Name : bramlfr.c // Version : 1.0
FR) // : LFR with PID Controller //
// Description : BRAM Line Follower Robot ( L Author : RWB // Target : AVRJazz Mega168 Board
13) // IDE : Atmel AVR Studio 4.17 // Programmer : AV
// Compiler : AVR-GCC 4.3.0; avr-libc 1.6.2 (WinAVR 20090 3RJazz Mega168 STK500 v2.0 Bootloader // : AVR Visual Studio 4.17, STK500 programmer
********** #include
// Last Updated : 28 July 2009 //****************************************************************
*
#include
#include
#include
#include
#include
#include


// BRAM Debugging Mode, 0-Debug Off, 1-Debug On
#define BRAM_DEBUG 0


#define BAUD_RATE 19200


#define MAX_TRIES 50


#define MCP23008_ID 0x40 // MCP23008 Device Identifier
#define MCP23008_ADDR 0x0E // MCP23008 Device Address
egister #define GPPU 0x06 // MCP23008 I/O Pull-Up Re
#define IODIR 0x00 // MCP23008 I/O Direction Rsistor Register
9 // MCP23008 General Purpose I/O Register
#define GPIO 0x 0


#define I2C_START 0
#define I2C_DATA 1
K 2 #define I2C_STOP 3
#define I2C_DATA_A C #define ACK 1
#define NACK 0


// Define BRAM Steering
#define MOVE_FORWARD 0
#define TURN_RIGHT 2
#define TURN_LEFT 1 #define ROTATE_LEFT 3
#define FULL_STOP 6
#define ROTATE_RIGHT 4
#define MOVE_BACKWARD 5


// Define BRAM Sensor
#define MAX_MAP 24
60 #define MAX_SENSOR
#define TARGET_VAL
_MAP 120


// Define BRAM Variables
const unsigned int sensor_map[] PROGMEM = {
0b00000,0, 0b00001,10, 0b00011,20,
0b00100,60
0b00010,30, 0b00111,40, 0b00110,50 ,, 0b01100,70, 0b11100,80, 0b01000,90,
0b11000,100, 0b10000,110
};


unsigned char MaxSpeed; // Hold Motor Maximum Speed
unsigned int Kp,Ki,Kd; // PID Parameters
iables
int prev_res=0, prev_err_1=0, prev_err_2=0; // PID Control Va r


void uart_init(void)
{
UBRR0H = (((F_CPU/BAUD_RATE)/16)-1)>>8; // set baud rate
UBRR0L = (((F_CPU/BAUD_RATE)/16)-1);
<
UCSR0B = (1&lt ;


void uart_flush(void)
{
unsigned char dummy;


while (UCSR0A & (1<


int uart_putch(char ch,FILE *stream)
{ if (ch == '\n')
stream);
uart_putch('\r',


while (!(UCSR0A & (1<
return 0;
}
int uart_getch(FILE *stream)
{
unsigned char ch;
while (!(UCSR0A & (1<
void ansi_cl(void)
{
// ANSI clear screen: cl=\E[H\E[J
putchar(27); putchar('[');
putchar('[');
putchar('H'); putchar(27); putchar('J');
}
void ansi_me(void)
{
// ANSI turn off all attribute: me=\E[0m
putchar(27); putchar('[');
}
putchar('0'); putchar('m')
;
void ansi_cm(unsigned char row,unsigned char col)
{ // ANSI cursor movement: cl=\E%row;%colH
row); putcha
putchar(27); putchar('['); printf("%d" ,r(';'); printf("%d",col); putchar('H');
}
// BRAM Steering Function
// PD2 - Input 1 (Left Motor), PD3 - Input 2 (Left Motor)
// PD4 - Input 3 (Right Motor), PD7 - Input 4 (Right Motor)
void BRAM_Steer(unsigned char steer)
{ switch(steer) {
D: PORTD &= ~(1
case MOVE_FORWA
R << PD4); PORTD |= (1 << PD7); // Right Motor On Forward
PORTD &= ~(1 << PD2); PORTD |= (1 << PD3); // Left Motor On Forward
break; case TURN_LEFT:
4); PORTD |= (1 << PD7); // Right Motor On Forward PORTD &= ~(1 << PD2
PORTD &= ~(1 << P D); PORTD &= ~(1 << PD3); // Left Motor Off break; case TURN_RIGHT:
PORTD &= ~(1 << PD2); PORTD |= (1 << PD3); // Left Motor On Forward
PORTD &= ~(1 << PD4); PORTD &= ~(1 << PD7); // Right Motor Off break; case ROTATE_LEFT: PORTD &= ~(1 << PD4); PORTD |= (1 << PD7); // Right Motor On Forward
PORTD |= (1 << PD4); PORTD &= ~(1 << PD7); // Right Motor On Rever
PORTD |= (1 << PD2); PORTD &= ~(1 << PD3); // Left Motor On Reverse break; case ROTATE_RIGHT :se PORTD &= ~(1 << PD2); PORTD |= (1 << PD3); // Left Motor On Forward break; case MOVE_BACKWARD:
~(1 << PD3); // Left Motor On Reverse break; case FULL_STOP: PORT
PORTD |= (1 << PD4); PORTD &= ~(1 << PD7); // Right Motor On Reverse PORTD |= (1 << PD2); PORTD &= D &= ~(1 << PD4); PORTD &= ~(1 << PD7); // Right Motor Off PORTD &= ~(1 << PD2); PORTD &= ~(1 << PD3); // Left Motor Off break; }
}
// BRAM Motor Speed Control
// PD5 - OC0B -> ENB1 (Left Motor)
// PD6 - OC0A -> ENB2 (Right Motor)
void BRAM_DriveMotor(int left_speed, int right_speed)
{
unsigned char left_pwm,right_pwm;
if (left_speed >= 0 && right_speed >= 0) {
// Move Forward BRAM_Steer(MOVE_FORWARD);
left_pwm=left_speed; right_pwm=right_speed
;
} else if (left_speed < 0 && right_speed < 0) {
// Move Backward
ACKWARD);
BRAM_Steer(MOVE_ B
left_pwm=left_speed * -1;
right_pwm=right_speed * -1;
} else if (left_speed < 0 && right_speed >= 0) {
// Rotate Left
TE_LEFT);
BRAM_Steer(ROT A
left_pwm=left_speed * -1;
right_pwm=right_speed;
} else if (left_speed >= 0 && right_speed < 0) {
// Rotate Right
E_RIGHT);
BRAM_Steer(ROTA T
left_pwm=left_speed;
right_pwm=right_speed * -1;
} else { // Full Stop
BRAM_Steer(FULL_STOP);
left_pwm=0;
right_pwm=0;
}
// Assigned the value to the PWM Output Compare Registers A and B
OCR0A=right_pwm; OCR0B=left_pwm;
}
/* START I2C Routine */
unsigned char i2c_transmit(unsigned char type) {
switch(type) {
RT: // Send Start Condition TWCR = (1
case I2C_ST A << TWINT) | (1 << TWSTA) | (1 << TWEN);
cknowledge
break; case I2C_DATA: // Send Data with No- A TWCR = (1 << TWINT) | (1 << TWEN); break;
(1 << TWEA) | (1 << TWINT) | (1 <<
case I2C_DATA_ACK: // Send Data with Acknowledge TWCR = TWEN); break; case I2C_STOP: // Send Stop Condition
TO); return 0; }
TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TW
S
// Wait for TWINT flag set on Register TWCR
while (!(TWCR & (1 << TWINT)));
// Return TWI Status Register, mask the prescaller bits (TWPS1,TWPS0)
return (TWSR & 0xF8);
}
char i2c_start(unsigned int dev_id, unsigned int dev_addr, unsigned char rw_type)
{ unsigned char n = 0; unsigned char twi_status;
char r_val = -1;
i2c_retry:
if (n++ >= MAX_TRIES) return r_val;
// Transmit Start Condition
twi_status=i2c_transmit(I2C_START);
// Check the TWI Status
RB_LOST) goto i2c_retry; if ((twi_status != TW_ST
if (twi_status == TW_MT_
AART) && (twi_status != TW_REP_START)) goto i2c_quit;
// Send slave address (SLA_W)
TWDR = (dev_id & 0xF0) | (dev_addr & 0x0E) | rw_type;
// Transmit I2C Data
twi_status=i2c_transmit(I2C_DATA);
// Check the TWSR status
if ((twi_status == TW_MT_SLA_NACK) || (twi_status == TW_MT_ARB_LOST)) goto i2c_retry;
if (twi_status != TW_MT_SLA_ACK) goto i2c_quit;
r_val=0;
i2c_quit:
return r_val;
}
void i2c_stop(void)
{
unsigned char twi_status;
// Transmit I2C Data
twi_status=i2c_transmit(I2C_STOP);
}
char i2c_write(char data)
{
unsigned char twi_status;
char r_val = -1;
// Send the Data to I2C Bus
TWDR = data;
// Transmit I2C Data
twi_status=i2c_transmit(I2C_DATA);
// Check the TWSR status
if (twi_status != TW_MT_DATA_ACK) goto i2c_quit;
r_val=0;
i2c_quit:
return r_val;
}
char i2c_read(char *data,char ack_type)
{ unsigned char twi_status;
if (ack_type) { // Rea
char r_val = -1; d I2C Data and Send Acknowledge
ATA_ACK);
twi_status=i2c_transmit(I2C_ D
if (twi_status != TW_MR_DATA_ACK) goto i2c_quit;
} else {
I2C Data and Send No Acknowledge twi_st
// Rea
datus=i2c_transmit(I2C_DATA);
if (twi_status != TW_MR_DATA_NACK) goto i2c_quit;
}
// Get the Data
*data=TWDR;
r_val=0;
i2c_quit:
return r_val;
}
void Write_MCP23008(unsigned char reg_addr,unsigned char data)
{ // Start the I2C Write Transmission
_WRITE);
i2c_start(MCP23008_ID,MCP23008_ADDR,T W
// Sending the Register Address
i2c_write(reg_addr);
// Write data to MCP23008 Register
i2c_write(data);
// Stop I2C Transmission
i2c_stop();
}
unsigned char Read_MCP23008(unsigned char reg_addr)
{
char data;
// Start the I2C Write Transmission
i2c_start(MCP23008_ID,MCP23008_ADDR,TW_WRITE);
// Read data from MCP23008 Register Address
i2c_write(reg_addr);
// Stop I2C Transmission
i2c_stop();
he I2C Read Transmission i2c_start(MC
// Re-Start
tP23008_ID,MCP23008_ADDR,TW_READ);
i2c_read(&data,NACK);
// Stop I2C Transmission
i2c_stop();
return data;
}
unsigned int BRAM_IRSensor()
{
static unsigned int old_val = TARGET_VAL;
unsigned int map_val;
unsigned char sensor_val,ptr;
// Turn On the Sensor IR LED
Write_MCP23008(GPIO,0b00011111);
// Read sensor
d_MCP23008(GPIO) & 0x1F;
sensor_val = Re a
// Turn Off the Sensor IR LED
Write_MCP23008(GPIO,0b00111111);
// Return Value from Sensor Map
map_val=TARGET_VAL; // Default always on center
if (sensor_val > 0) {
for(ptr = 0; ptr < MAX_MAP; ptr++) {
// Now get the sensor map value
tr) == sensor_val) { map_val=pgm_read_word(sensor_map + p
if (pgm_read_word(sensor_map + ptr + 1); }
}
old_val=map_val;   // Save the Current IR Array Value
} else {
map_val=0;
// Adjust for zero result if previous value greater than 5
if (old_val > TARGET_VAL) map_val=MAX_SENSOR_MAP;
}
// Display IR Sensor Value in Debugging Mode
#if BRAM_DEBUG ansi_cm(3,1);
Value: %03d",sensor_val,map_val); #endif
printf("IR Sensor: %02x, Map
return map_val;
}
void BRAM_PIDControl(unsigned int sensor_val)
{ int motor_res,err_func;
float KP,KI,KD,cont_res;
// Get the Error Function
err_func=sensor_val - TARGET_VAL;
// Divide all the PID parameters for decimal value
KP=Kp * 0.1; KI=Ki * 0.01;
KD=Kd * 0.01;
// Calculate the Motor Response using PID Control Equation
cont_res=(float)(prev_res + KP * (err_func - prev_err_1) + KI * (err_func + prev_err_1)/2.0
+ KD * (err_func - 2.0 * prev_err_1 + prev_err_2));
PWM Motor Value motor_res=(int)cont_res;
// Now we have to Limit the control response to the Maximum of our motor
if (motor_res > MaxSpeed)
motor_res = MaxSpeed;
peed) motor_res = -MaxSpeed
if (motor_res < -Max S;
Save the Motor Response and Error Function Result pr
// ev_res=motor_res; prev_err_2=prev_err_1;
Response Value in Debug
prev_err_1=err_func; // Display Motor ging Mode #if BRAM_DEBUG ansi_cm(4,1);
tor_res); #endif
printf("Motor Response Factor: %d ",m
o
// Negative result mean BRAM is on the right, so we need to adjust to the left
// Positive result mean BRAM is on the left, so we need to adjust to the right
if (motor_res < 0)
peed + motor_res,MaxSpeed); // Left less speed, Right full speed else BRAM_DriveM
BRAM_DriveMotor(Max Sotor(MaxSpeed,MaxSpeed - motor_res); // Left full speed, Right less speed
}
unsigned int getnumber(unsigned int min, unsigned int max)
{
int inumber;
scanf("%d",&inumber);
if (inumber < min || inumber > max) {
printf("\n\nInvalid [%d to %d]!",min,max);
_delay_ms(500); return -1; }
return inumber;
}
void Read_Parameter(void)
{
// Read the Kp,Ki and Kd From EEPROM at Address: 0x00,0x02,0x04
Kp=eeprom_read_word((unsigned int*) 0x0000);
Kd=eeprom_read_word((unsigned int*) 0x0004);
Ki=eeprom_read_word((unsigned int*) 0x0002);
}
// Assign I/O stream to UART
FILE uart_str = FDEV_SETUP_STREAM(uart_putch, uart_getch, _FDEV_SETUP_RW);
int main(void)
{
unsigned char mode,press_tm;
unsigned int ir_sensor;
int ichoice;
// Initial PORT Used
DDRB = 0b11111110; // Set PORTB: PB0=Input, Others as Output
PORTB = 0; DDRC = 0b00000000; // Set PORTC: Input
111; // Set PORTD: Output PORTD = 0
PORTC = 0xFF; // Pull-Up Input DDRD = 0b1111
1;
// Define Output/Input Stream
stdout = stdin = &uart_str;
// Initial UART Peripheral
uart_init();
// Initial BRAM Terminal Screen
ansi_me();
// Clear Screen
ansi_cl();
#if BRAM_DEBUG
ansi_cm(1,1);
printf("Welcome to AVRJazz Mega168 BRAM Debugging Mode");
#endif
// Initial The 8-bit PWM 0
// Fast PWM Frequency = fclk / (N * 256), Where N is the prescale
// f_PWM = 11059200 / (8 * 256) = 5400 Hz
ear OCA0/OCB0 on Compare Match, Set on TOP TCCR0B = 0b00000010; // Used 8 Prescaler
TCCR0A = 0b10100011; // Fast PWM 8 Bit, C l TCNT0 = 0; // Reset TCNT0 OCR0A = 0; // Initial the Output Compare register A & B
OCR0B = 0;
// Initial ATMega168 TWI/I2C Peripheral
TWSR = 0x00; // Select Prescale of 1
// SCL frequency = 11059200 / (16 + 2 * 48 * 1) = 98.743 kHz
TWBR = 0x30; // 48 Decimal
// Initial the MCP23008 Devices GP0 to GP4 Input, GP5 to GP7 Output
Write_MCP23008(IODIR,0b00011111);
// Enable Pull-Up on Input Write_MCP23008(GPIO,0b00111111);
Write_MCP23008(GPPU,0b00011111);
// Reset all the Output Port, Make GP5 High
// Set ADCSRA Register on ATMega168
ADCSRA = (1<<<
// Set ADMUX Register on ATMega168
ADMUX = (1<
// Initial the Motor
BRAM_DriveMotor(0,0);
mode=0; // Default BRAM Off
press_tm=0;
// Initial PID Control Variables
prev_res=0; prev_err_1=0;
50; // Initia
prev_err_2=0; MaxSpeed =
1l Maximum Speed
// Read Default BRAM Parameter from EEPROM
Read_Parameter();
// Loop Forever // Check if Butt
for(;;) { on is pressed than enter to the Setup Mode
// if button is pressed _delay_us(100);
if (bit_is_clear(PINB, PB0)) { // Wait for debouching if (bit_is_clear(PINB, PB0)) { press_tm++;
if (mode > 2) mode
if (press_tm > 100) { press_tm=0; mode++; = 0; } } } // Start conversion by setting ADSC on ADCSRA Register
ADCSRA |= (1&lt ;
// wait until convertion complete ADSC=0 -> Complete
while (ADCSRA & (1<
// Get ADC the Result
MaxSpeed = ADCH;
if (mode == 0) {
// Initial PID Control Variables
prev_res=0; prev_err_1=0;
prev_err_2=0;
}
if (mode == 1) {
// Read the IR Sensor
ir_sensor=BRAM_IRSensor();
// Give some delay Before PID Calculation
_delay_us(50);
// Execute the BRAM LFR PID Controller
BRAM_PIDControl(ir_sensor);
}
// Entering BRAM PID Setup Mode
if (mode == 2) {
tors BRAM_DriveMoto
// Stop BRAM M
or(0,0);
// Initial BRAM Terminal Screen
uart_flush(); // Flush UART
ansi_me();
ansi_cl(); // Clear Screen
ansi_cm(1,1);
e to AVRJazz Mega168 BRAM Setup");
printf("Welco m
ansi_cm(3,1);
printf("BRAM Maximum Speed Value: %d",MaxSpeed);
ansi_cm(5,1);
Proportional Parameter Factor (%d)\n",Kp); printf("2. Ki
printf("1. Kp -- Integral Parameter Factor (%d)\n",Ki);
Factor (%d)\n",Kd); printf("4. Save Parameters to the E
printf("3. Kd - Derivative Paramete rEPROM\n"); printf("5. Refresh Setup\n"); printf("6. Exit\n");
) continue; switch (ichoi
printf("\nEnter Choice: "); if ((ichoice=getnumber(1,6)) < 0ce) { case 1: // Kp Parameter printf("\n\nKp Parameter [0-1000]: ");
// Ki Parameter printf("\n\nKi Parameter [0
if ((Kp=getnumber(0,1000)) < 0) continue; break; case 2: -1000]: "); if ((Ki=getnumber(0,1000)) < 0) continue; break; case 3: // Kd Parameter
case 4: // Save to EEPROM // Wri
printf("\n\nKd Parameter [0-1000]: "); if ((Kd=getnumber(0,1000)) < 0) continue; break; te the Kp,Ki and Kd to EEPROM Address: 0x0000,0x0002,0x0004 eeprom_write_word((unsigned int*) 0x0000,Kp);
eeprom_write_word((unsigned int*) 0x0002,Ki);
eeprom_write_word((unsigned int*) 0x0004,Kd);
// Read BRAM Parameter from EEPROM
Read_Parameter();
break;
case 5: // Refresh Setup
// Start conversion by setting ADSC on ADCSRA Register
ADCSRA |= (1<
// wait until convertion complete ADSC=0 -> Complete
while (ADCSRA & (1<
// Get ADC the Result
MaxSpeed = ADCH;
// Read BRAM Parameter from EEPROM
Read_Parameter();
break;
case 6: // Exit Setup
mode = 0;
break;
ansi_cl() ; } } }
// Standard Return Code }
return 0;
/* EOF: bramflr.c */
The Power and DC Motor Control Circuits
The power circuit uses the Maxim MAX756 CMOS step-up DC to DC switching regulator to boost the 4.5 volt battery power up to 5 volt level.
The Maxim MAX756 could still work well by using just two AA Alkaline batteries (3 Volt) or two AA NiMH rechargeable batteries (2.4 Volt) and still giving you a solid 5 volt output to power your Line Follower Robot. Therefore by using this step-up switching regulator we could ensure BRAM II electronic circuits and DC motors will still work in their maximum performance until all the battery power being drained out.
BRAM II uses the “differential drive” for steering method which use two DC motors mounted in fixed positions on the left and right side of the BRAM II chassis. Each of these DC motors could rotate independently both in forward or reverse direction; therefore by adjusting both the motor’s rotation direction and speed we could easily control the BRAM II movement.
BRAM II LFR 01
The BRAM DC motors are connected to the popular dual channel H-Bridge SGS-Thompson L293D chip; by feeding the right logic from the AVR ATMega168 PORT-D I/O (PD2,PD3,PD4 and PD7) to the 4 buffers inputs we could get the desired result:
The C function that control the BRAM II steering is BRAM_Steer(), which simply supply the right logic to the L293D chip input ports according to the truth table shown above:
// Define BRAM Steering
#define MOVE_FORWARD 0
#define TURN_RIGHT 2
#define TURN_LEFT 1 #define ROTATE_LEFT 3
#define FULL_STOP 6
#define ROTATE_RIGHT 4 #define MOVE_BACKWARD 5 ... ...
case MOVE_FORWARD: PORTD &am
void BRAM_Steer(unsigned char steer) { switch(steer) {p;= ~(1 << PD4); PORTD |= (1 << PD7); // Right Motor On Forward
n Forward break; case TURN_LEFT: PORTD &= ~(1 << PD4); PORTD |
PORTD &= ~(1 << PD2); PORTD |= (1 << PD3); // Left Motor O= (1 << PD7); // Right Motor On Forward PORTD &= ~(1 << PD2); PORTD &= ~(1 << PD3); // Left Motor Off break;
D |= (1 << PD3
case TURN_RIGHT: PORTD &= ~(1 << PD4); PORTD &= ~(1 << PD7); // Right Motor Off PORTD &= ~(1 << PD2); POR T); // Left Motor On Forward break; case ROTATE_LEFT: PORTD &= ~(1 << PD4); PORTD |= (1 << PD7); // Right Motor On Forward
ORTD &= ~(1 << PD7); // Right Motor On Reverse PORTD &= ~(1 <<
PORTD |= (1 << PD2); PORTD &= ~(1 << PD3); // Left Motor On Reverse break; case ROTATE_RIGHT: PORTD |= (1 << PD4); PPD2); PORTD |= (1 << PD3); // Left Motor On Forward break; case MOVE_BACKWARD: PORTD |= (1 << PD4); PORTD &= ~(1 << PD7); // Right Motor On Reverse
// Right Motor Off PORTD &= ~(1 << PD2); PORTD &= ~(1 << PD3);
PORTD |= (1 << PD2); PORTD &= ~(1 << PD3); // Left Motor On Reverse break; case FULL_STOP: PORTD &= ~(1 << PD4); PORTD &= ~(1 << PD7) ; // Left Motor Off break; }
}
The DC motor speed is controlled by supplying the PWM (Pulse Width Modulation) to L293D input enableENABLE-1 and ENABLE-2 pins. In this design we use the AVR ATMega168 8-bit Timer/Counter0 PWM peripheral which give two independent PWM out on PD5 (OC0B) and PD6 (OC0A). For more information about using the PWM peripheral you could read my previous posted blogs Introduction to AVR Microcontroller Pulse Width Modulation (PWM) and Controlling Motor with AVR ATTiny13 PWM and ADC Project.
One of the important aspect when building a robot is to choose the precise PWM frequency for your DC motor as this could effecting your DC motor response to the PWM signal supplied to it. On this tutorial I apply 5400 Hz for the PWM frequency as it gives optimal response to the DC motor used on BRAM II. Following is the C code showing the AVR ATMega168 8-bit Timer/Counter0 PWM peripheral initialization:
// Initial The 8-bit Timer/Counter0 PWM
// Fast PWM Frequency = fclk / (N * 256), Where N is the prescaler
// f_PWM = 11059200 / (8 * 256) = 5400 Hz
ear OCA0/OCB0 on Compare Match, Set on TOP TCCR0B = 0b00000010; // Used 8 for the pr
TCCR0A = 0b10100011; // Fast PWM 8 Bit, C lescale TCNT0 = 0; // Reset TCNT0 OCR0A = 0; // Initial the Output Compare register A & B
OCR0B = 0;
By varying the value assigned to the AVR ATMega168 microcontroller output compare register OCR0Aand OCR0B we could varying the PWM signal from 0% to 100% (0 to 255). The C code that serve that purpose is the BRAM_DriveMotor() function which accept two arguments both for left and right DC motor speed. This function will also accept negative value arguments which are translated to the reverse DC motor rotation.
void BRAM_DriveMotor(int left_speed, int right_speed)
{ unsigned char left_pwm,right_pwm; ... ...
egisters A and B OCR0A=right_pwm; OCR0B=left_pwm; }
// Assigned the value to the PWM Output Compare
R
One last thing before you wire your DC motor power terminal to the L293D chip, is to find your DC motor power polarity where it give you the correct rotation direction base on the truth table shown above.
The Line Sensor Circuit
The line sensor is designed to takes advantage of the infra red reflective object sensor Junye JY209-01 or you could replace it with Fairchild QRE00034 to sense the black tape line; when the sensor is above the black tape line the infra red beam will not reflected back to the photo transistor and the photo transistor will turn off, if the sensor is on the white surface than the infra red beam will reflected back to the photo transistor and the photo transistor will turn on.
To make the sensor more sensitive and provide a smooth digital transition between “0” to “1” or vice verse; I use the NPN 2N3904 BJT to form the Darlington pair with the photo transistor to amplify the infra red signal received by the photo transistor, then feed the output (taken from the emitter) to the 74HC14 hex inverting Schmitt trigger chip. The infra red LED is controlled by NPN 2N3904 BJT through one of the 74HC14 inverting Schmitt trigger gate. For more information of using the transistor as switch you could read my previous blog Using Transistor as Switch.
BRAM II LFR 07
In order to lower the current taken from the MAX756 DC to DC Step-Up, two of the infra LED pairs are connected serially; this connection will produce about 3 x 20mA (60mA) instead of 5 x 20mA (100mA). I use the blue LED for displaying the sensor status as this type of LED need lesser current to produce decent light intensity compared to other LED. The reason why we have to carefully calculate the current needed, because all the electronic circuits and DC motors power is taken from the MAX756 chip which only capable to operate up to 200mA on 5 Volt output.
Finally the entire sensor output (IR0 to IR4) and input (CTRL) is connected to the Microchip MCP23008 I2C 8-bit I/O expander ports. The SCL and SDA pins connected directly to the AVR ATMega168 PORT-C PC5 (SCL) and PC4 (SDA) I2C bus pins.
As standard to all I2C devices; the MCP23008 has an unique 7 bits address consists of 4 bits device identification and 3 bits device physical address; the first 4 bits for MCP23008 device identification is “0100” and the last 3 bits of physical address is set to “111“, by connecting the A0A1 and A2 pins to the 5 Volt.
The read and write C code to the MCP23008 chip is provided by the Read_MCP23008() andWrite_MCP23008() functions respectively; these two functions used the MCP23008 I2C device protocol shown above to do this task. For more information of how to use the I2C devices please refer to my previous posted blog How to use I2C-bus on Atmel AVR Microcontroller.
Before we could use the MCP23008 I2C I/O expander, first we have to initialized the AVR ATMega168 microcontroller TWI (Two Wire Interfaces, Atmel own named for Philips I2C trademark) peripheral and assign the general purpose I/O GP0 to GP4 as the input port and GP5 to GP7 as the output port by writing to the MCP23008 IODIR register located on the address 0×00.
...
// Initial ATMega168 TWI/I2C Peripheral
TWSR = 0x00;         // Select Prescaler of 1
// SCL frequency = 11059200 / (16 + 2 * 48 * 1) = 98.743 khz
TWBR = 0x30; // 48 Decimal
// Initial the MCP23008 Devices GP0 to GP4 Input, GP5 to GP7 Output
Write_MCP23008(IODIR,0b00011111);
// Reset all the Output Port, Make GP5 High Write_MCP23008(GPPU,0b00011111);
Write_MCP23008(GPIO,0b00111111); // Enable Pull-Up on Input
...
Now you could read the reflective sensor status through the GP0 to GP4 port in the MCP23008 GPIOregister. To reduce the current consumed by these infra red LED; we only turn on these infra red LED when we read the data by setting the GP5 to “0” when we read the sensor and toggling back to “1” after we get the sensor status. The sensor reading task is done inside the BRAM_IRSensor() function.
...
// Turn On the Sensor IR LED
Write_MCP23008(GPIO,0b00011111);
// Read sensor
d_MCP23008(GPIO) & 0x1F;
sensor_val = Re a
// Turn Off the Sensor IR LED
Write_MCP23008(GPIO,0b00111111);
...
This function will also return the sensor value mapped according to the sensor_map array variable that is specifically designed to support our PID control algorithm.
// Define BRAM Variables
const unsigned int sensor_map[] PROGMEM = {
0b00000,0, 0b00001,10, 0b00011,20,
0b00100,60
0b00010,30, 0b00111,40, 0b00110,50 ,, 0b01100,70, 0b11100,80, 0b01000,90,
0b11000,100, 0b10000,110
};
If the black tape line is right at the center of all these five reflective sensors array, then the sensor circuit will return 0b00100, where the logical “1” means the third JY209-01 (connected to GP2) sensor position is right on the black tape line, while all the other sensors is on the white surface or logical “0“. This sensor’s value then will be mapped to the integer value 60 according to the sensor_map variable array above. This value later on will be used as our reference value (TARGET_VAL) for the PID control algorithm.
Now what happen if all of these sensors are out of the line, which mean the value returned by the sensor circuit will be 0b00000; the BRAM_IRSensor() function then will use it’s stored value (old_val variable) to determine whether the robot position is way out to the right of the line, it will return 0 or if the robot position is way out to the left of the line, it will return MAX_SENSOR_MAP (120). The following is the C code that serves that purpose:
// Define BRAM Sensor
#define MAX_MAP 24
60 #define MAX_SENSOR
#define TARGET_VAL
_MAP 120
...
...
val=0;
map _
// Adjust for zero result if previous value greater than TARGET_VAL
if (old_val > TARGET_VAL) map_val=MAX_SENSOR_MAP;
...
BRAM II Proportional, Integral and Derivative Control Algorithm
Now we come to the most interesting topics and yet it full of gray cloud and myth around it, especially for the robot builder. The proportional, integral and derivative control or PID for short actually is just one of the control methods that can be applied to the embedded system. The other popular controlling method includes the bang-bang and fuzzy logic is also widely used in the embedded system world.
From the diagram above you could see how I implement the PID control on the BRAM II Line Follower Robot. The BRAM_PIDControl() function get the reflective object sensor array output from theBRAM_IRSensor() function and calculate the required control response to steer correctly the BRAM II DC motors by supplying the required PWM value to the BRAM_DriveMotor() function.
The “Proportional Control” is used to fix the error produced by the line follower robot position toward the black tape line compared to reference value (TARGET_VAL) which represents the robot position is at the center of the line. Therefore we could write down the error function as this following formula:
ERROR = SENSOR_VALUE - TARGET_VALUE
The proportional control actually works similar to the gain (volume) control found at your stereo set where you could increase or decrease the music volume came out from the speaker, your ears act as the sensors which give you a feedback to your brain, while your target value is your preference music volume level. If  the music volume level suddenly become higher compared to your preference level (error occur) than you decrease the volume level and vice verse. This gain factor usually called as the Kp(proportional) factor in PID control term. Therefore we could right down the control response as this following formula:
RESPONSE = Kp x ERROR
As mention before the BRAM_IRSensor() function will return the sensor map value from 0 to 120 where0 mean our robot position is way out to the right while 120 mean our robot position is way out to the left; when BRAM_IRSensor() function return 60, mean our robot is right at the center of the line.
ERROR = SENSOR_VALUE - TARGET_VAL = 60 - 60 = 0
RESPONSE = Kp x ERROR = 0
If the response is 0 mean we could drive our robot to the maximum speed, so we could call theBRAM_DriveMotor() function as follow:
BRAM_DriveMotor(Left_Max_PWM, Right_Max_PWM);
When BRAM_IRSensor() function return 0
ERROR = SENSOR_VALUE - TARGET_VAL = 0 - 60 = -60
RESPONSE = Kp x ERROR = - 60 Kp
The negative response mean we have to turn or arc to the left to make the correction, this could be done by reducing the left motor PWM value (reducing left motor speed), while keep the right motor PWM value on it maximum value.
BRAM_DriveMotor(Left_Max_PWM + RESPONSE, Right_Max_PWM);
When BRAM_IRSensor() function return 120
ERROR = SENSOR_VALUE - TARGET_VAL = 120 - 60 = 60
RESPONSE = Kp x ERROR = 60 Kp
The positive response mean we have to turn or arc to the right to make the correction, this could be done by reducing the right motor PWM value (reducing right motor speed), while keep the left motor PWM value on it maximum value.
BRAM_DriveMotor(Left_Max_PWM, Right_Max_PWM - RESPONSE);
The Kp constant value could be any number that will boost the response value, following is the C code part of the BRAM_PIDControl() function that implements the explanation above:
...
...
egative result mean BRAM is on the right, so we need to adjust to the left //
// NPositive result mean BRAM is on the left, so we need to adjust to the right
ft less speed, Right
if (motor_res < 0) BRAM_DriveMotor(MaxSpeed + motor_res,MaxSpeed); // L efull speed else
(MaxSpeed,MaxSpeed - motor_res); // Left full speed, Right less speed
BRAM_DriveMoto r
Using just the “Proportional Control” alone will resulting the zigzag steering behavior of the robot, therefore we have to combine it with “Integral Control” or “Derivative Control” or both of them to produce more accurate and stable robot’s steering movement. The following is the industrial standard PID control mathematic formula:
The “Integral Control” is used to reduce the accumulated error produced by the proportional control over the time or it’s also called a steady-state error, therefore the longer the robot produces an error (not in the center of the black line) the higher the integral control response output value.
The “Derivative Control” is used to speed up the proportional control error response, therefore the faster the robot produce an error such as zigzag steering movement the higher the derivative control response output value. In other word the derivative control will help to reduce the zigzag steering behavior of the robot.
Now as you understand the PID concept it’s time to convert or transform the PID formula above to the form that could be applied directly to the AVR ATMega168 microcontroller. For the purpose of this tutorial I will not describe of how we discretization this mathematic formula using the trapezoidal rule, the following is the complete PID control formula taken from the Thomas Braunl, Embedded Robotics, Second Edition Springer-Verlag 2006:
The following is the BRAM II Line Follower Robot C code that implements the PID control formula shown above:
...
unsigned char MaxSpeed; // Hold Motor Maximum Speed
unsigned int Kp,Ki,Kd; // PID Parameters
iables
int prev_res=0, prev_err_1=0, prev_err_2=0; // PID Control Va r
...
...
motor_res,err_func; flo
int
at KP,KI,KD,cont_res;
// Get the Error Function
err_func=sensor_val - TARGET_VAL;
// Divide all the PID parameters for decimal value
KP=Kp * 0.1; KI=Ki * 0.01;
KD=Kd * 0.01;
// Calculate the Motor Response using PID Control Equation
cont_res=(float)(prev_res + KP * (err_func - prev_err_1) + KI * (err_func + prev_err_1)/2.0
+ KD * (err_func - 2.0 * prev_err_1 + prev_err_2));
Motor Value motor_res=(int)cont_res;
// Now we have to Limit the control response to the Maximum of our motor PW
M
if (motor_res > MaxSpeed)
motor_res = MaxSpeed;
if (motor_res < -MaxSpeed)
motor_res = -MaxSpeed;
// Save the Motor Response and Error Function Result
prev_res=motor_res; prev_err_2=prev_err_1;
prev_err_1=err_func;
Once you run the program you could tune the PID control parameter by entering BRAM II setup mode (pressing the AVRJazz Mega168 board user switch twice) and connecting the AVRJazz Mega168 RS232 port to your computer terminal program such as Windows HyperTerminal or puTTY using these following guide lines:
  • Turn off all the Integral and Derivative control by zeroing the Ki and Kd parameters
  • Slowly increase the Proportional parameter (Kd) by the factor of ten (i.e. 10, 20, 30,…) and watch the robot behavior, if the robot tend to oscillate (zigzag) decrease this parameter
  • Slowly increase the Integral parameter (Ki) by the factor of one (i.e. 1, 2, 3,…) again watch the robot behavior until it nearly oscillate
  • Slowly increase the Derivative parameter (Kd) by the factor of one (i.e. 1, 2, 3,…) again watch the robot behavior until it become stable
  • You have to repeat these whole processes for different speed setting. Remember there are no such exact values for the PID parameter; the PID parameter depends heavily on your motor and sensor characteristic.
Inside the C Code
The program start by initializing the ATmega168 ports, than we continue with the UART peripheral, 8-bit Timer/Counter0 PWM peripheral, TWI peripheral and finally the ADC peripheral that is used to set the maximum speed of the DC Motors. Again most of the functions used in this project are based on my previous posted blogs mention above.
The UART Functions:
  • The UART functions are used to handle the communication with the PC:
  • uart_init() function is used to initialize the ATMega168 UART peripheral
  • uart_flush() function is used to empty the ATMega168 UART data register
  • uart_putch() function is used to put single character to the UART port
  • uart_getch() function is used to read single character from the UART port
  • ansi_cl() function is used to clear the ANSI emulation terminal screen
  • ansi_me() function is used to turn off all attribute on the ANSI terminal emulation
  • ansi_cm() function is used to move cursor on the ANSI terminal emulation
  • get_num() function is used to validate the user input; this function used the standard I/O C function scanf() to read the user input from the HyperTerminal or puTTY program
The application standard input and output stream is being redirected to both UART input and output with theses statements bellow:
// Assign I/O stream to UART
FILE uart_str = FDEV_SETUP_STREAM(uart_putch, uart_getch, _FDEV_SETUP_RW);
...
// Define Output/Input Stream
stdout = stdin = &uart_str;
The BRAM II Motor Functions:
The BRAM II motor functions are used to control the BRAM II motor steering and speed:
  • BRAM_Steer() function is used as the basic function to control the DC motor steer where it supply correct logic to the L293D chip
  • BRAM_DriveMotor() function is used to control the BRAM II steer and speed
  • BRAM_PIDControl() function is the BRAM II implementation of Porportional, Integral and Derivative control
The MCP23008 I2C Functions:
The I2C functions are used to perform reading and writing to the I2C devices:
  • i2c_transmit() function is used to transmit data to the I2C devices and wait for transmission done by monitoring the interrupt flag bit (TWINT) to be set.
  • i2c_start() function is used to send the I2C start condition
  • i2c_stop() function is used to send the I2C stop condition
  • i2c_write() function is used to write data to the I2C slave device register
  • i2c_read() function is used to read data from the I2C slave device register
  • Write_MCP23008() function is used to send data to the MCP23008 register
  • Read_MCP23008() function is used to read data from the MCP23008 register
Inside the infinite loop
BRAM II used the AVRJazz Mega168 board user potentiometer connected to the AVR ATMega168 microcontroller PC0 (ADC0) pin to determine the BRAM II maximum speed. The ADC value read from this port will be used as the maximum PWM value for controlling the motor’s speed. Because we use 8-bit PWM value, therefore we only need 8-bit ADC precision from the 10-bit ADC precision provided by the AVR ATMega168 ADC peripheral. This following C code showing the ADC peripheral initiation where we use the right justified for the ADC result taken from the ADCH register. For more information of working with ADC peripheral you could read my previous posted blog Analog to Digital Converter AVR C Programming.
...
// Set ADCSRA Register on ATMega168
ADCSRA = (1<<<
// Set ADMUX Register on ATMega168
ADMUX = (1<
// wait until convertion complete ADSC=0 -> Complete
while (ADCSRA & (1<
// Get ADC the Result
MaxSpeed = ADCH;
Inside the for-loop (infinite loop) we simply read the AVRJazz Mega168 on board user switch (connected to AVR ATMega168 PB0 pin) and if pressed then we will run the BRAM II (mode 1) and if pressed twice (mode 2) we will enter the BRAM II parameter setup using Windows HyperTerminal or puTTY program and later on store this setup on the AVR ATMega168 microcontroller EEPROM.
...
// Write the Kp,Ki and Kd to EEPROM Address: 0x0000,0x0002,0x0004
eeprom_write_word((unsigned int*) 0x0000,Kp);
eeprom_write_word((unsigned int*) 0x0004,Kd);
eeprom_write_word((unsigned int*) 0x0002,Ki);
The stored PID control parameter later on will be read back from the EEPROM by theRead_Parameter() function every time we power on the BRAM II.
void Read_Parameter(void)
{
// Read the Kp,Ki and Kd From EEPROM at Address: 0x00,0x02,0x04
Kp=eeprom_read_word((unsigned int*) 0x0000);
Kd=eeprom_read_word((unsigned int*) 0x0004);
Ki=eeprom_read_word((unsigned int*) 0x0002);
}
Compile and Download the Code to the board
Before compiling the code, we have to make sure the AVR Studio 4.17 configuration is set properly by selecting menu project -> Configuration Option, the Configuration windows will appear as follow:
Make sure the Device selected is atmega168 and the Frequency use is 11059200 hz and because we use  float number calculation, you should include the link to AVR-GCC libm.a library object in your libraries option to reduce the program size.
After compiling and simulating our code we are ready to down load the code using the AVRJazz Mega168 bootloader facility. The bootloader program is activated by pressing the user switch and reset switch at the same time; after releasing both switches, the 8 blue LED indicator will show that the bootloader program is activate and ready to received command from Atmel AVR Studio v4.17 STK500 program.
We choose the HEX file and press the Program Button to down load the code into the AVRJazz Mega168 board. Now it’s time to relax and enjoy your hard work by watching BRAM II Line Follower Robot in action:
The Final Thought
As you’ve learned from this BRAM II Line Follower Robot tutorial, to design and build the microcontroller based robot successfully, you need to use many of the microcontroller supported peripheral features (e.g. I/O, PWM, UART, ADC, TWI) at the same time, therefore having a good and solid understanding of how these peripheral work will help you solve most of the problem occur when building the microcontroller based robot.

No comments:

Post a Comment