turbo kat racers
TRANSCRIPT
1
20/09/2010
FREESCALE SMART CAR RACE FREESCALE SMART CAR RACE FREESCALE SMART CAR RACE FREESCALE SMART CAR RACE
INDIA 2010INDIA 2010INDIA 2010INDIA 2010
TECHNICAL REPORT
TURBO KAT RACERSTURBO KAT RACERSTURBO KAT RACERSTURBO KAT RACERS
S.SIVAPRAKASH R.MADHUSUDHANAN
2
INDEX
1. INTRODUCTION 3
2. STUDY OF HARDWARE 4
2.1 HARDWARE INTERFACE BLOCK 4
2.2 POWER SUPPLY 4
2.3 SENSOR 4
2.4 SERVO MOTOR 5
2.5 DC MOTOR 6
3. SOFTWARE DESIGN 7
3.1 TABLE OF FUNCTIONS 7
3.2 TABLE OF VARIABLES 8
3.3 PROGRAM FLOWCHART 9
3.4 PROGRAM DESCRIPTION 10
3.4.A SENSOR CALIBRATION 10
3.4.B CALCULATION OF COMPENSATED READING 10
3.4.C CALCULATION OF CURRENT POSITION & ERROR 10
3.4.D SPEED FUNCTION 11
3.4.E STEER FUNCTION 12
4. CONTROL SYSTEM DESIGN 14
4.1 OVERVIEW OF PID CONTROLLER 14
4.2 SMART CAR CONTROL SYSTEM DESIGN 15
5. DEVELOPMENT TOOLS & DEBUG PROCESS 17
5.1 TOOLS USED 17
5.2 DEBUGGING PROCESS 18
6. KEY TECHNICAL PARAMETERS OF SMART CAR 18
7. BIBLIOGRAPHY 19
APPENDIX A: SMART CAR PROGRAM 20
3
1. INTRODUCTION
The Freescale Smart Car Race India 2010 is conducted by Freescale
Semiconductors to enhance the creativity and technical knowledge and to
familiarize the students with the Freescale S12X microcontroller among the
Indian Students.
This Competition gives us an opportunity to practice the programming
skills we learnt and the knowledge of the Microcontroller and programming
tools. It also provides us a good platform to learn the control system concepts
and programming concepts practically. At the same time, it also gives us a
chance to improve our Team Working and Self Learning skills.
The following Technical Report includes our work on developing a
Software design for Smart Car which could run on any given track
autonomously. It covers the study of hardware, provided by the Freescale
Semiconductors; Complete software design and Control system design.
The Competition involves the race between all Smart Cars, where it
should run on the track, given that day, for 2 LAPS, without being running out
of the track or stopping in between.
The rules of the competition are simple.
Be FAST!!! Be STEADY!!!
4
2. STUDY OF HARDWARE MODULES
2.1 HARDWARE INTERFACE BLOCK
2.2 POWER SUPPLY
The power for the car is supplied by a rechargeable battery. The battery
used here is a NI-CD battery, which can supply a voltage of 7.2V and current of
2A.
2.3 SENSOR
The Sensor board consists of Infrared emitting diode and an NPN silicon
phototransistor mounted side by side. The board is placed in the front of the car
which could cover a range of 8cm. The phototransistor responds to the radiation
emitted from the diode and the current produced is sent for processing.
The current produced varies depending on the reflecting surface colour and
it is highest for Black colour and lowest for White colour.
5
SENSOR COUNT : 7
SENSOR TYPE : SFH4550 (IR EMITTER) & SFH314 (PHOTO-TRANSISTOR)
ALIGNMENT : STRAIGHT
SENSOR PIN CONNECTION
2.4 SERVO MOTOR
SERVO MOTOR �STEER CONTROL
The servo motor is used for controlling the DIRECTION in which the car
moves. Only one servo motor, provided by Freescale is used. The servo expects a
pulse every 20 ms from PWM, in order to gain correct information about the
angle. Based on the width of the PWM signal, the angular motion of the SERVO
varies.
SERVO CONNECTION
SIGNAL PIN
Servo Motor 1 Port P Pin 2 / PWM2
Servo Motor 2 Port P Pin 3 / PWM3
SERVO CONTROL SIGNAL
The table shows the different pulse widths required to set the SERVO in
different positions.
POSITION PULSE WIDTH
RIGHT 1.715ms
CENTRE 1.415ms
LEFT 1.115ms
� SERVO TYPE - Futaba S3010
SENSOR PIN
SENSOR 0 -6 PORT AD PIN 0-6
6
2.5 DC MOTOR
The Car runs with the DC Motor. The speed of the car is controlled by
controlling the DC motor. The current in the DC motor is set by PWM signal which
drives the car at the desired speed.
The DC motor is interfaced with the PWM Module through the H- Bridge.
The H-Bridge IC runs the DC motor according to the PWM signal.
DC MOTOR CONNECTION
The table shows the DC motor control Signals and the direction in which
the Car runs with respect to that signal.
SIGNAL PIN DIRECTION
DC Motor Signal 1 Port P Pin 0 / PWM2 FORWARD
DC Motor Signal 2 Port P Pin 1 / PWM3 REVERSE
� DC MOTOR TYPE - RS389-ST/3545
DC MOTOR � SPEED CONTROL
7
3. SOFTWARE DESIGN
3.1 TABLE OF FUNCTIONS
FUNCTION PARAMETERS DESCRIPTION
ATD_init() - Initializing the port AD to read the
sensor values.
pwm_steer_init() -
Initializing the PWM pins for servo
motor and setting the frequency and
initial direction.
pwm_speed_init() -
Initializing the PWM pins for DC
motor and setting the frequency and
initial speed.
Readvalues() - Getting the initial Sensor readings for
Calibration.
ATD_getvalue() *char Storing Black and White values for
Calibrating the sensors.
compensation_fun() - Original readings are taken and
Compensation readings are found.
bubblesort() - Compensated readings are sorted
based on values & index.
Startrace() - Once the calibrations are over, Car
enters this infinite loop.
steer_regulate() - Sets the direction of the Car in every
loop.
speed_regulate() - Sets the speed of the Car in every
loop.
brakecar() - Used to have count on the laps and
stop the car.
swap() *int,*int Used during Bubble sorting of the
sensor readings for Error Calculation.
UpdatePID() -
A PID controller which takes in the
Error and gives the correction signal
to set the new direction.
Delay() int Used to set delay based on the input.
Tuning() - Used to set the gains and speeds.
8
3.2 TABLE OF VARIABLES
VARIABLE TYPE DESCRIPTION
POS_CENTRAL_SENSOR
1536 #define
1536 is the position of the car when
the car is at the centre of black line.
LEFT, STRAIGHT, RIGHT #define The value of ms to be given to set the
car in respective directions.
black[], white[] unsigned char Used to store the black and white
reading of each sensor for Calibration.
pos_data[] const int Used to store position data for Error
Calculation.
compen_ratio char Used to store the sensor’s
compensation ratio.
ERROR signed int Used to store Error value of Car’s
position.
current_pos int Holds the current position in which
the car runs.
pre_his[] int The previous positions of the car are
stored, used for retracing of car.
Stopcnt int The no. of laps the car has run is
stored, used for braking the car.
speedcnt, spdcnt_thresh int To count the no. of samples in retrace
and boost the speed of the car
MAXSPEED, MINSPEED char The maximum and minimum speed
for the car is stored here.
PID VARIABLES
dstate, prev_iTerm int Used to hold the previous derivative
and integral part values.
Kclamp int The clamping gain for the PID
controller.
iGain, pGain, dGain, int The gains for the PID controller are
stored here.
pTerm, dTerm, iTerm, int The Proportional, Derivative and
Integral correction terms are stored.
pid_value int Used to store the Overall Correction
Value for the direction, in every loop.
9
3.3 PROGRAM FLOWCHART
START
GAIN AND SPEED
SETTING
PORT
INITIALIZATIONS
SENSOR
CALIBRATION
If Switch 1
Pressed
No
Yes
READ SENSOR
VALUES & FIND
LINE POSITION
SET PWM FOR
STEERING SERVO
(CALL PID)
SET PWM FOR
DRIVING MOTOR
IF NO. OF
LAPS = 2
No
END
Yes
10
3.4 PROGRAM DESCRIPTION
In this part the important strategies of the Smart Car Program are
explained.
3.4.A SENSOR CALIBRATION
For calibrating the sensors to overcome the practical errors and to
get the extreme values for white and black values, the following steps are used.
Initially the sensor is read for the Black and White values. The original
reading is then fed to the below formula to get the COMPENSATION RATIO for
each sensor.
3.4.B CALCULATION OF COMPENSATED READING
In every loop of the car’s run the compensated reading of the sensors
is calculated as follows.
The original reading of each sensor from the ATD is taken and after
some process the compensated reading is achieved. Below is the function that
converts the original reading into required reading.
3.4.C CALCULATION OF CURRENT POSITION & ERROR
Once the compensated readings are found, the top three sensor
readings and their indexes are bubble sorted.
Current position = Index of Middle sensor *2* ATD resolution (8bit=256) +
(Largest index sensor reading - Smallest index sensor reading)
Compensation Ratio = 256/(Black - White)
Compensated Reading = (Original Reading - White) * Compensation Ratio
11
The current position of the car, based on the sensors below which
the black line of track lies, is found as above.
Now it is important to find the sensor which is above centre of the
black line. This helps in finding the amount of drive required to keep the car in the
centre of the Black line. As there are seven sensors equally spaced, it can be seen
that the car is running exactly at the centre, if the fourth sensor is on the middle of
black line.
POSITION DATA TABLE
Sensor 6 5 4 3 2 1 0
Position 3072 2560 2048 1536 1024 512 0
The above table shows the position data which shows the values the
current position variable would read when the corresponding sensor stays on top
of middle of Black line on the Track.
Looking from the above table, it can be seen that the current position
of the car would be 1536 if the car is running exactly on the centre of the track.
Now the Error, i.e. the difference between the current position and
1536(values of current pos. for car’s run on middle of track) can be found by
finding the difference between the current position and 1536.
ERROR = |CURRENT POSITION – 1536|
3.4.D SPEED FUNCTION
Based on the value of the steer correction signal a speed correction signal is
produced which is subtracted from Maximum speed applied to the Car. Eventually
when the steer is at its extreme the speed is at its lowest.
The formula below gives the relation between Steer and Speed control
signal.
12
SPEED CORRECTION = ((MAXSPEED - MINSPEED)* STEER CONTROL)/290
A special speed function has been applied for conditions requiring some
boost up in the speed. Whenever the car has maximum turn the normal speed is
not enough to move the car, thus more speed (BOOSTSPEED) is applied to move
it quickly without any sluggishness. The boost up is also done during the retracing
run of the car.
3.4.E STEER FUNCTION
The Steer Function of the Smart Car program can be explained
through the below Flowchart.
START
IF START
LINE FOUND
BRAKE CAR ()
IF CAR ON
LINE
SET PWM FOR
SERVO (CALL PID) RETRACE TRACK
Yes
END
Yes
No
No
13
As the above flowchart shows the steer function starts with the checking
for start line. Once the start line is detected at the end of 2 Laps the brake is
applied and the car is stopped. A dynamic braking function is used to stop the car
as quick as possible after the finish. If not, based on the current position of the
car, the correction value, as achieved from the PID function, is applied to drive the
car to the required position.
A special strategy is applied to bring the car back on track if it runs out of
the track.
Once the correction signal is achieved the value is added or subtracted
from the STRAIGHT value to steer LEFT or RIGHT.
RETRACING ALGORITHM
In order to bring the car back in track, whenever it goes out of the line, this
special algorithm is applied. According to this algorithm the positions of the car
are saved in array. At any instant up to eight previous positions are available.
Based on these previous position values the car is driven accordingly to bring it
back on track.
PRE HISTORY DRIVE
Positions indicating right turn RIGHT
Positions indicating left turn LEFT
14
4. CONTROL SYSTEM DESIGN
The Core of the control system used in the program is a PID Controller. A
PID controller is a closed loop robust controller.
4.1 OVERVIEW OF PID CONTROLLER
What is PID..?
A proportional–integral–derivative controller (PID controller) is a generic
control loop feedback mechanism (controller) widely used in industrial control
systems – a PID is the most commonly used feedback controller. A PID controller
calculates an "error" value as the difference between a measured process variable
and a desired Setpoint. The controller attempts to minimize the error by adjusting
the process control inputs. In the absence of knowledge of the underlying
process, a PID controller is the best controller.
THE PID ALGORITHM
The PID controller calculation (algorithm) involves three separate
parameters, and is accordingly sometimes called three-term control: the
proportional, the integral and derivative values, denoted P, I, and D. The
proportional value determines the reaction to the current error, the integral value
determines the reaction based on the sum of recent errors, and the derivative
value determines the reaction based on the rate at which the error has been
changing. The weighted sum of these three actions is used to adjust the process
via a control element such as the position of a control valve or the power supply
of a heating element.
Heuristically, these values can be interpreted in terms of time: P depends
on the present error, I on the accumulation of past errors, and D is a prediction of
future errors, based on current rate of change.
15
EVALUATION OF CONTROLLER RESPONSE
The response of the controller can be evaluated in terms of the factors
given below.
a) The responsiveness of the controller towards an error.
b) The degree to which the controller overshoots the setpoint.
c) The degree of system oscillation.
It is based on the above three factors the Proportional gain, Integral gain
and Differential gains are tuned respectively.
4.2 SMART CAR CONTROL SYSTEM DESIGN
The control system design of the Smart Car is represented as a block
diagram below.
PID CONTROLLER BLOCK DIAGRAM
16
CONTROL SYSTEM WORKING
In each sample, once the Error is calculated the controller is called. The
controller takes in the error and produces the three terms after some process.
The three terms are then summed up to produce the correction value. The
correction value is then applied to the steer the car in the required direction. The
process of achieving the three controller terms are given below.
PROPORTIONAL TERM:
The proportional term is achieved by scaling the error in terms of the
Proportional gain (Kp).
INTEGRAL TERM:
The Integral term is got by adding the product of Integral gain (Ki) and error
with the previous integral terms. The method is used here for integration is based
on the EULER’s FORWARD RULE. In order to prevent the Smart Car from Integral
windup problem an ANTI-WINDUP window is set up.
� ANTI – WINDUP
In order to establish the Anti windup, a gain namely Kclamp has been
introduced before the summing point of the integral arm of the
controller. The control signal is checked every time and whenever it is out
of bound the integral term is cut off by making the Kclamp zero. In all
other conditions the Kclamp is held one.
DIFFERENTIAL TERM:
For finding the differential term the difference between the present and
previous positions of the car is multiplied with the differential gain (Kd).
17
CONTROLLER GAIN TUNING
The controller gains are tuned to various values in order to provide a good
run for the car. The gains are tuned by trial and error method. The steps followed
in the tuning of the gains are given below.
1) The Proportional gain is set to critical gain with other two set to zero.
2) The Differential gain is set to a high value and then reduced until the
oscillations are suppressed.
3) The Proportional is now again set to a higher value and decreased to see no
oscillation.
4) Finally the Integral gain is increased starting from a lower value, until the
overshoot point.
5. DEVELOPMENT TOOLS & DEBUG PROCESS
The only development tool used in this project is CODE WARRIOR IDE V5.1.
Since only the software design was required no other development tool were
used.
5.1 TOOLS USED
Code warrior IDE contains many tools integrated which are used for
programming the smart car. The tools used are given below
� Source Code Editor: Text editor program created to write and edit
the source code.
� Compiler: Program that converts the source code in C language to
computer language.
� Linker: It associates all the required files and produces an
absolute file (.ABS).
� Burner: The program converts the .ABS file into an S-Record file.
18
� Debugger: It is used to download the object file into target and to
debug the program to eliminate errors.
5.2 DEBUGGING PROCESS
Below is the listing of some of the common errors displayed by the
compiler. The compiler lists only the syntax errors and the logical errors are found
only during the debugging process.
ERROR/WARNING DESCRIPTION SOLUTION
Symbol missing
Usually symbols such as
semi-colon or parenthesis
are missed.
Go to the line of error
and put the missing
symbol.
Possible loss of data A variable’s value the
may go out of bound.
Analyze the variable and
change type if required.
Name not declared Some variables may be
missing declaration.
The declaration for the
missing variable is given.
Recursive comments Comments having a no. of
Backslashes
This warning does not
require any action.
ERRORS FOUND DURING DEBUGGING:
1. Missing port initializations.
2. Wrong port assignments.
3. Value given is not same as the type of variable.
6. KEY TECHNICAL PARAMETERS OF SMART CAR
A. SAMPLING TIME OF THE CAR : 3.3ms
B. PROCESSOR UTILIZATION FACTOR:
U = 70/330 + 50/330 + 150/330 + 50/330 + 8/330 = 328/330
U = 99.39%
19
C. ACCURACY
a. TRACKING: 95%
b. SPEED: 95%
D. SOFTWARE USED: CODE WARRIOR IDE
E. DIMENSIONS: 31cm X 16 cm
7. BIBLIOGRAPHY
A. MC9S12XDT256 Micro controller User Manual.
B. PID Tutorial (http://www.engin.umich.edu/group/ctm/PID/PID.html).
C. PID Without a PhD, Tim Wescott, FLIR Systems.
D. Freescale Semiconductor Smart Car Race Reference Documents.
a. Mexico Car Race Guidelines.
b. Mexico Car Race Rules.
c. Mexico Car Race Requirements.
20
APPENDIX A: SMART CAR PROGRAM
#include <hidef.h> /* common defines and macros */
#include "derivative.h" /* derivative-specific definitions */
#define POS_CENTRAL_SENSOR 1536
#define LEFT 1115
#define STRAIGHT 1415
#define RIGHT 1715
///////////////////////////////////////// VARIABLE DECLARATIONS ////////////////////////////////////////
unsigned char black[]={0,0,0,0,0,0,0},white[]={0,0,0,0,0,0,0};
const int pos_data[]={0,512,1024,1536,2048,2560,3072};
unsigned char compen_ratio[]={0,0,0,0,0,0,0};
unsigned char index[]={0,1,2,3,4,5,6};
signed long int ERROR=0;
unsigned char original_reading[]={0,0,0,0,0,0,0},compen_reading[]={0,0,0,0,0,0,0},i,j;
unsigned char value[]={0,0,0,0,0,0,0};
unsigned char three_value[]={0,0,0},three_index[]={0,0,0};
unsigned char indx_middle, larg_indx_read, small_indx_read;
unsigned int current_pos;
unsigned int pre_his[]={0,0,0,0,0,0,0,0,0,0};
unsigned int stopcnt=0,speedcnt=0,retracecnt=0,bstcnt_thresh=0,minspd_cnt=0,minspd_thresh=0;
//speed boosting variables
unsigned int m=0,x=0; //speed scaling variables
unsigned char MAXSPEED,MINSPEED,BOOSTSPEED;
static char flagb=3,flagw=3; // 3 is assigned for reading ATD values by pressing switch correctly...
////////////////////////////////// TUNING VARIABLE DECLARATIONS //////////////////////////////////
unsigned char kp_port=0, ki_port=0, kd_port=0, MAXSPEED_port=0, MINSPEED_port=0;
///////////////////////////////////////// PID VARIABLES ///////////////////////////////////////////////////
signed int dState=0;
long int iGain, pGain, dGain; // put actual_values * 10000
long int pTerm=0, dTerm=0, iTerm=0, prev_iTerm=0;
int kclamp=1;
int pid_value=0;
/***********************************************************************************/
21
/////////////////////////////////////// FUNCTION PROTOTYPES ////////////////////////////////////////////
void Tuning(void);
void ATD_init(void);
void pwm_steer_init(void);
void pwm_speed_init(void);
void ATD_getvalue(unsigned char *temp_variable[]);
void Readvalues(void);
void compensation_fun(void);
void bubblesort(void);
void swap(unsigned char *x,unsigned char *y);
void Startrace(void);
void steer_regulate(void);
void speed_regulate(void);
void UpdatePID(void);
void brakecar(void);
void Delay(unsigned char);
/***********************************************************************************/
////////////////////////////////////////////// MAIN FUNCTION /////////////////////////////////////////////
void main(void)
{
DDRB = 0x00; // SETTING PORT B AS INPUT PORT.
DDRT=0xF0; // Setting Port T pin 4,5,6,7 as output for LED.
PTT=0xFF; // Turning off all LEDs.
Tuning(); // Setting gain and speed.
ATD_init();
pwm_steer_init(); // Calling steering PWM_Initialization.
Readvalues(); // Sensor Calibration.
PTT=0x0F; // First turning ON all LEDs for perfect toggling.
Startrace(); // Car Starts and Runs on Track.
} //END OF MAIN.
************************************************************************************/
22
///////////////////// FUNCTION FOR READING INITIAL BLACK AND WHITE VALUES ////////////////////////
void Readvalues()
{
flagb=3;flagw=3; PTP_PTP5=1; //PTP_PTP7=1;
DDRP_DDRP5=0;DDRP_DDRP7=0; //Port P Pin 5 set to input
PERP_PERP5=1;PERP_PERP7=1; //Port P Pin 5 Pullup Enable
// for reading black values sw1...
while(flagb)
{
if(PTP_PTP5==0)
{
flagb--;
}
}
ATD_getvalue(&black);
PTT_PTT4=0; //LED 1 On
// for reading white values sw2...
while(flagw)
{
if(PTP_PTP7==0)
{
flagw--;
}
}
ATD_getvalue(&white);
PTT_PTT5=0; //LED 2 On
// compensation ratio=256/(black-white) * 100 multiplied with 100 for fixed point calculation...
for(i=0;i<7;i++)
{
compen_ratio[i]=25600/(black[i]-white[i]);
}
} //End of Readvalues();
/***********************************************************************************/
23
///////////////////////////////////////////// START CAR /////////////////////////////////////////////////
void Startrace()
{
unsigned int flag=1; // THIS FLAG IS FOR PRESSING SWITCH...
while(flag)
{
PTT=~PTT;Delay(15); // Toggling ON all LEDs
if(PTP_PTP5==0)
flag=0;
else
flag=1;
}
PTT=0xFF; // Turning OFF all LEDs
pwm_speed_init(); // INITIALLY START THE CAR...
for(;;)
{
for(i=0;i<7;i++)
index[i]=i;
ATD_getvalue(&original_reading);
compensation_fun();
bubblesort();
steer_regulate(); // Servo regulate...
speed_regulate(); // Speed regulate...
} //end of for
} //end of startrace().
/***********************************************************************************/
////////////////////////////////// READING VALUES FROM SENSORS //////////////////////////////////
void ATD_getvalue(unsigned char *temp_variable)
{
ATD0CTL2_ADPU=1; // ATD Enable
ATD0CTL2_ASCIE=1;
ATD0CTL5=0x10; // multisequence RE-TRIGGERING ATD.
while(!(ATD0CTL2_ASCIF)); //loop until COMPLETE SEQUENCE FLAG is set.
temp_variable[0]=ATD0DR0H;
temp_variable[1]=ATD0DR1H;
temp_variable[2]=ATD0DR2H;
24
temp_variable[3]=ATD0DR3H;
temp_variable[4]=ATD0DR4H;
temp_variable[5]=ATD0DR5H;
temp_variable[6]=ATD0DR6H;
}
/***********************************************************************************/
///////////////////////////////// COMPENSATED READING CALCULATION /////////////////////////////////
void compensation_fun()
{
for (i=0;i<7;i++)
{
if (original_reading[i]>black[i])
original_reading[i]=black[i];
else if(original_reading[i]<white[i])
original_reading[i]=white[i];
}
////////////////////////////////////////////////////////////
// CALCULATING COMPENSATED READING....
/* Compensated Reading = (Reading - White) * Compensation Ratio */
for (i=0;i<7;i++)
{
compen_reading[i]=((original_reading[i]- white[i]) * compen_ratio[i])/100;
// DIVIDED BY 100 TO COMPENSATE FOR FIXED POINT REP.
value[i]=compen_reading[i];
}
}
/***********************************************************************************/
////////////////////////////////////////// BUBBLE SORT FUNCTION ////////////////////////////////////////
void bubblesort()
{
// bubblesort for all seven (based on value).
for(i=0;i<(7-1);i++)
for(j=0;j<(7-(i+1));j++)
{
if(value[j] > value[j+1])
{
25
swap(&value[j],&value[j+1]);
swap(&index[j],&index[j+1]);
}
}
//filtering top three values only....
for(i=4;i<7;i++)
{
three_value[i-4]=value[i]; // since last three values hold highest values.
three_index[i-4]=index[i];
}
//bubblesort for first three values (based on index).
for(i=0;i<(3-1);i++)
for(j=0;j<(3-(i+1));j++)
{
if(three_index[j] > three_index[j+1])
{
swap(&three_index[j],&three_index[j+1]);
swap(&three_value[j],&three_value[j+1]);
}
}
//achieved line data...
// Calculating current position
indx_middle=three_index[1];
larg_indx_read=three_value[2];
small_indx_read=three_value[0];
current_pos = (indx_middle*2*256) + (larg_indx_read-small_indx_read);
/* current position = Index of Middle sensor *2* ATD resolution(8bit=256) + (Larest index sensor
reading - Smallest index sensor reading) */
}
/***********************************************************************************/
///////////////////////////////////// SWAP FUNCTION FOR SORTING /////////////////////////////////////
void swap(unsigned char *x,unsigned char *y)
{
char temp;
temp = *x;
*x = *y;
*y = temp;
}
/***********************************************************************************/
26
/////////////////////////////////////////// STEERING CONTROL ////////////////////////////////////////////
void steer_regulate()
{
int i=0,cptemp=0;
/****************************** START PAD LOGIC *********************************/
if(((three_index[0]+1)!=three_index[1] || (three_index[1]+1)!=three_index[2]) &&
((compen_reading[0]>compen_reading[1]) && (compen_reading[6]>compen_reading[5]) &&
(compen_reading[3]>compen_reading[1]) && (compen_reading[3]>compen_reading[5])))
{
if(((compen_reading[0]-compen_reading[1])>=30) && ((compen_reading[6]-
compen_reading[5])>=30))
{
PWMDTY23 = 1415; // SETTING STEER STRAIGHT FINALLY...
brakecar();
}
}
/**********************************************************************************/
if (compen_reading[0]>=30 || compen_reading[1]>=30 || compen_reading[2]>=30 ||
compen_reading[3]>=30 || compen_reading[4]>=30 || compen_reading[5]>=30 ||
compen_reading[6]>=30)
{
if( current_pos<POS_CENTRAL_SENSOR)
{
ERROR = POS_CENTRAL_SENSOR - current_pos; // STEER LEFT
UpdatePID();
PWMDTY23 = 1415-pid_value; // PWMDTY23 = 1415-((kp * ERROR)/10000);
}
else if (current_pos>POS_CENTRAL_SENSOR )
{
ERROR = current_pos - POS_CENTRAL_SENSOR; // STEER RIGHT
UpdatePID();
PWMDTY23 = 1415+pid_value; // PWMDTY23 = 1415+((kp * ERROR)/10000); /10000)
}
pre_his[0]=current_pos;
for(i=8;i>1;i--)
{
pre_his[i-1]=pre_his[i-2];
}
}
27
/**************************** RETRACING ALGORITHM *****************************/
else if ((compen_reading[0]<30 && compen_reading[1]<30 && compen_reading[2]<30 &&
compen_reading[3]<30 && compen_reading[4]<30 && compen_reading[5]<30 &&
compen_reading[6]<30))
{
if(pre_his[0]>2580 || pre_his[1]>2580 || pre_his[2]>2580)
{
PWMDTY23=RIGHT;
}
else if((pre_his[0]<1000 || pre_his[1]<1000 || pre_his[2]<1000 || pre_his[3]<1000 ||
pre_his[4]<1000 || pre_his[5]<1000 || pre_his[6]<1000 || pre_his[7]<1000) || ((pre_his[0]>2490 &&
pre_his[0]<2565)) || ((pre_his[1]>2490 && pre_his[1]<2565) || (pre_his[2]>2490 && pre_his[2]<2565)))
{
PWMDTY23=LEFT;
}
// WAIT UNTIL RETRACES FOR MAINTAINING THE PREVIOUS STATE.
while(compen_reading[0]<30 && compen_reading[1]<30 && compen_reading[2]<30 &&
compen_reading[3]<30 && compen_reading[4]<30 && compen_reading[5]<30 &&
compen_reading[6]<30)
{
ATD_getvalue(&original_reading);
compensation_fun();
speedcnt++;
if (speedcnt<bstcnt_thresh)
{
PWMDTY0 = MINSPEED ;
}
else
{
PWMDTY0 = BOOSTSPEED; // boosting when out of track
}
}
}
} //end of steer()
/***********************************************************************************/
28
////////////////////////////////////////////// SPEED CONTROL /////////////////////////////////////////////
void speed_regulate(void)
{ /**********************************************************/
if ((compen_reading[0]>20 && compen_reading[2]<30 && compen_reading[3]<30 &&
compen_reading[4]<30 && compen_reading[5]<30 && compen_reading[6]<30) ||
(compen_reading[0]<30 && compen_reading[1]<30 && compen_reading[2]<30 &&
compen_reading[3]<30 && compen_reading[4]<30 && compen_reading[6]>20))
{
speedcnt++;
}
else
{
speedcnt=0;
}
/////////////////////////////////////////////////////////////////
if (speedcnt<bstcnt_thresh)
{
x=pid_value/30;
PWMDTY0 = MAXSPEED - (m*x); // speed_regulate=m*x=((MAXSPEED-MINSPEED) *
pid_value)/300;
}
else
{
PWMDTY0 = BOOSTSPEED; // boosting at corner after 50 sample delay.
}
/**********************************************************/
/**********************************************************/
if(PWMDTY0<=27)
minspd_cnt++;
else
minspd_cnt=0;
/////////////////////////////////////////////////////////////////
if(minspd_cnt>minspd_thresh)
PWMDTY0 = MAXSPEED -1;
/*******************************************************************/
}
/***********************************************************************************/
29
/*///////////////////////////////////////////////////////////////////////////////////////////////////////////
PID FUNCTION
///////////////////////////////////////////////////////////////////////////////////////////////////////////*/
void UpdatePID()
{
int i_subterm=0;
pTerm = pGain * ERROR; // calculate the proportional term
i_subterm = (iGain * ERROR * kclamp)/100; // DIVIDED BY TEN TO COMPENSATE FOR PRECISION.
iTerm = i_subterm + prev_iTerm;
prev_iTerm = iTerm; // calculate the integral term
dTerm = (dGain * (current_pos - dState)/10);
dState = current_pos; // calculate the differential term
pid_value = (pTerm + iTerm - dTerm)/10000;
if(pid_value<300)
kclamp = 1;
else
kclamp = 0;
}
/***********************************************************************************/
//////////////////////////////////////// INITIALIZATION FUNCTIONS ///////////////////////////////////////
void ATD_init()
{
ATD0CTL1=0x87; // no external trigger
ATD0CTL2_ADPU=1; // ATD Enable
ATD0CTL2_ETRIGE=0; // no external trigger
ATD0CTL3_S8C=1; // 8 adc channels sequence
ATD0CTL4_SRES8=1; // 8-bit Resolution
ATD0CTL5=0x10; // multisequence
}
30
/////////////////////////////// STEERING SERVO MOTOR INITIALIZATION //////////////////////////////////
void pwm_steer_init(void)
{
PWMCTL_CON23=1;
PWME_PWME2=1; //PWM channel 2 Enable
PWME_PWME3=1; //PWM channel 3 Enable
PWMPOL_PPOL2=1; //PWM pulse High at begining of Period
PWMPOL_PPOL3=1; //PWM pulse High at begining of Period
PWMCLK_PCLK3=1; // clock SB as clock source for PWM
PWMPRCLK =0x00; //clock A = 2MHz clockB = 2MHz
PWMSCLB =1; //clock SB = clock B / (2 * 100) = 10KHz
PWMPER23 = 20000; // PWM Period 20ms 50Hz
PWMDTY23 = 1415; // SETTING STEER STRAIGHT INITIALLY.
}
/***********************************************************************************/
/////////////////////////////////////// DC MOTOR INITIALIZATION ////////////////////////////////////////
void pwm_speed_init(void)
{
/*//////////////////////// H-BRIDGE //////////////////////////*/
DDRE_DDRE2=1; // Port E pin 2 & 3 set to output
DDRE_DDRE3=1;
PORTE_PE2=0; //H-bridge enable
PORTE_PE3=1;
/*//////////////////// SPEED MOTOR CONTROL ///////////////////*/
PWMPOL_PPOL0=1; //PWM pulse High at begining of Period
PWMPOL_PPOL1=1; //PWM pulse High at begining of Period
PWMPRCLK =0x00; //clock A = 2MHz clockB = 2MHz
PWMSCLA =5; //clock SA = clock A / (2 * 5) = 200KHz
PWMPER0 = 50; // PWM Period = 2KHz
PWMDTY0 = MAXSPEED;
PWMDTY1 = 0;
PWME_PWME0=1; //PWM channel enable
PWME_PWME1=1;
}
/***********************************************************************************/
////////////////////////////////// BRAKE CAR AFTER COMPLETING 2 LAPS /////////////////////////////////
void brakecar()
{
stopcnt++;
31
switch(stopcnt)
{
case 1:
PTT=0xEF;break; //TURNING ON LED 1.
case 2:
PTT=0xCF;break; //TURNING ON LED 2.
case 3:
PTT=0x8F; //TURNING ON LED 3.
PWMDTY23 = 1415; // SETTING STEER STRAIGHT FINALLY...
/*//////////////////// BRAKE CONTROL MODULE ////////////////////*/
/* Setting motor in reverse direction for counteracting inertia after finish */
PWMDTY1 =100;
Delay(3);
PWMDTY1 =80;
Delay(2);
PWMDTY1 =60;
Delay(2);
PWMDTY1 =PWMDTY0;
/*///////////////////// DISABLING MOTOR /////////////////////////////////////*/
PORTE_PE2=1; //disable H-BRIDGE
PORTE_PE3=0;
//disable PWM Channels
PWME_PWME0=0;
PWME_PWME1=0; //SPEED MOTOR BACK
PWME_PWME2=0;
PWME_PWME3=0; //STEER MOTOR FRONT
/*///////////////////// DISABLING CAR ///////////////////////*/
for(;;)
{
PWMDTY23 = 1415; // SETTING STEER STRAIGHT FINALLY...
} //END OF RACE...
}
Delay(20); //for preventing two counts on the start pad at same time itself...
}
/***********************************************************************************/
32
//////////////////////////////////////////////// Delay function /////////////////////////////////////////////
void Delay(unsigned char a)
{
unsigned int i,j;
for(j=1;j<=a;j++) //Delay DONT CHANGE ANY VALUE IN THIS FUNCTION...
for(i=0;i<=500;i++);
}
/***********************************************************************************/
/////////////////////////////////////// GAIN AND SPEED SETTINGS /////////////////////////////////////////
void Tuning()
{
/////////////////////////////// TUNE HERE INITIALLY USING JUMBER PORT-B /////////////////////////////
kp_port= PORTB & 0x03; // PORT B PIN 0 AND 1 FOR pGain.
ki_port= PORTB & 0x0c; // PORT B PIN 2 AND 3 FOR iGain.
kd_port= PORTB & 0x30; // PORT B PIN 4 AND 5 FOR dGain.
MAXSPEED_port= PORTB & 0x40; // PORT B PIN 6 FOR MAXSPEED.
MINSPEED_port= PORTB & 0x80; // PORT B PIN 7 FOR MINSPEED.
switch (kp_port)
{
case 0x00: pGain=1462; break;
case 0x01: pGain=1461; break;
case 0x02: pGain=1460; break;
case 0x03: pGain=1458; break;
}
switch (ki_port)
{
case 0x00: iGain=1; break;
case 0x01: iGain=2; break;
case 0x02: iGain=3; break;
case 0x03: iGain=4; break;
}
switch (kd_port)
{
case 0x00: dGain=15; break;
case 0x01: dGain=16; break;
case 0x02: dGain=17; break;
case 0x03: dGain=18; break;
}
switch (MAXSPEED_port)
{
case 0x00: MAXSPEED=32; break;
case 0x40: MAXSPEED=33; break;
}
33
switch (MINSPEED_port)
{
case 0x00: MINSPEED=25; break;
case 0x80: MINSPEED=26; break;
}
BOOSTSPEED=34;
bstcnt_thresh=250;
minspd_thresh=500;
m=(MAXSPEED-MINSPEED)/10; //SPEED REGULATION RATIO BASED ON MAXSPEED & MINSPEED...
}
/***********************************************************************************/