20/09/2010

FREESCALE SMART CAR RACE INDIA 2010

TECHNICAL REPORT

TURBO KAT RACERS

S.SIVAPRAKASH R.MADHUSUDHANAN

INDEX
1. INTRODUCTION 2. STUDY OF HARDWARE 2.1 HARDWARE INTERFACE BLOCK 2.2 POWER SUPPLY 2.3 SENSOR 2.4 SERVO MOTOR 2.5 DC MOTOR 3. SOFTWARE DESIGN 3.1 TABLE OF FUNCTIONS 3.2 TABLE OF VARIABLES 3.3 PROGRAM FLOWCHART 3.4 PROGRAM DESCRIPTION 3.4.A SENSOR CALIBRATION 3.4.B CALCULATION OF COMPENSATED READING 3.4.C CALCULATION OF CURRENT POSITION & ERROR 3.4.D SPEED FUNCTION 3.4.E STEER FUNCTION 4. CONTROL SYSTEM DESIGN 4.1 OVERVIEW OF PID CONTROLLER 4.2 SMART CAR CONTROL SYSTEM DESIGN 5. DEVELOPMENT TOOLS & DEBUG PROCESS 5.1 TOOLS USED 5.2 DEBUGGING PROCESS 6. KEY TECHNICAL PARAMETERS OF SMART CAR 7. BIBLIOGRAPHY APPENDIX A: SMART CAR PROGRAM 3 4 4 4 4 5 6 7 7 8 9 10 10 10 10 11 12 14 14 15 17 17 18 18 19 20

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!!!

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.
4

SENSOR COUNT SENSOR TYPE ALIGNMENT

: 7 : SFH4550 (IR EMITTER) & SFH314 (PHOTO-TRANSISTOR) : STRAIGHT

SENSOR PIN CONNECTION

SENSOR PIN SENSOR 0 -6 PORT AD PIN 0-6

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 Servo Motor 1 Servo Motor 2 PIN Port P Pin 2 / PWM2 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

5

2.5 DC MOTOR
DC MOTOR SPEED CONTROL

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 DC Motor Signal 1 DC Motor Signal 2 PIN Port P Pin 0 / PWM2 Port P Pin 1 / PWM3 DIRECTION FORWARD REVERSE

DC MOTOR TYPE - RS389-ST/3545

3. SOFTWARE DESIGN
3.1 TABLE OF FUNCTIONS
FUNCTION ATD_init() pwm_steer_init() PARAMETERS DESCRIPTION Initializing the port AD to read the sensor values. Initializing the PWM pins for servo motor and setting the frequency and initial direction. Initializing the PWM pins for DC motor and setting the frequency and initial speed. Getting the initial Sensor readings for Calibration. Storing Black and White values for Calibrating the sensors. Original readings are taken and Compensation readings are found. Compensated readings are sorted based on values & index. Once the calibrations are over, Car enters this infinite loop. Sets the direction of the Car in every loop. Sets the speed of the Car in every loop. Used to have count on the laps and stop the car. Used during Bubble sorting of the sensor readings for Error Calculation. A PID controller which takes in the Error and gives the correction signal to set the new direction. Used to set delay based on the input. Used to set the gains and speeds.
7

pwm_speed_init() Readvalues() ATD_getvalue() compensation_fun() bubblesort() Startrace() steer_regulate() speed_regulate() brakecar() swap() UpdatePID() Delay() Tuning()

*char *int,*int int -

3.2 TABLE OF VARIABLES

VARIABLE POS_CENTRAL_SENSOR 1536 LEFT, STRAIGHT, RIGHT black[], white[] pos_data[] compen_ratio ERROR current_pos pre_his[] Stopcnt speedcnt, spdcnt_thresh MAXSPEED, MINSPEED TYPE #define #define unsigned char const int char signed int int int int int char DESCRIPTION 1536 is the position of the car when the car is at the centre of black line. The value of ms to be given to set the car in respective directions. Used to store the black and white reading of each sensor for Calibration. Used to store position data for Error Calculation. Used to store the sensors compensation ratio. Used to store Error value of Cars position. Holds the current position in which the car runs. The previous positions of the car are stored, used for retracing of car. The no. of laps the car has run is stored, used for braking the car. To count the no. of samples in retrace and boost the speed of the car The maximum and minimum speed for the car is stored here. Used to hold the previous derivative and integral part values. The clamping gain for the PID controller. The gains for the PID controller are stored here. The Proportional, Derivative and Integral correction terms are stored. Used to store the Overall Correction Value for the direction, in every loop.
8

PID VARIABLES dstate, prev_iTerm Kclamp iGain, pGain, dGain, pTerm, dTerm, iTerm, pid_value int int int int int

3.3 PROGRAM FLOWCHART

START
GAIN AND SPEED SETTING

PORT INITIALIZATIONS

SENSOR CALIBRATION

No If Switch 1 Pressed 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

Yes END 9

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. Compensation Ratio = 256/(Black - White)

3.4.B CALCULATION OF COMPENSATED READING

In every loop of the cars 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. Compensated Reading = (Original Reading - White) * Compensation Ratio

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)
10

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 Position 6 3072 5 2560 4 2048 3 1536 2 1024 1 512 0 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 cars 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.
11

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

Yes No

BRAKE CAR ()

IF CAR ON LINE Yes SET PWM FOR SERVO (CALL PID) No RETRACE TRACK

END 12

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

13

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 proportionalintegralderivative 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.

14

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

15

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 EULERs 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).

16

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.
17

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
Symbol missing

Possible loss of data Name not declared Recursive comments

DESCRIPTION Usually symbols such as semi-colon or parenthesis are missed. A variables value the may go out of bound. Some variables may be missing declaration. Comments having a no. of Backslashes

SOLUTION Go to the line of error and put the missing symbol. Analyze the variable and change type if required. The declaration for the missing variable is given. 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%
18

C. ACCURACY a. TRACKING: b. SPEED: 95% 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.

19

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; /***********************************************************************************/

20

/////////////////////////////////////// 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) { // SETTING PORT B AS INPUT PORT. DDRB = 0x00; DDRT=0xF0; // Setting Port T pin 4,5,6,7 as output for LED. PTT=0xFF; // Turning off all LEDs. Tuning(); ATD_init(); pwm_steer_init(); Readvalues(); PTT=0x0F; Startrace(); // Setting gain and speed.

// Calling steering PWM_Initialization. // Sensor Calibration. // First turning ON all LEDs for perfect toggling. // Car Starts and Runs on Track.

} //END OF MAIN. ************************************************************************************/

21

///////////////////// 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; PERP_PERP5=1;PERP_PERP7=1; // 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(); /***********************************************************************************/ //Port P Pin 5 set to input //Port P Pin 5 Pullup Enable

22

///////////////////////////////////////////// START CAR ///////////////////////////////////////////////// void Startrace() { unsigned int flag=1; while(flag) { PTT=~PTT;Delay(15); if(PTP_PTP5==0) flag=0; else flag=1; } PTT=0xFF; pwm_speed_init(); 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. // THIS FLAG IS FOR PRESSING SWITCH...

// Toggling ON all LEDs

// Turning OFF all LEDs // INITIALLY START THE CAR...

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

temp_variable[0]=ATD0DR0H; temp_variable[1]=ATD0DR1H; temp_variable[2]=ATD0DR2H; 23

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]) { 24

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]; three_index[i-4]=index[i]; }

// since last three values hold highest values.

//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; } /***********************************************************************************/

25

/////////////////////////////////////////// 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; } else if (current_pos>POS_CENTRAL_SENSOR ) { ERROR = current_pos - POS_CENTRAL_SENSOR; UpdatePID(); PWMDTY23 = 1415+pid_value; } pre_his[0]=current_pos; for(i=8;i>1;i--) { pre_his[i-1]=pre_his[i-2]; } } 26 // PWMDTY23 = 1415+((kp * ERROR)/10000); /10000) // PWMDTY23 = 1415-((kp * ERROR)/10000);

// STEER RIGHT

/**************************** 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() /***********************************************************************************/

27

////////////////////////////////////////////// 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; /*******************************************************************/ } /***********************************************************************************/ 28

/*/////////////////////////////////////////////////////////////////////////////////////////////////////////// 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; dTerm = (dGain * (current_pos - dState)/10); dState = current_pos; pid_value = (pTerm + iTerm - dTerm)/10000; if(pid_value<300) kclamp = 1; else kclamp = 0; } /***********************************************************************************/ //////////////////////////////////////// INITIALIZATION FUNCTIONS /////////////////////////////////////// void ATD_init() { ATD0CTL1=0x87; ATD0CTL2_ADPU=1; ATD0CTL2_ETRIGE=0; ATD0CTL3_S8C=1; ATD0CTL4_SRES8=1; ATD0CTL5=0x10; } // no external trigger // ATD Enable // no external trigger // 8 adc channels sequence // 8-bit Resolution // multisequence // calculate the differential term // calculate the integral term

29

/////////////////////////////// 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 //PWM pulse High at begining of Period PWMPOL_PPOL2=1; 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; PORTE_PE3=1; //H-bridge enable

/*//////////////////// SPEED MOTOR CONTROL ///////////////////*/ PWMPOL_PPOL0=1; PWMPOL_PPOL1=1; PWMPRCLK =0x00; PWMSCLA =5; PWMPER0 = 50; PWMDTY0 = MAXSPEED; PWMDTY1 = 0; PWME_PWME0=1; PWME_PWME1=1; //PWM pulse High at begining of Period //PWM pulse High at begining of Period //clock A = 2MHz clockB = 2MHz //clock SA = clock A / (2 * 5) = 200KHz // PWM Period = 2KHz

//PWM channel enable

} /***********************************************************************************/ ////////////////////////////////// BRAKE CAR AFTER COMPLETING 2 LAPS ///////////////////////////////// void brakecar() { stopcnt++;

30

switch(stopcnt) { case 1: PTT=0xEF;break; case 2: PTT=0xCF;break; case 3: PTT=0x8F;

//TURNING ON LED 1. //TURNING ON LED 2. //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; PORTE_PE3=0; PWME_PWME0=0; PWME_PWME1=0; //disable H-BRIDGE //disable PWM Channels //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... } /***********************************************************************************/

31

//////////////////////////////////////////////// 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; } 32

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... } /***********************************************************************************/

33