Turbo Kat Racers

33
1 20/09/2010 FREESCALE SMART CAR RACE FREESCALE SMART CAR RACE FREESCALE SMART CAR RACE FREESCALE SMART CAR RACE INDIA 2010 INDIA 2010 INDIA 2010 INDIA 2010 TECHNICAL REPORT TURBO KAT RACERS TURBO KAT RACERS TURBO KAT RACERS TURBO KAT RACERS S.SIVAPRAKASH R.MADHUSUDHANAN

Transcript of Turbo Kat Racers

Page 1: Turbo Kat Racers

1

20/09/2010

FREESCALE SMART CAR RACE FREESCALE SMART CAR RACE FREESCALE SMART CAR RACE FREESCALE SMART CAR RACE

INDIA 2010INDIA 2010INDIA 2010INDIA 2010

TECHNICAL REPORT

TURBO KAT RACERSTURBO KAT RACERSTURBO KAT RACERSTURBO KAT RACERS

S.SIVAPRAKASH R.MADHUSUDHANAN

Page 2: Turbo Kat Racers

2

INDEX

1. INTRODUCTION 3

2. STUDY OF HARDWARE 4

2.1 HARDWARE INTERFACE BLOCK 4

2.2 POWER SUPPLY 4

2.3 SENSOR 4

2.4 SERVO MOTOR 5

2.5 DC MOTOR 6

3. SOFTWARE DESIGN 7

3.1 TABLE OF FUNCTIONS 7

3.2 TABLE OF VARIABLES 8

3.3 PROGRAM FLOWCHART 9

3.4 PROGRAM DESCRIPTION 10

3.4.A SENSOR CALIBRATION 10

3.4.B CALCULATION OF COMPENSATED READING 10

3.4.C CALCULATION OF CURRENT POSITION & ERROR 10

3.4.D SPEED FUNCTION 11

3.4.E STEER FUNCTION 12

4. CONTROL SYSTEM DESIGN 14

4.1 OVERVIEW OF PID CONTROLLER 14

4.2 SMART CAR CONTROL SYSTEM DESIGN 15

5. DEVELOPMENT TOOLS & DEBUG PROCESS 17

5.1 TOOLS USED 17

5.2 DEBUGGING PROCESS 18

6. KEY TECHNICAL PARAMETERS OF SMART CAR 18

7. BIBLIOGRAPHY 19

APPENDIX A: SMART CAR PROGRAM 20

Page 3: Turbo Kat Racers

3

1. INTRODUCTION

The Freescale Smart Car Race India 2010 is conducted by Freescale

Semiconductors to enhance the creativity and technical knowledge and to

familiarize the students with the Freescale S12X microcontroller among the

Indian Students.

This Competition gives us an opportunity to practice the programming

skills we learnt and the knowledge of the Microcontroller and programming

tools. It also provides us a good platform to learn the control system concepts

and programming concepts practically. At the same time, it also gives us a

chance to improve our Team Working and Self Learning skills.

The following Technical Report includes our work on developing a

Software design for Smart Car which could run on any given track

autonomously. It covers the study of hardware, provided by the Freescale

Semiconductors; Complete software design and Control system design.

The Competition involves the race between all Smart Cars, where it

should run on the track, given that day, for 2 LAPS, without being running out

of the track or stopping in between.

The rules of the competition are simple.

Be FAST!!! Be STEADY!!!

Page 4: Turbo Kat Racers

4

2. STUDY OF HARDWARE MODULES

2.1 HARDWARE INTERFACE BLOCK

2.2 POWER SUPPLY

The power for the car is supplied by a rechargeable battery. The battery

used here is a NI-CD battery, which can supply a voltage of 7.2V and current of

2A.

2.3 SENSOR

The Sensor board consists of Infrared emitting diode and an NPN silicon

phototransistor mounted side by side. The board is placed in the front of the car

which could cover a range of 8cm. The phototransistor responds to the radiation

emitted from the diode and the current produced is sent for processing.

The current produced varies depending on the reflecting surface colour and

it is highest for Black colour and lowest for White colour.

Page 5: Turbo Kat Racers

5

SENSOR COUNT : 7

SENSOR TYPE : SFH4550 (IR EMITTER) & SFH314 (PHOTO-TRANSISTOR)

ALIGNMENT : STRAIGHT

SENSOR PIN CONNECTION

2.4 SERVO MOTOR

SERVO MOTOR �STEER CONTROL

The servo motor is used for controlling the DIRECTION in which the car

moves. Only one servo motor, provided by Freescale is used. The servo expects a

pulse every 20 ms from PWM, in order to gain correct information about the

angle. Based on the width of the PWM signal, the angular motion of the SERVO

varies.

SERVO CONNECTION

SIGNAL PIN

Servo Motor 1 Port P Pin 2 / PWM2

Servo Motor 2 Port P Pin 3 / PWM3

SERVO CONTROL SIGNAL

The table shows the different pulse widths required to set the SERVO in

different positions.

POSITION PULSE WIDTH

RIGHT 1.715ms

CENTRE 1.415ms

LEFT 1.115ms

� SERVO TYPE - Futaba S3010

SENSOR PIN

SENSOR 0 -6 PORT AD PIN 0-6

Page 6: Turbo Kat Racers

6

2.5 DC MOTOR

The Car runs with the DC Motor. The speed of the car is controlled by

controlling the DC motor. The current in the DC motor is set by PWM signal which

drives the car at the desired speed.

The DC motor is interfaced with the PWM Module through the H- Bridge.

The H-Bridge IC runs the DC motor according to the PWM signal.

DC MOTOR CONNECTION

The table shows the DC motor control Signals and the direction in which

the Car runs with respect to that signal.

SIGNAL PIN DIRECTION

DC Motor Signal 1 Port P Pin 0 / PWM2 FORWARD

DC Motor Signal 2 Port P Pin 1 / PWM3 REVERSE

� DC MOTOR TYPE - RS389-ST/3545

DC MOTOR � SPEED CONTROL

Page 7: Turbo Kat Racers

7

3. SOFTWARE DESIGN

3.1 TABLE OF FUNCTIONS

FUNCTION PARAMETERS DESCRIPTION

ATD_init() - Initializing the port AD to read the

sensor values.

pwm_steer_init() -

Initializing the PWM pins for servo

motor and setting the frequency and

initial direction.

pwm_speed_init() -

Initializing the PWM pins for DC

motor and setting the frequency and

initial speed.

Readvalues() - Getting the initial Sensor readings for

Calibration.

ATD_getvalue() *char Storing Black and White values for

Calibrating the sensors.

compensation_fun() - Original readings are taken and

Compensation readings are found.

bubblesort() - Compensated readings are sorted

based on values & index.

Startrace() - Once the calibrations are over, Car

enters this infinite loop.

steer_regulate() - Sets the direction of the Car in every

loop.

speed_regulate() - Sets the speed of the Car in every

loop.

brakecar() - Used to have count on the laps and

stop the car.

swap() *int,*int Used during Bubble sorting of the

sensor readings for Error Calculation.

UpdatePID() -

A PID controller which takes in the

Error and gives the correction signal

to set the new direction.

Delay() int Used to set delay based on the input.

Tuning() - Used to set the gains and speeds.

Page 8: Turbo Kat Racers

8

3.2 TABLE OF VARIABLES

VARIABLE TYPE DESCRIPTION

POS_CENTRAL_SENSOR

1536 #define

1536 is the position of the car when

the car is at the centre of black line.

LEFT, STRAIGHT, RIGHT #define The value of ms to be given to set the

car in respective directions.

black[], white[] unsigned char Used to store the black and white

reading of each sensor for Calibration.

pos_data[] const int Used to store position data for Error

Calculation.

compen_ratio char Used to store the sensor’s

compensation ratio.

ERROR signed int Used to store Error value of Car’s

position.

current_pos int Holds the current position in which

the car runs.

pre_his[] int The previous positions of the car are

stored, used for retracing of car.

Stopcnt int The no. of laps the car has run is

stored, used for braking the car.

speedcnt, spdcnt_thresh int To count the no. of samples in retrace

and boost the speed of the car

MAXSPEED, MINSPEED char The maximum and minimum speed

for the car is stored here.

PID VARIABLES

dstate, prev_iTerm int Used to hold the previous derivative

and integral part values.

Kclamp int The clamping gain for the PID

controller.

iGain, pGain, dGain, int The gains for the PID controller are

stored here.

pTerm, dTerm, iTerm, int The Proportional, Derivative and

Integral correction terms are stored.

pid_value int Used to store the Overall Correction

Value for the direction, in every loop.

Page 9: Turbo Kat Racers

9

3.3 PROGRAM FLOWCHART

START

GAIN AND SPEED

SETTING

PORT

INITIALIZATIONS

SENSOR

CALIBRATION

If Switch 1

Pressed

No

Yes

READ SENSOR

VALUES & FIND

LINE POSITION

SET PWM FOR

STEERING SERVO

(CALL PID)

SET PWM FOR

DRIVING MOTOR

IF NO. OF

LAPS = 2

No

END

Yes

Page 10: Turbo Kat Racers

10

3.4 PROGRAM DESCRIPTION

In this part the important strategies of the Smart Car Program are

explained.

3.4.A SENSOR CALIBRATION

For calibrating the sensors to overcome the practical errors and to

get the extreme values for white and black values, the following steps are used.

Initially the sensor is read for the Black and White values. The original

reading is then fed to the below formula to get the COMPENSATION RATIO for

each sensor.

3.4.B CALCULATION OF COMPENSATED READING

In every loop of the car’s run the compensated reading of the sensors

is calculated as follows.

The original reading of each sensor from the ATD is taken and after

some process the compensated reading is achieved. Below is the function that

converts the original reading into required reading.

3.4.C CALCULATION OF CURRENT POSITION & ERROR

Once the compensated readings are found, the top three sensor

readings and their indexes are bubble sorted.

Current position = Index of Middle sensor *2* ATD resolution (8bit=256) +

(Largest index sensor reading - Smallest index sensor reading)

Compensation Ratio = 256/(Black - White)

Compensated Reading = (Original Reading - White) * Compensation Ratio

Page 11: Turbo Kat Racers

11

The current position of the car, based on the sensors below which

the black line of track lies, is found as above.

Now it is important to find the sensor which is above centre of the

black line. This helps in finding the amount of drive required to keep the car in the

centre of the Black line. As there are seven sensors equally spaced, it can be seen

that the car is running exactly at the centre, if the fourth sensor is on the middle of

black line.

POSITION DATA TABLE

Sensor 6 5 4 3 2 1 0

Position 3072 2560 2048 1536 1024 512 0

The above table shows the position data which shows the values the

current position variable would read when the corresponding sensor stays on top

of middle of Black line on the Track.

Looking from the above table, it can be seen that the current position

of the car would be 1536 if the car is running exactly on the centre of the track.

Now the Error, i.e. the difference between the current position and

1536(values of current pos. for car’s run on middle of track) can be found by

finding the difference between the current position and 1536.

ERROR = |CURRENT POSITION – 1536|

3.4.D SPEED FUNCTION

Based on the value of the steer correction signal a speed correction signal is

produced which is subtracted from Maximum speed applied to the Car. Eventually

when the steer is at its extreme the speed is at its lowest.

The formula below gives the relation between Steer and Speed control

signal.

Page 12: Turbo Kat Racers

12

SPEED CORRECTION = ((MAXSPEED - MINSPEED)* STEER CONTROL)/290

A special speed function has been applied for conditions requiring some

boost up in the speed. Whenever the car has maximum turn the normal speed is

not enough to move the car, thus more speed (BOOSTSPEED) is applied to move

it quickly without any sluggishness. The boost up is also done during the retracing

run of the car.

3.4.E STEER FUNCTION

The Steer Function of the Smart Car program can be explained

through the below Flowchart.

START

IF START

LINE FOUND

BRAKE CAR ()

IF CAR ON

LINE

SET PWM FOR

SERVO (CALL PID) RETRACE TRACK

Yes

END

Yes

No

No

Page 13: Turbo Kat Racers

13

As the above flowchart shows the steer function starts with the checking

for start line. Once the start line is detected at the end of 2 Laps the brake is

applied and the car is stopped. A dynamic braking function is used to stop the car

as quick as possible after the finish. If not, based on the current position of the

car, the correction value, as achieved from the PID function, is applied to drive the

car to the required position.

A special strategy is applied to bring the car back on track if it runs out of

the track.

Once the correction signal is achieved the value is added or subtracted

from the STRAIGHT value to steer LEFT or RIGHT.

RETRACING ALGORITHM

In order to bring the car back in track, whenever it goes out of the line, this

special algorithm is applied. According to this algorithm the positions of the car

are saved in array. At any instant up to eight previous positions are available.

Based on these previous position values the car is driven accordingly to bring it

back on track.

PRE HISTORY DRIVE

Positions indicating right turn RIGHT

Positions indicating left turn LEFT

Page 14: Turbo Kat Racers

14

4. CONTROL SYSTEM DESIGN

The Core of the control system used in the program is a PID Controller. A

PID controller is a closed loop robust controller.

4.1 OVERVIEW OF PID CONTROLLER

What is PID..?

A proportional–integral–derivative controller (PID controller) is a generic

control loop feedback mechanism (controller) widely used in industrial control

systems – a PID is the most commonly used feedback controller. A PID controller

calculates an "error" value as the difference between a measured process variable

and a desired Setpoint. The controller attempts to minimize the error by adjusting

the process control inputs. In the absence of knowledge of the underlying

process, a PID controller is the best controller.

THE PID ALGORITHM

The PID controller calculation (algorithm) involves three separate

parameters, and is accordingly sometimes called three-term control: the

proportional, the integral and derivative values, denoted P, I, and D. The

proportional value determines the reaction to the current error, the integral value

determines the reaction based on the sum of recent errors, and the derivative

value determines the reaction based on the rate at which the error has been

changing. The weighted sum of these three actions is used to adjust the process

via a control element such as the position of a control valve or the power supply

of a heating element.

Heuristically, these values can be interpreted in terms of time: P depends

on the present error, I on the accumulation of past errors, and D is a prediction of

future errors, based on current rate of change.

Page 15: Turbo Kat Racers

15

EVALUATION OF CONTROLLER RESPONSE

The response of the controller can be evaluated in terms of the factors

given below.

a) The responsiveness of the controller towards an error.

b) The degree to which the controller overshoots the setpoint.

c) The degree of system oscillation.

It is based on the above three factors the Proportional gain, Integral gain

and Differential gains are tuned respectively.

4.2 SMART CAR CONTROL SYSTEM DESIGN

The control system design of the Smart Car is represented as a block

diagram below.

PID CONTROLLER BLOCK DIAGRAM

Page 16: Turbo Kat Racers

16

CONTROL SYSTEM WORKING

In each sample, once the Error is calculated the controller is called. The

controller takes in the error and produces the three terms after some process.

The three terms are then summed up to produce the correction value. The

correction value is then applied to the steer the car in the required direction. The

process of achieving the three controller terms are given below.

PROPORTIONAL TERM:

The proportional term is achieved by scaling the error in terms of the

Proportional gain (Kp).

INTEGRAL TERM:

The Integral term is got by adding the product of Integral gain (Ki) and error

with the previous integral terms. The method is used here for integration is based

on the EULER’s FORWARD RULE. In order to prevent the Smart Car from Integral

windup problem an ANTI-WINDUP window is set up.

� ANTI – WINDUP

In order to establish the Anti windup, a gain namely Kclamp has been

introduced before the summing point of the integral arm of the

controller. The control signal is checked every time and whenever it is out

of bound the integral term is cut off by making the Kclamp zero. In all

other conditions the Kclamp is held one.

DIFFERENTIAL TERM:

For finding the differential term the difference between the present and

previous positions of the car is multiplied with the differential gain (Kd).

Page 17: Turbo Kat Racers

17

CONTROLLER GAIN TUNING

The controller gains are tuned to various values in order to provide a good

run for the car. The gains are tuned by trial and error method. The steps followed

in the tuning of the gains are given below.

1) The Proportional gain is set to critical gain with other two set to zero.

2) The Differential gain is set to a high value and then reduced until the

oscillations are suppressed.

3) The Proportional is now again set to a higher value and decreased to see no

oscillation.

4) Finally the Integral gain is increased starting from a lower value, until the

overshoot point.

5. DEVELOPMENT TOOLS & DEBUG PROCESS

The only development tool used in this project is CODE WARRIOR IDE V5.1.

Since only the software design was required no other development tool were

used.

5.1 TOOLS USED

Code warrior IDE contains many tools integrated which are used for

programming the smart car. The tools used are given below

� Source Code Editor: Text editor program created to write and edit

the source code.

� Compiler: Program that converts the source code in C language to

computer language.

� Linker: It associates all the required files and produces an

absolute file (.ABS).

� Burner: The program converts the .ABS file into an S-Record file.

Page 18: Turbo Kat Racers

18

� Debugger: It is used to download the object file into target and to

debug the program to eliminate errors.

5.2 DEBUGGING PROCESS

Below is the listing of some of the common errors displayed by the

compiler. The compiler lists only the syntax errors and the logical errors are found

only during the debugging process.

ERROR/WARNING DESCRIPTION SOLUTION

Symbol missing

Usually symbols such as

semi-colon or parenthesis

are missed.

Go to the line of error

and put the missing

symbol.

Possible loss of data A variable’s value the

may go out of bound.

Analyze the variable and

change type if required.

Name not declared Some variables may be

missing declaration.

The declaration for the

missing variable is given.

Recursive comments Comments having a no. of

Backslashes

This warning does not

require any action.

ERRORS FOUND DURING DEBUGGING:

1. Missing port initializations.

2. Wrong port assignments.

3. Value given is not same as the type of variable.

6. KEY TECHNICAL PARAMETERS OF SMART CAR

A. SAMPLING TIME OF THE CAR : 3.3ms

B. PROCESSOR UTILIZATION FACTOR:

U = 70/330 + 50/330 + 150/330 + 50/330 + 8/330 = 328/330

U = 99.39%

Page 19: Turbo Kat Racers

19

C. ACCURACY

a. TRACKING: 95%

b. SPEED: 95%

D. SOFTWARE USED: CODE WARRIOR IDE

E. DIMENSIONS: 31cm X 16 cm

7. BIBLIOGRAPHY

A. MC9S12XDT256 Micro controller User Manual.

B. PID Tutorial (http://www.engin.umich.edu/group/ctm/PID/PID.html).

C. PID Without a PhD, Tim Wescott, FLIR Systems.

D. Freescale Semiconductor Smart Car Race Reference Documents.

a. Mexico Car Race Guidelines.

b. Mexico Car Race Rules.

c. Mexico Car Race Requirements.

Page 20: Turbo Kat Racers

20

APPENDIX A: SMART CAR PROGRAM

#include <hidef.h> /* common defines and macros */

#include "derivative.h" /* derivative-specific definitions */

#define POS_CENTRAL_SENSOR 1536

#define LEFT 1115

#define STRAIGHT 1415

#define RIGHT 1715

///////////////////////////////////////// VARIABLE DECLARATIONS ////////////////////////////////////////

unsigned char black[]={0,0,0,0,0,0,0},white[]={0,0,0,0,0,0,0};

const int pos_data[]={0,512,1024,1536,2048,2560,3072};

unsigned char compen_ratio[]={0,0,0,0,0,0,0};

unsigned char index[]={0,1,2,3,4,5,6};

signed long int ERROR=0;

unsigned char original_reading[]={0,0,0,0,0,0,0},compen_reading[]={0,0,0,0,0,0,0},i,j;

unsigned char value[]={0,0,0,0,0,0,0};

unsigned char three_value[]={0,0,0},three_index[]={0,0,0};

unsigned char indx_middle, larg_indx_read, small_indx_read;

unsigned int current_pos;

unsigned int pre_his[]={0,0,0,0,0,0,0,0,0,0};

unsigned int stopcnt=0,speedcnt=0,retracecnt=0,bstcnt_thresh=0,minspd_cnt=0,minspd_thresh=0;

//speed boosting variables

unsigned int m=0,x=0; //speed scaling variables

unsigned char MAXSPEED,MINSPEED,BOOSTSPEED;

static char flagb=3,flagw=3; // 3 is assigned for reading ATD values by pressing switch correctly...

////////////////////////////////// TUNING VARIABLE DECLARATIONS //////////////////////////////////

unsigned char kp_port=0, ki_port=0, kd_port=0, MAXSPEED_port=0, MINSPEED_port=0;

///////////////////////////////////////// PID VARIABLES ///////////////////////////////////////////////////

signed int dState=0;

long int iGain, pGain, dGain; // put actual_values * 10000

long int pTerm=0, dTerm=0, iTerm=0, prev_iTerm=0;

int kclamp=1;

int pid_value=0;

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

Page 21: Turbo Kat Racers

21

/////////////////////////////////////// FUNCTION PROTOTYPES ////////////////////////////////////////////

void Tuning(void);

void ATD_init(void);

void pwm_steer_init(void);

void pwm_speed_init(void);

void ATD_getvalue(unsigned char *temp_variable[]);

void Readvalues(void);

void compensation_fun(void);

void bubblesort(void);

void swap(unsigned char *x,unsigned char *y);

void Startrace(void);

void steer_regulate(void);

void speed_regulate(void);

void UpdatePID(void);

void brakecar(void);

void Delay(unsigned char);

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

////////////////////////////////////////////// MAIN FUNCTION /////////////////////////////////////////////

void main(void)

{

DDRB = 0x00; // SETTING PORT B AS INPUT PORT.

DDRT=0xF0; // Setting Port T pin 4,5,6,7 as output for LED.

PTT=0xFF; // Turning off all LEDs.

Tuning(); // Setting gain and speed.

ATD_init();

pwm_steer_init(); // Calling steering PWM_Initialization.

Readvalues(); // Sensor Calibration.

PTT=0x0F; // First turning ON all LEDs for perfect toggling.

Startrace(); // Car Starts and Runs on Track.

} //END OF MAIN.

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

Page 22: Turbo Kat Racers

22

///////////////////// FUNCTION FOR READING INITIAL BLACK AND WHITE VALUES ////////////////////////

void Readvalues()

{

flagb=3;flagw=3; PTP_PTP5=1; //PTP_PTP7=1;

DDRP_DDRP5=0;DDRP_DDRP7=0; //Port P Pin 5 set to input

PERP_PERP5=1;PERP_PERP7=1; //Port P Pin 5 Pullup Enable

// for reading black values sw1...

while(flagb)

{

if(PTP_PTP5==0)

{

flagb--;

}

}

ATD_getvalue(&black);

PTT_PTT4=0; //LED 1 On

// for reading white values sw2...

while(flagw)

{

if(PTP_PTP7==0)

{

flagw--;

}

}

ATD_getvalue(&white);

PTT_PTT5=0; //LED 2 On

// compensation ratio=256/(black-white) * 100 multiplied with 100 for fixed point calculation...

for(i=0;i<7;i++)

{

compen_ratio[i]=25600/(black[i]-white[i]);

}

} //End of Readvalues();

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

Page 23: Turbo Kat Racers

23

///////////////////////////////////////////// START CAR /////////////////////////////////////////////////

void Startrace()

{

unsigned int flag=1; // THIS FLAG IS FOR PRESSING SWITCH...

while(flag)

{

PTT=~PTT;Delay(15); // Toggling ON all LEDs

if(PTP_PTP5==0)

flag=0;

else

flag=1;

}

PTT=0xFF; // Turning OFF all LEDs

pwm_speed_init(); // INITIALLY START THE CAR...

for(;;)

{

for(i=0;i<7;i++)

index[i]=i;

ATD_getvalue(&original_reading);

compensation_fun();

bubblesort();

steer_regulate(); // Servo regulate...

speed_regulate(); // Speed regulate...

} //end of for

} //end of startrace().

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

////////////////////////////////// READING VALUES FROM SENSORS //////////////////////////////////

void ATD_getvalue(unsigned char *temp_variable)

{

ATD0CTL2_ADPU=1; // ATD Enable

ATD0CTL2_ASCIE=1;

ATD0CTL5=0x10; // multisequence RE-TRIGGERING ATD.

while(!(ATD0CTL2_ASCIF)); //loop until COMPLETE SEQUENCE FLAG is set.

temp_variable[0]=ATD0DR0H;

temp_variable[1]=ATD0DR1H;

temp_variable[2]=ATD0DR2H;

Page 24: Turbo Kat Racers

24

temp_variable[3]=ATD0DR3H;

temp_variable[4]=ATD0DR4H;

temp_variable[5]=ATD0DR5H;

temp_variable[6]=ATD0DR6H;

}

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

///////////////////////////////// COMPENSATED READING CALCULATION /////////////////////////////////

void compensation_fun()

{

for (i=0;i<7;i++)

{

if (original_reading[i]>black[i])

original_reading[i]=black[i];

else if(original_reading[i]<white[i])

original_reading[i]=white[i];

}

////////////////////////////////////////////////////////////

// CALCULATING COMPENSATED READING....

/* Compensated Reading = (Reading - White) * Compensation Ratio */

for (i=0;i<7;i++)

{

compen_reading[i]=((original_reading[i]- white[i]) * compen_ratio[i])/100;

// DIVIDED BY 100 TO COMPENSATE FOR FIXED POINT REP.

value[i]=compen_reading[i];

}

}

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

////////////////////////////////////////// BUBBLE SORT FUNCTION ////////////////////////////////////////

void bubblesort()

{

// bubblesort for all seven (based on value).

for(i=0;i<(7-1);i++)

for(j=0;j<(7-(i+1));j++)

{

if(value[j] > value[j+1])

{

Page 25: Turbo Kat Racers

25

swap(&value[j],&value[j+1]);

swap(&index[j],&index[j+1]);

}

}

//filtering top three values only....

for(i=4;i<7;i++)

{

three_value[i-4]=value[i]; // since last three values hold highest values.

three_index[i-4]=index[i];

}

//bubblesort for first three values (based on index).

for(i=0;i<(3-1);i++)

for(j=0;j<(3-(i+1));j++)

{

if(three_index[j] > three_index[j+1])

{

swap(&three_index[j],&three_index[j+1]);

swap(&three_value[j],&three_value[j+1]);

}

}

//achieved line data...

// Calculating current position

indx_middle=three_index[1];

larg_indx_read=three_value[2];

small_indx_read=three_value[0];

current_pos = (indx_middle*2*256) + (larg_indx_read-small_indx_read);

/* current position = Index of Middle sensor *2* ATD resolution(8bit=256) + (Larest index sensor

reading - Smallest index sensor reading) */

}

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

///////////////////////////////////// SWAP FUNCTION FOR SORTING /////////////////////////////////////

void swap(unsigned char *x,unsigned char *y)

{

char temp;

temp = *x;

*x = *y;

*y = temp;

}

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

Page 26: Turbo Kat Racers

26

/////////////////////////////////////////// STEERING CONTROL ////////////////////////////////////////////

void steer_regulate()

{

int i=0,cptemp=0;

/****************************** START PAD LOGIC *********************************/

if(((three_index[0]+1)!=three_index[1] || (three_index[1]+1)!=three_index[2]) &&

((compen_reading[0]>compen_reading[1]) && (compen_reading[6]>compen_reading[5]) &&

(compen_reading[3]>compen_reading[1]) && (compen_reading[3]>compen_reading[5])))

{

if(((compen_reading[0]-compen_reading[1])>=30) && ((compen_reading[6]-

compen_reading[5])>=30))

{

PWMDTY23 = 1415; // SETTING STEER STRAIGHT FINALLY...

brakecar();

}

}

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

if (compen_reading[0]>=30 || compen_reading[1]>=30 || compen_reading[2]>=30 ||

compen_reading[3]>=30 || compen_reading[4]>=30 || compen_reading[5]>=30 ||

compen_reading[6]>=30)

{

if( current_pos<POS_CENTRAL_SENSOR)

{

ERROR = POS_CENTRAL_SENSOR - current_pos; // STEER LEFT

UpdatePID();

PWMDTY23 = 1415-pid_value; // PWMDTY23 = 1415-((kp * ERROR)/10000);

}

else if (current_pos>POS_CENTRAL_SENSOR )

{

ERROR = current_pos - POS_CENTRAL_SENSOR; // STEER RIGHT

UpdatePID();

PWMDTY23 = 1415+pid_value; // PWMDTY23 = 1415+((kp * ERROR)/10000); /10000)

}

pre_his[0]=current_pos;

for(i=8;i>1;i--)

{

pre_his[i-1]=pre_his[i-2];

}

}

Page 27: Turbo Kat Racers

27

/**************************** RETRACING ALGORITHM *****************************/

else if ((compen_reading[0]<30 && compen_reading[1]<30 && compen_reading[2]<30 &&

compen_reading[3]<30 && compen_reading[4]<30 && compen_reading[5]<30 &&

compen_reading[6]<30))

{

if(pre_his[0]>2580 || pre_his[1]>2580 || pre_his[2]>2580)

{

PWMDTY23=RIGHT;

}

else if((pre_his[0]<1000 || pre_his[1]<1000 || pre_his[2]<1000 || pre_his[3]<1000 ||

pre_his[4]<1000 || pre_his[5]<1000 || pre_his[6]<1000 || pre_his[7]<1000) || ((pre_his[0]>2490 &&

pre_his[0]<2565)) || ((pre_his[1]>2490 && pre_his[1]<2565) || (pre_his[2]>2490 && pre_his[2]<2565)))

{

PWMDTY23=LEFT;

}

// WAIT UNTIL RETRACES FOR MAINTAINING THE PREVIOUS STATE.

while(compen_reading[0]<30 && compen_reading[1]<30 && compen_reading[2]<30 &&

compen_reading[3]<30 && compen_reading[4]<30 && compen_reading[5]<30 &&

compen_reading[6]<30)

{

ATD_getvalue(&original_reading);

compensation_fun();

speedcnt++;

if (speedcnt<bstcnt_thresh)

{

PWMDTY0 = MINSPEED ;

}

else

{

PWMDTY0 = BOOSTSPEED; // boosting when out of track

}

}

}

} //end of steer()

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

Page 28: Turbo Kat Racers

28

////////////////////////////////////////////// SPEED CONTROL /////////////////////////////////////////////

void speed_regulate(void)

{ /**********************************************************/

if ((compen_reading[0]>20 && compen_reading[2]<30 && compen_reading[3]<30 &&

compen_reading[4]<30 && compen_reading[5]<30 && compen_reading[6]<30) ||

(compen_reading[0]<30 && compen_reading[1]<30 && compen_reading[2]<30 &&

compen_reading[3]<30 && compen_reading[4]<30 && compen_reading[6]>20))

{

speedcnt++;

}

else

{

speedcnt=0;

}

/////////////////////////////////////////////////////////////////

if (speedcnt<bstcnt_thresh)

{

x=pid_value/30;

PWMDTY0 = MAXSPEED - (m*x); // speed_regulate=m*x=((MAXSPEED-MINSPEED) *

pid_value)/300;

}

else

{

PWMDTY0 = BOOSTSPEED; // boosting at corner after 50 sample delay.

}

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

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

if(PWMDTY0<=27)

minspd_cnt++;

else

minspd_cnt=0;

/////////////////////////////////////////////////////////////////

if(minspd_cnt>minspd_thresh)

PWMDTY0 = MAXSPEED -1;

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

}

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

Page 29: Turbo Kat Racers

29

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

PID FUNCTION

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

void UpdatePID()

{

int i_subterm=0;

pTerm = pGain * ERROR; // calculate the proportional term

i_subterm = (iGain * ERROR * kclamp)/100; // DIVIDED BY TEN TO COMPENSATE FOR PRECISION.

iTerm = i_subterm + prev_iTerm;

prev_iTerm = iTerm; // calculate the integral term

dTerm = (dGain * (current_pos - dState)/10);

dState = current_pos; // calculate the differential term

pid_value = (pTerm + iTerm - dTerm)/10000;

if(pid_value<300)

kclamp = 1;

else

kclamp = 0;

}

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

//////////////////////////////////////// INITIALIZATION FUNCTIONS ///////////////////////////////////////

void ATD_init()

{

ATD0CTL1=0x87; // no external trigger

ATD0CTL2_ADPU=1; // ATD Enable

ATD0CTL2_ETRIGE=0; // no external trigger

ATD0CTL3_S8C=1; // 8 adc channels sequence

ATD0CTL4_SRES8=1; // 8-bit Resolution

ATD0CTL5=0x10; // multisequence

}

Page 30: Turbo Kat Racers

30

/////////////////////////////// STEERING SERVO MOTOR INITIALIZATION //////////////////////////////////

void pwm_steer_init(void)

{

PWMCTL_CON23=1;

PWME_PWME2=1; //PWM channel 2 Enable

PWME_PWME3=1; //PWM channel 3 Enable

PWMPOL_PPOL2=1; //PWM pulse High at begining of Period

PWMPOL_PPOL3=1; //PWM pulse High at begining of Period

PWMCLK_PCLK3=1; // clock SB as clock source for PWM

PWMPRCLK =0x00; //clock A = 2MHz clockB = 2MHz

PWMSCLB =1; //clock SB = clock B / (2 * 100) = 10KHz

PWMPER23 = 20000; // PWM Period 20ms 50Hz

PWMDTY23 = 1415; // SETTING STEER STRAIGHT INITIALLY.

}

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

/////////////////////////////////////// DC MOTOR INITIALIZATION ////////////////////////////////////////

void pwm_speed_init(void)

{

/*//////////////////////// H-BRIDGE //////////////////////////*/

DDRE_DDRE2=1; // Port E pin 2 & 3 set to output

DDRE_DDRE3=1;

PORTE_PE2=0; //H-bridge enable

PORTE_PE3=1;

/*//////////////////// SPEED MOTOR CONTROL ///////////////////*/

PWMPOL_PPOL0=1; //PWM pulse High at begining of Period

PWMPOL_PPOL1=1; //PWM pulse High at begining of Period

PWMPRCLK =0x00; //clock A = 2MHz clockB = 2MHz

PWMSCLA =5; //clock SA = clock A / (2 * 5) = 200KHz

PWMPER0 = 50; // PWM Period = 2KHz

PWMDTY0 = MAXSPEED;

PWMDTY1 = 0;

PWME_PWME0=1; //PWM channel enable

PWME_PWME1=1;

}

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

////////////////////////////////// BRAKE CAR AFTER COMPLETING 2 LAPS /////////////////////////////////

void brakecar()

{

stopcnt++;

Page 31: Turbo Kat Racers

31

switch(stopcnt)

{

case 1:

PTT=0xEF;break; //TURNING ON LED 1.

case 2:

PTT=0xCF;break; //TURNING ON LED 2.

case 3:

PTT=0x8F; //TURNING ON LED 3.

PWMDTY23 = 1415; // SETTING STEER STRAIGHT FINALLY...

/*//////////////////// BRAKE CONTROL MODULE ////////////////////*/

/* Setting motor in reverse direction for counteracting inertia after finish */

PWMDTY1 =100;

Delay(3);

PWMDTY1 =80;

Delay(2);

PWMDTY1 =60;

Delay(2);

PWMDTY1 =PWMDTY0;

/*///////////////////// DISABLING MOTOR /////////////////////////////////////*/

PORTE_PE2=1; //disable H-BRIDGE

PORTE_PE3=0;

//disable PWM Channels

PWME_PWME0=0;

PWME_PWME1=0; //SPEED MOTOR BACK

PWME_PWME2=0;

PWME_PWME3=0; //STEER MOTOR FRONT

/*///////////////////// DISABLING CAR ///////////////////////*/

for(;;)

{

PWMDTY23 = 1415; // SETTING STEER STRAIGHT FINALLY...

} //END OF RACE...

}

Delay(20); //for preventing two counts on the start pad at same time itself...

}

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

Page 32: Turbo Kat Racers

32

//////////////////////////////////////////////// Delay function /////////////////////////////////////////////

void Delay(unsigned char a)

{

unsigned int i,j;

for(j=1;j<=a;j++) //Delay DONT CHANGE ANY VALUE IN THIS FUNCTION...

for(i=0;i<=500;i++);

}

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

/////////////////////////////////////// GAIN AND SPEED SETTINGS /////////////////////////////////////////

void Tuning()

{

/////////////////////////////// TUNE HERE INITIALLY USING JUMBER PORT-B /////////////////////////////

kp_port= PORTB & 0x03; // PORT B PIN 0 AND 1 FOR pGain.

ki_port= PORTB & 0x0c; // PORT B PIN 2 AND 3 FOR iGain.

kd_port= PORTB & 0x30; // PORT B PIN 4 AND 5 FOR dGain.

MAXSPEED_port= PORTB & 0x40; // PORT B PIN 6 FOR MAXSPEED.

MINSPEED_port= PORTB & 0x80; // PORT B PIN 7 FOR MINSPEED.

switch (kp_port)

{

case 0x00: pGain=1462; break;

case 0x01: pGain=1461; break;

case 0x02: pGain=1460; break;

case 0x03: pGain=1458; break;

}

switch (ki_port)

{

case 0x00: iGain=1; break;

case 0x01: iGain=2; break;

case 0x02: iGain=3; break;

case 0x03: iGain=4; break;

}

switch (kd_port)

{

case 0x00: dGain=15; break;

case 0x01: dGain=16; break;

case 0x02: dGain=17; break;

case 0x03: dGain=18; break;

}

switch (MAXSPEED_port)

{

case 0x00: MAXSPEED=32; break;

case 0x40: MAXSPEED=33; break;

}

Page 33: Turbo Kat Racers

33

switch (MINSPEED_port)

{

case 0x00: MINSPEED=25; break;

case 0x80: MINSPEED=26; break;

}

BOOSTSPEED=34;

bstcnt_thresh=250;

minspd_thresh=500;

m=(MAXSPEED-MINSPEED)/10; //SPEED REGULATION RATIO BASED ON MAXSPEED & MINSPEED...

}

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