20/09/2010

FREESCALE SMART CAR RACE INDIA 2010

TECHNICAL REPORT

TURBO KAT RACERS
S.SIVAPRAKASH R.MADHUSUDHANAN

1

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

2

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

3

4 .1 HARDWARE INTERFACE BLOCK 2. The battery used here is a NI-CD battery.3 SENSOR The Sensor board consists of Infrared emitting diode and an NPN silicon phototransistor mounted side by side.2.2V and current of 2A. 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. which can supply a voltage of 7. The current produced varies depending on the reflecting surface colour and it is highest for Black colour and lowest for White colour. STUDY OF HARDWARE MODULES 2. 2.2 POWER SUPPLY The power for the car is supplied by a rechargeable battery.

Only one servo motor.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. The servo expects a pulse every 20 ms from PWM.115ms SERVO TYPE . POSITION PULSE WIDTH RIGHT 1. provided by Freescale is used.4 SERVO MOTOR SERVO MOTOR STEER CONTROL The servo motor is used for controlling the DIRECTION in which the car moves. in order to gain correct information about the angle.415ms LEFT 1. 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. the angular motion of the SERVO varies. Based on the width of the PWM signal.Futaba S3010 5 .715ms CENTRE 1.

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 . 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. The speed of the car is controlled by controlling the DC motor.Bridge. The H-Bridge IC runs the DC motor according to the PWM signal.2.5 DC MOTOR DC MOTOR SPEED CONTROL The Car runs with the DC Motor.RS389-ST/3545 6 . DC MOTOR CONNECTION The table shows the DC motor control Signals and the direction in which the Car runs with respect to that signal.

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

white[] pos_data[] compen_ratio ERROR current_pos pre_his[] Stopcnt speedcnt. Used to store the sensor’s compensation ratio. in every loop. 8 PID VARIABLES dstate. of samples in retrace and boost the speed of the car The maximum and minimum speed for the car is stored here. pTerm. of laps the car has run is stored. Used to store the Overall Correction Value for the direction. The value of ms to be given to set the car in respective directions. Holds the current position in which the car runs. 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 no. prev_iTerm Kclamp iGain. used for retracing of car.2 TABLE OF VARIABLES VARIABLE POS_CENTRAL_SENSOR 1536 LEFT. pGain. The gains for the PID controller are stored here. pid_value int int int int int . iTerm. Used to hold the previous derivative and integral part values. RIGHT black[]. dGain. The clamping gain for the PID controller.3. The Proportional. The previous positions of the car are stored. Used to store the black and white reading of each sensor for Calibration. To count the no. spdcnt_thresh MAXSPEED. used for braking the car. dTerm. STRAIGHT. Derivative and Integral correction terms are stored. Used to store Error value of Car’s position. Used to store position data for Error Calculation.

OF LAPS = 2 No Yes END 9 .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.

B CALCULATION OF COMPENSATED READING In every loop of the car’s run the compensated reading of the sensors is calculated as follows. Compensated Reading = (Original Reading . The original reading is then fed to the below formula to get the COMPENSATION RATIO for each sensor. The original reading of each sensor from the ATD is taken and after some process the compensated reading is achieved. Compensation Ratio = 256/(Black .C CALCULATION OF CURRENT POSITION & ERROR Once the compensated readings are found.A SENSOR CALIBRATION For calibrating the sensors to overcome the practical errors and to get the extreme values for white and black values. Initially the sensor is read for the Black and White values.4. Below is the function that converts the original reading into required reading.3.4 PROGRAM DESCRIPTION In this part the important strategies of the Smart Car Program are explained. the following steps are used.White) 3.4. the top three sensor readings and their indexes are bubble sorted.4. 3. Current position = Index of Middle sensor *2* ATD resolution (8bit=256) + (Largest index sensor reading .White) * Compensation Ratio 3.Smallest index sensor reading) 10 .

4. is found as above. i.e. The formula below gives the relation between Steer and Speed control signal. As there are seven sensors equally spaced.The current position of the car. 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. based on the sensors below which the black line of track lies. This helps in finding the amount of drive required to keep the car in the centre of the Black line.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. for car’s run on middle of track) can be found by finding the difference between the current position and 1536. Eventually when the steer is at its extreme the speed is at its lowest. 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 it is important to find the sensor which is above centre of the black line. it can be seen that the car is running exactly at the centre. Now the Error. Looking from the above table. if the fourth sensor is on the middle of black line. 11 . ERROR = |CURRENT POSITION – 1536| 3. the difference between the current position and 1536(values of current pos.

The boost up is also done during the retracing run of the car. 3. Whenever the car has maximum turn the normal speed is not enough to move the car.E STEER FUNCTION The Steer Function of the Smart Car program can be explained through the below Flowchart. thus more speed (BOOSTSPEED) is applied to move it quickly without any sluggishness.SPEED CORRECTION = ((MAXSPEED .MINSPEED)* STEER CONTROL)/290 A special speed function has been applied for conditions requiring some boost up in the speed.4. 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 .

Based on these previous position values the car is driven accordingly to bring it back on track. is applied to drive the car to the required position.As the above flowchart shows the steer function starts with the checking for start line. If not. as achieved from the PID function. At any instant up to eight previous positions are available. whenever it goes out of the line. A dynamic braking function is used to stop the car as quick as possible after the finish. PRE HISTORY DRIVE Positions indicating right turn RIGHT Positions indicating left turn LEFT 13 . RETRACING ALGORITHM In order to bring the car back in track. Once the correction signal is achieved the value is added or subtracted from the STRAIGHT value to steer LEFT or RIGHT. Once the start line is detected at the end of 2 Laps the brake is applied and the car is stopped. the correction value. this special algorithm is applied. based on the current position of the car. According to this algorithm the positions of the car are saved in array. A special strategy is applied to bring the car back on track if it runs out of the track.

? 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. based on current rate of change. Heuristically. THE PID ALGORITHM The PID controller calculation (algorithm) involves three separate parameters. 4. a PID controller is the best controller. 14 . A PID controller is a closed loop robust controller. the integral and derivative values. and D is a prediction of future errors. the integral value determines the reaction based on the sum of recent errors.. In the absence of knowledge of the underlying process. and D.1 OVERVIEW OF PID CONTROLLER What is PID. The proportional value determines the reaction to the current error. The controller attempts to minimize the error by adjusting the process control inputs. and the derivative value determines the reaction based on the rate at which the error has been changing. A PID controller calculates an "error" value as the difference between a measured process variable and a desired Setpoint. I on the accumulation of past errors. and is accordingly sometimes called three-term control: the proportional. 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. these values can be interpreted in terms of time: P depends on the present error.4. denoted P. CONTROL SYSTEM DESIGN The Core of the control system used in the program is a PID Controller. I.

2 SMART CAR CONTROL SYSTEM DESIGN The control system design of the Smart Car is represented as a block diagram below. c) The degree of system oscillation. a) The responsiveness of the controller towards an error. 4. b) The degree to which the controller overshoots the setpoint. Integral gain and Differential gains are tuned respectively.EVALUATION OF CONTROLLER RESPONSE The response of the controller can be evaluated in terms of the factors given below. It is based on the above three factors the Proportional gain. PID CONTROLLER BLOCK DIAGRAM 15 .

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. once the Error is calculated the controller is called. 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. a gain namely Kclamp has been introduced before the summing point of the integral arm of the controller. INTEGRAL TERM: The Integral term is got by adding the product of Integral gain (Ki) and error with the previous integral terms. 16 . ANTI – WINDUP In order to establish the Anti windup.CONTROL SYSTEM WORKING In each sample. The controller takes in the error and produces the three terms after some process. In order to prevent the Smart Car from Integral windup problem an ANTI-WINDUP window is set up. The process of achieving the three controller terms are given below. The method is used here for integration is based on the EULER’s FORWARD RULE. 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). PROPORTIONAL TERM: The proportional term is achieved by scaling the error in terms of the Proportional gain (Kp).

ABS file into an S-Record file. until the overshoot point. 5. 2) The Differential gain is set to a high value and then reduced until the oscillations are suppressed. 17 . 3) The Proportional is now again set to a higher value and decreased to see no oscillation. The tools used are given below Source Code Editor: Text editor program created to write and edit the source code. DEVELOPMENT TOOLS & DEBUG PROCESS The only development tool used in this project is CODE WARRIOR IDE V5.1.CONTROLLER GAIN TUNING The controller gains are tuned to various values in order to provide a good run for the car. 1) The Proportional gain is set to critical gain with other two set to zero. The gains are tuned by trial and error method. 4) Finally the Integral gain is increased starting from a lower value. Compiler: Program that converts the source code in C language to computer language. Burner: The program converts the .1 TOOLS USED Code warrior IDE contains many tools integrated which are used for programming the smart car. Linker: It associates all the required files and produces an absolute file (. 5. The steps followed in the tuning of the gains are given below. Since only the software design was required no other development tool were used.ABS).

A variable’s value the may go out of bound. 3.39% 18 . KEY TECHNICAL PARAMETERS OF SMART CAR A. Some variables may be missing declaration.2 DEBUGGING PROCESS Below is the listing of some of the common errors displayed by the compiler. Analyze the variable and change type if required. PROCESSOR UTILIZATION FACTOR: U = 70/330 + 50/330 + 150/330 + 50/330 + 8/330 = 328/330 U = 99. of Backslashes SOLUTION Go to the line of error and put the missing symbol. Missing port initializations. 5. Wrong port assignments. 6. This warning does not require any action. Value given is not same as the type of variable. Comments having a no. 2.Debugger: It is used to download the object file into target and to debug the program to eliminate errors. 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. The declaration for the missing variable is given. SAMPLING TIME OF THE CAR : 3.3ms B. ERRORS FOUND DURING DEBUGGING: 1.

BIBLIOGRAPHY A. B.C. Mexico Car Race Rules.umich. TRACKING: b. Mexico Car Race Requirements. PID Without a PhD. SPEED: 95% 95% D. SOFTWARE USED: CODE WARRIOR IDE E. ACCURACY a. DIMENSIONS: 31cm X 16 cm 7. D. PID Tutorial (http://www. Mexico Car Race Guidelines. Freescale Semiconductor Smart Car Race Reference Documents.html). MC9S12XDT256 Micro controller User Manual. C. Tim Wescott. c. 19 . b. a. FLIR Systems.edu/group/ctm/PID/PID.engin.

int pid_value=0.1.h" /* derivative-specific definitions */ #define POS_CENTRAL_SENSOR 1536 #define LEFT 1115 #define STRAIGHT 1415 #define RIGHT 1715 ///////////////////////////////////////// VARIABLE DECLARATIONS //////////////////////////////////////// unsigned char black[]={0. larg_indx_read. unsigned char value[]={0.0.6}.0.0. // 3 is assigned for reading ATD values by pressing switch correctly.white[]={0.0.0.0}.APPENDIX A: SMART CAR PROGRAM #include <hidef. small_indx_read.4.0.speedcnt=0. unsigned char index[]={0.x=0. iTerm=0.. static char flagb=3.0.0. MINSPEED_port=0. unsigned int pre_his[]={0.0.minspd_cnt=0.0.0.bstcnt_thresh=0.0.0.0}. ///////////////////////////////////////// PID VARIABLES /////////////////////////////////////////////////// signed int dState=0. unsigned char compen_ratio[]={0. unsigned char three_value[]={0.0.h> /* common defines and macros */ #include "derivative. pGain. dGain. signed long int ERROR=0.BOOSTSPEED.three_index[]={0.0}.0.0}. MAXSPEED_port=0.2048.2.0. long int iGain. ////////////////////////////////// TUNING VARIABLE DECLARATIONS ////////////////////////////////// unsigned char kp_port=0. unsigned char indx_middle.0.compen_reading[]={0. //speed boosting variables unsigned int m=0. unsigned int stopcnt=0.0.0}.0.0.0.flagw=3.0.1536.0. //speed scaling variables unsigned char MAXSPEED. prev_iTerm=0..0}.0}. kd_port=0. /***********************************************************************************/ 20 .0}.0.3.2560.0.5. unsigned char original_reading[]={0.0.3072}.0.1024.MINSPEED.0.0.j. dTerm=0.0.i.minspd_thresh=0.retracecnt=0.0}.0. ki_port=0.0.0.0.0.0. unsigned int current_pos.512.0.0. int kclamp=1.0. const int pos_data[]={0. // put actual_values * 10000 long int pTerm=0.0.

// Setting Port T pin 4. } //END OF MAIN. // First turning ON all LEDs for perfect toggling. // Turning off all LEDs. /***********************************************************************************/ ////////////////////////////////////////////// MAIN FUNCTION ///////////////////////////////////////////// void main(void) { // SETTING PORT B AS INPUT PORT. void UpdatePID(void). void pwm_speed_init(void).6.7 as output for LED. void ATD_init(void). pwm_steer_init(). ATD_init(). // Sensor Calibration. DDRT=0xF0. DDRB = 0x00. // Setting gain and speed.5. void compensation_fun(void). void steer_regulate(void). void brakecar(void).unsigned char *y). void speed_regulate(void). void Readvalues(void). // Car Starts and Runs on Track. Tuning(). Readvalues(). void pwm_steer_init(void). // Calling steering PWM_Initialization. void swap(unsigned char *x. void ATD_getvalue(unsigned char *temp_variable[]). Startrace(). PTT=0x0F. void Startrace(void)./////////////////////////////////////// FUNCTION PROTOTYPES //////////////////////////////////////////// void Tuning(void). PTT=0xFF. void bubblesort(void). ************************************************************************************/ 21 . void Delay(unsigned char).

} } ATD_getvalue(&black). //PTP_PTP7=1. PTP_PTP5=1.PERP_PERP7=1. for(i=0. while(flagw) { if(PTP_PTP7==0) { flagw--... PERP_PERP5=1.i<7. // for reading black values sw1. } } //End of Readvalues(). /***********************************************************************************/ //Port P Pin 5 set to input //Port P Pin 5 Pullup Enable 22 .DDRP_DDRP7=0.i++) { compen_ratio[i]=25600/(black[i]-white[i]).///////////////////// FUNCTION FOR READING INITIAL BLACK AND WHITE VALUES //////////////////////// void Readvalues() { flagb=3. DDRP_DDRP5=0...flagw=3. //LED 1 On // for reading white values sw2. } } ATD_getvalue(&white).. //LED 2 On // compensation ratio=256/(black-white) * 100 multiplied with 100 for fixed point calculation. PTT_PTT5=0. PTT_PTT4=0. while(flagb) { if(PTP_PTP5==0) { flagb--..

. // Speed regulate. /***********************************************************************************/ ////////////////////////////////// READING VALUES FROM SENSORS ////////////////////////////////// void ATD_getvalue(unsigned char *temp_variable) { ATD0CTL2_ADPU=1.) { for(i=0.. temp_variable[2]=ATD0DR2H.. compensation_fun(). ATD0CTL5=0x10. pwm_speed_init(). //loop until COMPLETE SEQUENCE FLAG is set...///////////////////////////////////////////// START CAR ///////////////////////////////////////////////// void Startrace() { unsigned int flag=1.i++) index[i]=i. if(PTP_PTP5==0) flag=0.. // ATD Enable ATD0CTL2_ASCIE=1. temp_variable[1]=ATD0DR1H. speed_regulate(). ATD_getvalue(&original_reading). // Servo regulate. } //end of for } //end of startrace(). while(flag) { PTT=~PTT.Delay(15). } PTT=0xFF.i<7. steer_regulate(). // Toggling ON all LEDs // Turning OFF all LEDs // INITIALLY START THE CAR.. bubblesort(). for(. // multisequence RE-TRIGGERING ATD. // THIS FLAG IS FOR PRESSING SWITCH.. 23 . temp_variable[0]=ATD0DR0H. else flag=1.. while(!(ATD0CTL2_ASCIF)).

i<7. } } /***********************************************************************************/ ////////////////////////////////////////// BUBBLE SORT FUNCTION //////////////////////////////////////// void bubblesort() { // bubblesort for all seven (based on value). temp_variable[5]=ATD0DR5H..White) * Compensation Ratio */ for (i=0.i++) for(j=0.j<(7-(i+1)).white[i]) * compen_ratio[i])/100.i++) { if (original_reading[i]>black[i]) original_reading[i]=black[i].i++) { compen_reading[i]=((original_reading[i]. } //////////////////////////////////////////////////////////// // CALCULATING COMPENSATED READING. value[i]=compen_reading[i].temp_variable[3]=ATD0DR3H. for(i=0.i<7.j++) { if(value[j] > value[j+1]) { 24 . else if(original_reading[i]<white[i]) original_reading[i]=white[i]. // DIVIDED BY 100 TO COMPENSATE FOR FIXED POINT REP. temp_variable[6]=ATD0DR6H..i<(7-1).. } /***********************************************************************************/ ///////////////////////////////// COMPENSATED READING CALCULATION ///////////////////////////////// void compensation_fun() { for (i=0. /* Compensated Reading = (Reading . temp_variable[4]=ATD0DR4H.

} } //achieved line data.unsigned char *y) { char temp. small_indx_read=three_value[0]. temp = *x.. for(i=4.. } // since last three values hold highest values.swap(&value[j].&index[j+1]).&three_index[j+1]). swap(&index[j].Smallest index sensor reading) */ } /***********************************************************************************/ ///////////////////////////////////// SWAP FUNCTION FOR SORTING ///////////////////////////////////// void swap(unsigned char *x.j<(3-(i+1)). //bubblesort for first three values (based on index)..i<(3-1). for(i=0.i<7.&three_value[j+1])..&value[j+1]). } } //filtering top three values only.i++) { three_value[i-4]=value[i].i++) for(j=0. larg_indx_read=three_value[2]. current_pos = (indx_middle*2*256) + (larg_indx_read-small_indx_read). *y = temp.. three_index[i-4]=index[i].j++) { if(three_index[j] > three_index[j+1]) { swap(&three_index[j]. } /***********************************************************************************/ 25 . /* current position = Index of Middle sensor *2* ATD resolution(8bit=256) + (Larest index sensor reading . // Calculating current position indx_middle=three_index[1]. *x = *y. swap(&three_value[j].

} } 26 // PWMDTY23 = 1415+((kp * ERROR)/10000). UpdatePID(). PWMDTY23 = 1415-pid_value..current_pos. for(i=8.POS_CENTRAL_SENSOR. // STEER LEFT UpdatePID().i--) { pre_his[i-1]=pre_his[i-2]. // STEER RIGHT . } } /**********************************************************************************/ 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 . /10000) // PWMDTY23 = 1415-((kp * ERROR)/10000). PWMDTY23 = 1415+pid_value. } else if (current_pos>POS_CENTRAL_SENSOR ) { ERROR = current_pos . /****************************** 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..cptemp=0.i>1./////////////////////////////////////////// STEERING CONTROL //////////////////////////////////////////// void steer_regulate() { int i=0. brakecar(). // SETTING STEER STRAIGHT FINALLY. } pre_his[0]=current_pos.

// boosting when out of track } } } } //end of steer() /***********************************************************************************/ 27 . speedcnt++. if (speedcnt<bstcnt_thresh) { PWMDTY0 = MINSPEED . } 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. 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). } // WAIT UNTIL RETRACES FOR MAINTAINING THE PREVIOUS STATE./**************************** 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 { PWMDTY0 = BOOSTSPEED. compensation_fun().

} /**********************************************************/ /**********************************************************/ if(PWMDTY0<=27) minspd_cnt++.////////////////////////////////////////////// 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++. // boosting at corner after 50 sample delay. // speed_regulate=m*x=((MAXSPEED-MINSPEED) * pid_value)/300.(m*x). PWMDTY0 = MAXSPEED . } ///////////////////////////////////////////////////////////////// if (speedcnt<bstcnt_thresh) { x=pid_value/30. ///////////////////////////////////////////////////////////////// if(minspd_cnt>minspd_thresh) PWMDTY0 = MAXSPEED -1. } else { PWMDTY0 = BOOSTSPEED. else minspd_cnt=0. } else { speedcnt=0. /*******************************************************************/ } /***********************************************************************************/ 28 .

// DIVIDED BY TEN TO COMPENSATE FOR PRECISION. dState = current_pos. pTerm = pGain * ERROR. iTerm = i_subterm + prev_iTerm. } /***********************************************************************************/ //////////////////////////////////////// INITIALIZATION FUNCTIONS /////////////////////////////////////// void ATD_init() { ATD0CTL1=0x87. ATD0CTL3_S8C=1. if(pid_value<300) kclamp = 1. ATD0CTL2_ADPU=1. else kclamp = 0. ATD0CTL5=0x10. ATD0CTL4_SRES8=1. } // 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 ./*/////////////////////////////////////////////////////////////////////////////////////////////////////////// PID FUNCTION ///////////////////////////////////////////////////////////////////////////////////////////////////////////*/ void UpdatePID() { int i_subterm=0. ATD0CTL2_ETRIGE=0. prev_iTerm = iTerm. pid_value = (pTerm + iTerm . dTerm = (dGain * (current_pos .dTerm)/10000. // calculate the proportional term i_subterm = (iGain * ERROR * kclamp)/100.dState)/10).

// clock SB as clock source for PWM PWMPRCLK =0x00. // PWM Period 20ms 50Hz PWMDTY23 = 1415. PWMPRCLK =0x00. PWME_PWME0=1./////////////////////////////// STEERING SERVO MOTOR INITIALIZATION ////////////////////////////////// void pwm_steer_init(void) { PWMCTL_CON23=1. PWME_PWME1=1. PWMDTY1 = 0. //PWM channel 3 Enable //PWM pulse High at begining of Period PWMPOL_PPOL2=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++. //H-bridge enable /*//////////////////// SPEED MOTOR CONTROL ///////////////////*/ PWMPOL_PPOL0=1. //clock A = 2MHz clockB = 2MHz PWMSCLB =1. PORTE_PE2=0. PWMSCLA =5. //PWM pulse High at begining of Period PWMCLK_PCLK3=1. //clock SB = clock B / (2 * 100) = 10KHz PWMPER23 = 20000. // SETTING STEER STRAIGHT INITIALLY. PWMPOL_PPOL1=1. PWMPER0 = 50. PWMPOL_PPOL3=1. // Port E pin 2 & 3 set to output DDRE_DDRE3=1. } /***********************************************************************************/ /////////////////////////////////////// DC MOTOR INITIALIZATION //////////////////////////////////////// void pwm_speed_init(void) { /*//////////////////////// H-BRIDGE //////////////////////////*/ DDRE_DDRE2=1. PORTE_PE3=1. PWMDTY0 = MAXSPEED. 30 . //PWM channel 2 Enable PWME_PWME3=1. PWME_PWME2=1.

} Delay(20).. PWME_PWME0=0. } /***********************************************************************************/ 31 . //TURNING ON LED 2. PWME_PWME3=0. //STEER MOTOR FRONT /*///////////////////// DISABLING CAR ///////////////////////*/ for(. //TURNING ON LED 1. // SETTING STEER STRAIGHT FINALLY.. case 2: PTT=0xCF. //TURNING ON LED 3.switch(stopcnt) { case 1: PTT=0xEF. PORTE_PE3=0... //disable H-BRIDGE //disable PWM Channels //SPEED MOTOR BACK PWME_PWME2=0. PWMDTY1 =60. /*//////////////////// BRAKE CONTROL MODULE ////////////////////*/ /* Setting motor in reverse direction for counteracting inertia after finish */ PWMDTY1 =100. Delay(2).) { PWMDTY23 = 1415.break..break. // SETTING STEER STRAIGHT FINALLY. Delay(2). PWME_PWME1=0.. PWMDTY23 = 1415... /*///////////////////// DISABLING MOTOR /////////////////////////////////////*/ PORTE_PE2=1. } //END OF RACE. //for preventing two counts on the start pad at same time itself.. PWMDTY1 =80. case 3: PTT=0x8F. Delay(3). PWMDTY1 =PWMDTY0.

case 0x01: dGain=16.j++) //Delay DONT CHANGE ANY VALUE IN THIS FUNCTION.i++). break. break. case 0x02: iGain=3. // PORT B PIN 0 AND 1 FOR pGain.//////////////////////////////////////////////// Delay function ///////////////////////////////////////////// void Delay(unsigned char a) { unsigned int i. break. kd_port= PORTB & 0x30. for(j=1. break. } switch (kd_port) { case 0x00: dGain=15. break.j<=a. } switch (ki_port) { case 0x00: iGain=1. case 0x01: iGain=2. switch (kp_port) { case 0x00: pGain=1462. for(i=0. break. break. break. case 0x03: pGain=1458. case 0x02: dGain=17. case 0x02: pGain=1460. break.. } /***********************************************************************************/ /////////////////////////////////////// GAIN AND SPEED SETTINGS ///////////////////////////////////////// void Tuning() { /////////////////////////////// TUNE HERE INITIALLY USING JUMBER PORT-B ///////////////////////////// kp_port= PORTB & 0x03. break. case 0x03: dGain=18. } switch (MAXSPEED_port) { case 0x00: MAXSPEED=32. // PORT B PIN 2 AND 3 FOR iGain. break. break. break. MAXSPEED_port= PORTB & 0x40. // PORT B PIN 6 FOR MAXSPEED. case 0x03: iGain=4.j. case 0x01: pGain=1461.. case 0x40: MAXSPEED=33. // PORT B PIN 4 AND 5 FOR dGain. ki_port= PORTB & 0x0c. // PORT B PIN 7 FOR MINSPEED. break. } 32 .i<=500. MINSPEED_port= PORTB & 0x80.

. case 0x80: MINSPEED=26. } /***********************************************************************************/ 33 . break. m=(MAXSPEED-MINSPEED)/10. //SPEED REGULATION RATIO BASED ON MAXSPEED & MINSPEED. minspd_thresh=500.. break. bstcnt_thresh=250.switch (MINSPEED_port) { case 0x00: MINSPEED=25. } BOOSTSPEED=34.

Sign up to vote on this title
UsefulNot useful