P. 1
Turbo Kat Racers

Turbo Kat Racers

|Views: 35|Likes:
Published by Madhusudhanan Ravi

More info:

Published by: Madhusudhanan Ravi on Dec 13, 2011
Copyright:Attribution Non-commercial


Read on Scribd mobile: iPhone, iPad and Android.
download as PDF, TXT or read online from Scribd
See more
See less












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



3 SENSOR The Sensor board consists of Infrared emitting diode and an NPN silicon phototransistor mounted side by side. 4 .2 POWER SUPPLY The power for the car is supplied by a rechargeable battery.1 HARDWARE INTERFACE BLOCK 2. which can supply a voltage of 7.2V and current of 2A. 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. STUDY OF HARDWARE MODULES 2. The board is placed in the front of the car which could cover a range of 8cm. The battery used here is a NI-CD battery. 2.2.

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

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

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

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


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

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

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

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

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

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

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

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

This warning does not require any action. Some variables may be missing declaration.39% 18 . ERRORS FOUND DURING DEBUGGING: 1. of Backslashes SOLUTION Go to the line of error and put the missing symbol. 6. 3. A variable’s value the may go out of bound. Wrong port assignments.2 DEBUGGING PROCESS Below is the listing of some of the common errors displayed by the compiler. Missing port initializations. Value given is not same as the type of variable. 5. PROCESSOR UTILIZATION FACTOR: U = 70/330 + 50/330 + 150/330 + 50/330 + 8/330 = 328/330 U = 99. ERROR/WARNING Symbol missing Possible loss of data Name not declared Recursive comments DESCRIPTION Usually symbols such as semi-colon or parenthesis are missed. KEY TECHNICAL PARAMETERS OF SMART CAR A.Debugger: It is used to download the object file into target and to debug the program to eliminate errors. The declaration for the missing variable is given. 2.3ms B. Comments having a no. SAMPLING TIME OF THE CAR : 3. The compiler lists only the syntax errors and the logical errors are found only during the debugging process. Analyze the variable and change type if required.

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

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

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

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

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

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

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

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

compensation_fun(). 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. } else { PWMDTY0 = BOOSTSPEED. // boosting when out of track } } } } //end of steer() /***********************************************************************************/ 27 . 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.

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

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

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

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

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

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

You're Reading a Free Preview

/*********** DO NOT ALTER ANYTHING BELOW THIS LINE ! ************/ var s_code=s.t();if(s_code)document.write(s_code)//-->