FREERUN ITCH Final Report2 Vguadalajara

The Freescale Cup V1
Project Final Report

Smart Car Design
School: ITCH (Instituto Tecnlogico de Chihuahua) Team: FREERUN

Abel Flix Prez Vctor Hugo Velasco Fernndez Edgar Abraham Gonzlez Romero
Final Project Report
Chihuahua Mexico 2011
Contents
3 23
4 24
6 26
9 27
14 28
22
In this report detailed information about the smart car design, such as electronic, mechanic and software design. This smart car was reengineered in order to participate in the Freescale following line robots competition, therefore a set of rules and parameters limitations had to be thought of. This competition consists in 2 laps on a black line on a white surface in a circuit, so the car must be able to go through a series of turns, a slope and be smart enough to know when to accelerate, decelerate, turn and stop. Next we will talk about hardware used for sensing the line, as well as the interface and software involved; the power circuits that works as a drive for the DC main forward motor and the Servomotor in charge of the car steering. The brain that controls all this is a Freescale TRK_MPC564B Board, this brain will be in charge of getting the signals coming from the sensors, process all the information and provide the control signals such as PWM for the motors. Also, after winning the 1st place on the Local version of the Freescale cup on Chihuahua, we saw the opportunity of having a sponsorship by a local company named IDEA
Using the provided parts, we choose the servomotor to be in this position to get the most of the space inside the chassis for our custom electronics. The image below shows the position of the servomotor on the car.
Figure 1 Placing the Servomotor
The direction system of the car couldnt be modified at all because of the design of itself and also because of the rules of the competition. However, after a few tries, we managed to adjust the mechanism to have a well-defined center position and to be able to symmetrically get the full movement range until the tires got jammed against the structure. On the new car design there are 2 motors on the back part of the car, intended to give forward movement and they are the main power leaker in the system. On the previous version we had to deal with the differential gears tightness to have a better control. This time since there are 2 motors we will, by software, adjust the power on each one when turning so the wheels wont drift on every turn giving a more controllable and smooth movement.
Figure 2 Differential mechanism adjustment
A servomotor is in charge of the steering of the car, this servo can be controlled directly from the TRK_MPC5604B without an additional interface and for forward movement we have a DC motor that has a driver interface shown next. One of the most used circuit for controling motor speed and direction is the H bridge, this one is an easy-to-build and it needs few components. As for the car caracteriztics there is no need for the motor to move in both directions so a half H bridge circuit is enough.
Figure 3 Motor control representations
BJT or MOSFET are commonly used to build the H Bridge, because of the motor characteristics MOSFET were used in this Half H Bridge design. This circuit have a single N channel MOSFET (IRF740) that works as a switch that allows to enable or disable the motor, for the IRF740 activation we use a NPN transistor (2n2222) that is in charge of amplifying the PWM signal from the TRK_MPC5604B to the point where the signal is high enough for the MOSFET to enter in saturated mode.
Figure 4 Half H Bridge Schematic
For the circuit design CAD Eagle 5.11 and OrCad 10.5 were used. Original designs by the team made on Eagle and OrCads Capture had to be redesigned and were taken by our Sponsor IDEA who made some minor changes in order to adapt the thru-hole components to SMD components for Printed Circuit boards made more professionally and with surface mount components.
Figure 5 a) schematic using EAGLE CAD
b) Resulting PCB footprint
Among the electrical characteristics of the half bridge is that it supports up to 10A current through the drain and 400v in Vdss these features were made based on the ability of the mosfet. The drive for the motor with digital input for use as on-off controls is explained on next table: PWM IN Motor OUT High (or high Z) Motor on Low Motor off
To control the amount of power supplied to the motor, it was necessary to use a well-known industry standard called Pulse Width Modulation with a base period of 1kHz because after a few tests we found this frecuency useful to provide enough power to the motor and have few electrical and acoustic noise.
Figure 6 the Half H Bridge assembly
In order to provide the most functionality to the car, we had to make some custom electronics so we were able to give the bolero eyes, power and connectivity. The next illustration shows which and how are the modules connected to the Freescale Embedded board.
Figure 7 The interconnection of the custom modules
IR sensor Interface QRD1113 IR sensors were used in a printed circuit board to monitor the car position along the line. So the circuit board were designed with a bumper shape and positioned at the front of the car. This board has 11 QRDs 1.651 cm away from each other, in order to fit the available space 2 layers were used and the sensors were routed in groups of 3 and 2 for less supply lines and current limiter resistances for the IR leds. Also comparators werent used, as we preferred to use software to save more space and have a self adjustment method for the reference value.
Figure 8 IR line detection board block diagram
Figure 9 IR board schematics
Figure 10 Layout of the IR sensors PCB (bottom view)
10
Figure 11 IR analog sensors board location
Tachometer module A tachometer is used as a motor speed feedback, a SFH 9240 IR reflection sensor along with a passive low-pass filter were used for this purpose, this one was chosen because it can give us a TTL type output signal which makes the processing easier.
Figure 12 a) Tachometer Schematic
b) Tacho PCB layout
To make this work, we had to draw four white stripes on the black gear; this gear spins at a speed that is directly proportional to the motor speed, the wheels have a differential mechanism so the measure may not be exact but it gives us a good reference about the cars speed.
11
Figure 13 tachometer sensor board location
The tachometer reads a logic 1 (about 5v) when it sees a white stipe on the black gear (0 otherwise) so it triggers four times per gear turn, due to the gears relation, the black gears spins much slower than the motors actual angular speed and we get a digital pulse train with the same duty but variable frequency that goes from about 0Hz (which means that the gear is not moving at all) to 200Hz at max speed with no load, divided by four stripes gives us a max frequency of 50Hz (about 3,000 rpm) . Unfortunately the car cant fly yet so we dont expect the gear to spin that fast. Interconnection interface This boards main propose is to simplify; reduce amount of wires in the car and to provide easy-to-use power supply outputs both at 5V and 7.2V directly from the battery, so it is easier to connect additional sensor and other boards. It also provides a power on/off switch for the entire system and finally allows us to test using modules one by one, two or more, or all at the same time.
Figure 14 Interconnection interface circuit. Software: Orcad Capture
12
Figure 15 Interconnection interface PCB. Program: Orcad Layout
These are the original designs made for the Freescale Cup Chihuahua Edition, they were redesigned by our team, then taken by our sponsor IDEA in order to make more professional, Surface mounting, Printed Circuit Boards. The final diagrams are annexed alongside this Report on the CD.
13
The main goal (as you should know by now) of the car is to stay above the line and to have the highest possible speed without losing control (or getting into flames). To achieve this purpose, we needed the car to be smart, fast and power efficient. There are two things needed to control a process; a way to change the response of the mechanism and a way to measure how successful we are on doing it. To save space in this section, well assume that the servomotor is able to turn the wheels wherever we want to inside their own range of movement we have a well-known value for the center, right and left positions (check the servo section), and also we can control the speed of the back dc motor.
Looking down to control

First of all, the sensors; these are the inputs of our system so we need them to mean something useful. In the case of the IR analog sensors were used to detect the line position, we had to choose one of the following options: Using the analog output signals from the sensors, connecting directly and converting them by using the ADC module on the MCP5604B microcontroller. Using analog voltage comparators with a voltage reference driven by a potentiometer so we can get a binary signal for white/black detection and then connecting those signals to a GPIO pad.
Both of the options have advantages, while the first requires less hardware and have no calibration problems, the second option requires less processor time to have a significant data to work with. For further information about the sensors refer to the sensors section. We took the first option because of the capabilities of the microcontroller that allows us to have a continuous scan without wasting processor time and the facility of having a self-adjustment of the reference value. For now, we have decided to work with analog signals so now we know that we are going to need analog inputs and a way to get a reference value to compare with so we can decide if the ground under the sensor is either white or black. Configuring the ADC input pads, the scan mode, the conversion clock, Watchdog timer and some other features; we are ready to start sensing the sensors (see the source code for further information). Now we are able to get 10-bit readings from each sensor but its still kind of useless because we only care if its either black or white so a reference value is essential.
14
To get the reference value, we initialize the sensors by reading their start value one by one, at the same time, we save the highest and lowest values. In this moment we expect at least one of the sensors to be above the black line and at least one of them to be over the white area. If all the sensors are working, we should use highest and lowest levels to compute an average value which is supposed to be in the middle value between black and white so its our gray value, we store it for later comparisons.
Figure 16 Determination of "middle" value
We have now a way to differentiate white from black (by comparing value with reference) but we still need to put this information into a more useful array. The function uint16_t binSENSORS (void) is used to get an array of Boolean values of the sensor measures, using inverted logic (contrary to sensors voltage levels), we take 1 as a black and 0 as a white. Once we have an 11-bit (because we are using 11 IR sensors) array its time to finally get the line position, there were many options to determine lines relative position: Using a Look Up Table (LUT) with every possible response of the sensors. Using the raw value of the binary array. Using bitwise operations to determine the position of the line according to some logic.
The LUT is apparently a bad idea because of the huge number of combinations that can be made with N sensors (11 in our case), but looking closely it takes much less time to the processor to determine the value since it only has to jump and get the value so it seems to be the most efficient way to do it. Raw binaries would get a bad reference since the numbers vary depending on how many sensors are watching a line and how close are them to the border. Taking those considerations, we decide to sacrifice some of the abundant MIPS and go bitwise. The basic idea is to go from left to right and find where the line begins, then we save the value which is an index value of the bit (sensor) that is being evaluated. Then we go in the opposite way (right to left) and find where the line ends. By knowing where the line begins and where it ends, it becomes easy to determine the center value which is where the line is allocated by a simple average of this two values. By using this
15
method, it doesnt matter how many sensors are on the line, we should get a precise value and a plus of precision between sensors.
Figure 17 Line detection representation (line at sensor 5.5)
The following code was written for DEV-C++ 4.9.9.2 Compiler for console, debugging the algorithm. The actual code in the microcontroller can be found at the controller source code.
/******************* FREERUN's line detection algorithm ***********************/ /* This program has been made to debug the line detection algorithm due to */ /* the obvious difficulty of debugging in system. The serial communication */ /* hasn't been configured on the bolero board so printfs will be removed */ /* and the variable definitions changed to fit the typedefs of the compiler */ /******************************************************************************/ #include <stdio.h> //replace with MCPXXXXX.h /*global variables*/ int linepos; //our main goal, value from 0 to 110 that represent line position const int LUT[14]={0x0,0x1,0x3,0x7,0xe,0x1c,0x38,0x70,0xe0,0x1c0,0x380,0x700,0x600,0x400}; void getLINE (int array); int main(void) { int i; //index used as counter int sensors; //the simulated bit array that represent sensors while(getch()!='e'){ //to prevent program from running like hell if (i>13) {i=0;} //if the index i //sensors=sensors<<1; sensors=LUT[i]; //simulating sensor stimulus getLINE(sensors); //we now call our big idea printf("\n\r"); //new line i++; //next i } return 0; //done }
16

void getLINE (int array){ int i; int upcount=0, downcount=0; for (i=10;i>=0;i--){ //printf bit per bit (remove this for) if (array & (0 + (1<<i))) {printf("1");} else {printf("0");} } for (i=10;i>=0;){ //go downcount until the line's is found if (array & (0 + (1<<i))) {downcount=i+1; break;} i--; } printf("\tdowncount >%i\t",downcount);
//printf the result (remove)
for (i=0;i<=10;){ //now go upcount until the other line's border is found if (array & (0 + (1<<i))) {upcount=i+1; break;} i++; } printf("upcount <%i\t",upcount); //printf the result (remove)
linepos = (upcount + downcount) * 5; //finally we get the result!! //multiplied by 5 to have 0-110 range printf("average } %i",linepos); //printf the result (remove)
FREERUN's line detection algorithm source
Figure 18 FREERUN's line detection algorithm console debug
Once we have finally found the lines center position, we can proceed to use this value into our calculations. By setting a reference center position we can now determine the error in the line position.
Time to get some turns

Simply setting the wheels on lines direction is not enough, if we do so, the car will move into an erratically zigzag movement and eventually lose control (see figure), so we actually need a way to give
17
the car a change to mathematically determine how much should turn the wheels in order to keep a nice and smooth movement over the line and also take the hardest curves as a piece of cake.
Figure 19 Using direct control (error*k to servo)
Proportional Integral Derivative

For the control algorithm we did choose a PID loop for both direction and speed control (independent from each other). Since theres vast information about PID control on discrete time, well try to make a clear and brief explanation. Proportional control means how much compensation the system directly should give against perturbations (moving away from the lines center). The difference between actual and past errors is multiplied by a constant KP and so added to the previous response to determine the new response value. The higher KP value, the faster and wider the turns will be.
Figure 20 Proportional controller diagram
Integral control is used to minimize the steady-state error of Proportional controller; basically this is accomplished by using actual and past error values to determine how they changed trough time. The longer perturbation lasts, the higher the integral response will be. Setting a KI value too high will cause the controller to react slowly and we dont want that.
18
Derivative control is used to speed up the Proportional response to a change of input, the faster error change, the higher value of the derivative controller. Setting the KD value would result in more or less zigzagging for the car. For further information about PID control refer to: (Ogata, 1995). Using some discrete time tricks, we get to the following PID equation (Brunl, 2006):
Now we know what to control, how to control, also the controller knows where the line is and its able to turn its wheels and change the speed of the back dc motor. Theres only one thing to do: Implement the PID into the microcontroller, run a few laps and then stop it and adjust the KP, KI and KD constant and try again until we have a very accurate yet fast line follower robot. Its a time-consuming procedure but it worth it.
19
Figure 21 PID direction control diagram
20
Figure 22 PID speed control diagram
21
These are the nominal values, real values may change
22
Freerun car has 15 sensors overall, 11 of these are QRD1113 IR sensors used for line detection; 1 SFH9240 IR sensor more as a tachometer for motor feedback; 1 ADXL335 accelerometer for tilt sensing; 1 IDG500 gyroscope for movement and steering feedback and finally a 1x128 bits TSL1401-DB camera. Part QRD1113 SFH9240 Description REFLECTIVE OBJECT SENSOR Reflective Interrupter with Schmitt-Trigger Small, Low Power, 3-Axis 3 g Accelerometer Integrated Dual-Axis Gyro Linescan Camera Module Output Analog Digital Datasheet (Fairchild Semiconductors) (OSRAM)
ADXL335
Analog
(Analog Devices)
IDG500 TSL1401-DB
Analog Analog
(InvenSense) (Parallax)
23
Weight = 1.1Kg
Height: 9.7cm
Length: 30cm
24
Width: 19cm
25
In the car only use safety futaba servo motor and DC motor that came with the original kit we did consider unnecessary to add another engine to improve the performance of the car. Servo motor Futaba s3004 Specifications Speed (sec/60o): 0.19 Torque (Kg-cm/Oz-in): 4.1/57 Size (mm): 41x20x36 Weight (g/oz): 37.2/1.3
DC Motor RS-380SH
26
The Freescale TRK_MPC564B board easly outnumbers the amount of resources needed for this project, but that also gives us any number of possibilities of how to solve any kind of problem we might face. As for the smart car, it can get better; we are working on how to add a camera for a better line reference as well as upgrading the algorithm doing the math helped by Matlab Although the very same design, having a professionally made circuit board proved less problematic overall, and with sensors and general hardware more stable. Giving us the chance to have more time to improve the software rather than dealing with connection or soldering problems.
27
Analog Devices. (n.d.). adxl335.pdf. Retrieved from http://www.sparkfun.com/datasheets/Components/SMD/adxl335.pdf Brunl, T. (2006). Embedded Robotics (2nd ed.). Springer-Verlag. Fairchild Semiconductors. (n.d.). QRD1113.pdf. Retrieved from http://www.datasheetcatalog.org/datasheet/fairchild/QRD1113.pdf InvenSense. (n.d.). IDG 500 - Integrated Dual-Axis Gyro. Ogata, K. (1995). Discrete-Time Control Systems (2nd Edition ed.). Prentice Hall. OSRAM. (n.d.). SFH9240. Retrieved from http://catalog.osramos.com/jsp/download.jsp?name=sfh9240_Pb_free_2007_04_02.pdf&url=/media/_en/Graphics/ 00043112_0.pdf Parallax. (n.d.). TSL1401-DB_manual.pdf. Retrieved from http://www.parallax.com/Portals/0/Downloads/docs/prod/acc/TSL1401-DB_manual.pdf
28
APENDIX A: SOURCE CODE

<main.c>
/*************************************MAIN.c*****************************************/ /*FREERUN source code for Street smarts competition */ /*FREERUN team members: */ /* Abel Flix Prez */ /* Vctor Hugo Velasco Fernndez */ /* Edgar Abraham Gonzlez Romero */ /* */ /* NOTE: some of the functions were taken from the Application Note: */ /* "Using a RCA camera for line detection" */ /* by: Francisco Ramirez Fuentes, Marco Trujillo, Cuauhtli Padilla, Rodrigo Mendoza /************************************************************************************/ #include "MPC5604B_M27V.h" #include "dMPC5604B.h" #include "hBOARD.h" //#include "setup.c" int16_t lectura,line,servo; uint16_t variable[14]={0x0,0x1,0x3,0x7,0xe,0x1c,0x38,0x70,0xe0,0x1c0,0x380,0x700,0x600,0x400}; const uint16_t NORMALIZATOR = 7*75/77;
*/
int main(void) { uint32_t val=500,arriba=1;
vfnInit_All(); /* Init mode entries and sys clock, Disable watchdog, etc. vfnSetup_Emios_0(); /* Enable global time base etc. */ setBOARDIOS (); /* Initialize default SIUs values for LEDs, PUSHs & SWs, etc initSERVO(); /* Initialize OPWM for the futaba servomotor */ vfnSetup_CamLin(); /* Configure clock signals for the parallax linear scan initADC (); /* Initialize the ADC module for normal scan */ vfnInit_Emios_0(); /* GO eMIOS GO!! */ initSENSORS (); /* Adjust to estimate a reference value (white/black values) initMOTOR (); /* Initialize the PWM signal for the motor at eMIOS1 */ /* Loop forever */
*/
*/
*/
*/
29

for (;;) {
lectura = binSENSORS (); // if (lectura != 0){ //if the line is lost... we pray line = 120 - getLINE(lectura); //a trick to have a full range 0-110 } servo = controlpid(line); //here's the magic, we calculate the servomotor response /*The leds indicate us where is the line (right, center, left)*/ if (lectura & 0x0007){LED2=1; /*setSERVOpos(700);(mere reference)*/ setMOTORspeed(100); } else {LED2=0;} if (lectura & 0x00F8){LED3=1; /*setSERVOpos(350);(mere reference)*/ setMOTORspeed(100); } else {LED3=0;} if (lectura & 0x0700){LED4=1; /*setSERVOpos(0); (mere reference)*/ setMOTORspeed(100); } else {LED4=0;} setSERVOpos(servo); //here the magic is used, we set the servo position
//while (i++ < 10000){while (j++ < 10){}} /*a brute delay for debug use*/ //TO BE CONTINUED... } }
<setup.c>
#include "MPC5604B_M27V.h" /** Disables Watchdog */ void vfnDisable_Watchdog(void) { SWT.SR.R = 0x0000c520; SWT.SR.R = 0x0000d928; SWT.CR.R = 0x8000010A; } /** Initializes the peripheral clock */ void vfnInit_Peri_Clk_Gen(void) { CGM.SC_DC[2].R = 0x80; divided by 1 */ }
/* Write keys to clear soft lock bit */ /* Clear watchdog enable (WEN) */
/* MPC56xxB/S: Enable peri set 3 sysclk
/** Initializes the general modes (ADC, SIU, EMIOS, etc) and clock (PLL, etc) */ void vfnInit_Modes_And_Clock(void) { ME.MER.R = 0x0000001D; /* Enable DRUN, RUN0, SAFE, RESET modes */ /* Initialize PLL before turning it on: */ CGM.FMPLL_CR.R = 0x02400100; /* 8 MHz xtal: Set PLL0 to 64 MHz */ ME.RUN[0].R = 0x001F0074; /* RUN0 cfg: 16MHzIRCON,OSC0ON,PLL0ON,syclk=PLL0 */ ME.RUNPC[1].R = 0x00000010; /* Peri. Cfg. 1 settings: only run in RUN0 mode */ ME.PCTL[32].R = 0x01; /* MPC56xxB/P/S ADC 0: select ME.RUNPC[1] */ ME.PCTL[57].R = 0x01; /* MPC56xxB CTUL: select ME.RUNPC[1] */ ME.PCTL[68].R = 0x01; /* MPC56xxB/S SIU: select ME.RUNPC[1] */ ME.PCTL[72].R = 0x01; /* MPC56xxB/S EMIOS 0: select ME.RUNPC[0] */
30

ME.PCTL[73].R = 0x01; //////////////// ME.PCTL[92].R = 0x01; ME.MCTL.R = 0x40005AF0; ME.MCTL.R = 0x4000A50F; while (ME.GS.B.S_MTRANS) {} */ while(ME.GS.B.S_CURRENTMODE != 4) {} } /** Calls the initialization functions */ void vfnInit_All(void) { vfnInit_Modes_And_Clock(); vfnDisable_Watchdog(); vfnInit_Peri_Clk_Gen(); */ } /* MPC56xxB/S EMIOS 1: /* /* /* /* /* /*

select ME.RUNPC[0] */
PIT, RTI: select ME_RUN_PC[1] */ Mode Transition to enter RUN0 mode: */ Enter RUN0 Mode & Key */ Enter RUN0 Mode & Inverted Key */ Wait for mode transition to complete */ Note: could wait here using timer and/or I_TC IRQ
/* Verify RUN0 is the current mode */
/* Initialize mode entries and system clock */ /* Disable watchdog */ /* Initialize peripheral clock generation for DSPIs
<dSENS.c>
#include "MPC5604B_M27V.h" #include "dMPC5604B.h" uint16_t top_ref=0,bot_ref=1023, avg_ref=512, linepos;
uint16_t readSENS (uint16_t SENS_NUM); uint16_t getLINE (uint16_t array); void initADC (void) { ADC.MCR.R = 0x20000000; ADC.NCMR[0].R = 0x00007FF0; ADC.CTR[0].R = 0x00008606; ADC.MCR.B.NSTART=1; } void initSENSORS (void) { uint16_t i,j=0,value; ADC.NCMR[0].R = 0x00007FF0; /* Select ANP4:14 inputs for conversion */ /* /* /* /* Initialize ADC0 for scan mode */ Select ANP4:14 inputs for conversion */ Conversion times for 32MHz ADClock */ Trigger normal conversions for ADC0 */
for (i=0;i<=10;i++){ SIU.PCR[i+48].R = 0x2000; }
/* MPC56xxB: Initialize PD[0-10] as ANP0 */
while (i++ < 10000){while (j++ < 10){}} /** short delay until we can read good data **/ for (i=0;i<=10;i++){ //bitwise get the top and bot values from the sensors value = readSENS(i); if (value > top_ref){top_ref = value;} if (value < bot_ref){bot_ref = value;} }
31
avg_ref = (top_ref+bot_ref) / 2; //the average is our reference to differentiate black from white } uint16_t binSENSORS (void) { uint16_t i, boolean=0; for(i=0;i<=10;i++){ /*if you have a black line on white track (high sensor output means 0 in the array)*/ boolean = (uint16_t) boolean + ((readSENS(i) < avg_ref) << i); /*else use this for a white line on black track (the oposite) */ /* boolean = (uint16_t) boolean + ((readSENS(i) > avg_ref) << i); */ } return boolean; } uint16_t readSENS (uint16_t SENS_NUM) { return ADC.CDR[SENS_NUM+4].B.CDATA; }
uint16_t getLINE (uint16_t array){ int16_t i; uint16_t upcount=0, downcount=0; //the results of both processes if (array==0){return(0);} for (i=10;i>=0;){ //downcount if (array & (0 + (1<<i))) {/*printf("1");*/ downcount=i+1; break;} i--; } for (i=0;i<=10;){ //upcount if (array & (0 + (1<<i))) {/*printf("1");*/ upcount=i+1; break;} i++; } linepos = (upcount + downcount) * 5; //finally! return linepos; //woop! }
<dADC.c>
#include "MPC5604B_M27V.h" #include "dMPC5604B.h"
/** Set an Adc channel as normal conversion channel **/ void vfnInit_NormalConversion_Adc(uint8_t u8ChannelType,uint32_t u32Channel) { //ADC.MCR.R = 0xA0000000; /* Initialize ADC0 for scan mode y abilitando sobre escritura*/ ADC.MCR.R = 0x20000000; /* Initialize ADC0 for scan mode and allow cross triggering */
32
ADC.NCMR[u8ChannelType].R = 0x00000007; //ADC.NCMR[u8ChannelType].R | u32Channel; /* Set channel mask as normal conversion mask */ ADC.CTR[u8ChannelType].R = 0x00008606; /* Conversion times for 32MHz ADClock */ //ADC.MCR.B.ADCLKSEL = 0; ADC.MCR.B.NSTART=1; /* Start Normal Conversion */ } /** Read Adc channel in scale from 0 to Maximum Value **/ uint16_t u16Read_Adc(uint8_t u8Channel, uint16_t u16MaximumValue) { uint16_t u16Result; if (ADC.CDR[u8Channel].B.VALID == 1) for last scan to complete */ { u16Result = (uint16_t)ADC.CDR[u8Channel].B.CDATA; /* Assign ADC scan value to u16Result */ u16Result = (uint16_t)(u16MaximumValue * u16Result / 1023); Convert to range from 0 to maximum value */ return u16Result; } else { return 1024; } } /* Wait
/*
<dBOARD.c>
#include "MPC5604B_M27V.h" #include "dMPC5604B.h" #include "hBOARD.h" void setBOARDIOS (void){ PE0_IN; PE1_IN; PE2_IN; PE3_IN; del bolero, pr PE4_OUT; PE5_OUT; PE6_OUT; PE7_OUT; PG6_IN; PG7_IN; PG8_IN; PG9_IN; } // // // // Definicion de PEx_IN para escritura al registro de control PCR como entradas en los PUSH Buttons El registro PCR para configuracion del pad de entrada/salida El bit IBE es para configuracion como salida // Pagina 214 del manual
// Definicion de PEx_OUT para escritura al registro de // control PCR como salidas en los Leds
// Definicion de PPGx_IN para escritura al registro de // control PCR como entradas en los Switches
<hBOARD.h>
/*TRK-MPC5604B PINPAD DEFINITIONS*/
33
/* This file has been created to establish a relation between */ /* the built-in hardware features of the TRK-MPC5604B board in */ /* order to explode the most of its functionality. */ /*This relations had been taken from the file: /* TRK_MPC5604B_Rev_B_Schematic_Layout.pdf #include "MPC5604B_M27V.h" #define PE0_IN #define PE1_IN #define PE2_IN entrada/salida #define PE3_IN 214 del manual #define #define #define #define #define #define #define #define SIU.PCR[64].B.IBE=1 SIU.PCR[65].B.IBE=1 SIU.PCR[66].B.IBE=1 SIU.PCR[67].B.IBE=1 del bolero, pr SIU.PCR[68].B.OBE=1 SIU.PCR[69].B.OBE=1 SIU.PCR[70].B.OBE=1 SIU.PCR[71].B.OBE=1
*/ */
// Definicion de PEx_IN para escritura al registro de // control PCR como entradas en los PUSH Buttons // El registro PCR para configuracion del pad de // El bit IBE es para configuracion como salida // Pagina
PE4_OUT PE5_OUT PE6_OUT PE7_OUT PG6_IN PG7_IN PG8_IN PG9_IN
// Definicion de PEx_OUT para escritura al registro de // control PCR como salidas en los Leds
SIU.PCR[102].B.IBE=1 SIU.PCR[103].B.IBE=1 SIU.PCR[104].B.IBE=1 SIU.PCR[105].B.IBE=1
// Definicion de PPGx_IN para escritura al registro de // control PCR como entradas en los Switches
/* Definicion de identificadores para lectura o escitura a los pins*/ #define #define #define #define #define #define #define #define #define #define #define #define PUSH1 PUSH2 PUSH3 PUSH4 LED1 LED2 LED3 LED4 SIU.GPDI[64].B.PDI SIU.GPDI[65].B.PDI SIU.GPDI[66].B.PDI SIU.GPDI[67].B.PDI // identificadores para lectura a los pins de push buttons
SIU.GPDO[68].B.PDO SIU.GPDO[69].B.PDO SIU.GPDO[70].B.PDO SIU.GPDO[71].B.PDO
// identificadores para escritura a los pins de leds
SWITCH1 SWITCH2 SWITCH3 SWITCH4
SIU.GPDI[102].B.PDI // identificadores para lectura a los pins de switches SIU.GPDI[103].B.PDI SIU.GPDI[104].B.PDI SIU.GPDI[105].B.PDI
<dEMIOS.c>
#include "MPC5604B_M27V.h" /****************************************** SETUP FUNCTIONS ****************************************/ /** Main Configuration Register to setup Emios **/ void vfnSetup_Emios_0(void) { EMIOS_0.MCR.B.GPRE= 15; MHz sysclk by 15+1 = 16 for 4MHz eMIOS clk*/ EMIOS_0.MCR.B.GTBE = 1; global time base */ EMIOS_0.MCR.B.FRZ = 1; stopping channels when in debug mode */ } /** Main Configuration Register to initialize Emios **/
/* Divide 64 /* Enable /* Enable
34

void vfnInit_Emios_0(void) { EMIOS_0.MCR.B.GPREN = 1; eMIOS clock, start counting */ }
/* Enable
/** Define Emios Channel as Modulus Counter **/ void vfnInit_Emios_0_Mcb(uint8_t u8Channel, uint16_t u16Period) { EMIOS_0.CH[u8Channel].CADR.R = u16Period; /* Period will be u16Period clocks (usec) */ EMIOS_0.CH[u8Channel].CCR.B.MODE = 0x50; /* Set as Modulus Up Counter Buffered (MCB) */ EMIOS_0.CH[u8Channel].CCR.B.UCPRE = 3; /* Set channel prescaler divide by 3+1=4 (to 1Mhz) */ EMIOS_0.CH[u8Channel].CCR.B.UCPEN = 1; /* Enable prescaler*/ EMIOS_0.CH[u8Channel].CCR.B.FREN = 1; /* Freeze channel counting when in debug mode */ } /** Define Emios Channel as Opwm **/ void vfnInit_Emios_0_Opwm(uint8_t u8Channel, uint16_t u16A, uint16_t u16B) { EMIOS_0.CH[u8Channel].CADR.R = u16A; /* Leading edge when channel counter bus=u16A */ EMIOS_0.CH[u8Channel].CBDR.R = u16B; /* Trailing edge when channel's counter bus=u16B */ EMIOS_0.CH[u8Channel].CCR.B.BSL = 0x1; /* Use counter bus B,C,D,or E */ EMIOS_0.CH[u8Channel].CCR.B.EDPOL = 1; /* Polarity-leading edge sets output/trailing clears */ EMIOS_0.CH[u8Channel].CCR.B.MODE = 0x60; /* Mode is OPWM Buffered */ }
<dCAM.c>
#include "MPC5604B_M27V.h" #include "dMPC5604B.h" /********************* Definitions **************************/ #define CHANNEL_CK 7 #define CHANNEL_DCK 6 //defased clock signal for flip flop triggering #define CHANNEL_SI 9 #define CHANNEL_TRIG 10 #define CAM_CNT_CHAN 8 #define CAM_ADC_CHAN 0 /*************************************************************/
/****************** Global Variables *************************/ uint16_t u16PulseWidthMeasure; uint16_t u16DeltaX ; uint16_t u16PulseWidth ; uint16_t u16Pixel[128]; //la pase al main uint16_t u16CamCounter; uint8_t u8i; uint8_t u8scandone; uint8_t killo=0; uint8_t pipe=500; /*************************************************************/
35
/************** FUNCTIONS USING OPWM FROM EMIOS MODULE TO GENERATE CAMERA INPUT SIGNALS ***********************/ /** Setup configuration for Camera, Define clock and start pulse **/ void vfnSetup_CamLin(void) { vfnInit_Emios_Output_Pad(PCR_EMIOS_0_7); signal */ vfnInit_Emios_Output_Pad(PCR_EMIOS_0_6); signal */ vfnInit_Emios_Output_Pad(PCR_EMIOS_0_9); signal */ vfnSetup_Emios_0(); Emios */ vfnInit_Emios_0_Mcb(0,4); Modulus Counter for CK pulse*/ vfnInit_Emios_0_Mcb(8,2000); Counter for SI pulse*/ /* Path configuration, Emios Output for CK /* Path configuration, Emios Output for DCK /* Path configuration, Emios Output for SI
/* Main Configuration Register to initialize /* Define Emios Channel as /* Define Emios Channel as Modulus //era 2000
vfnInit_Emios_0_Opwm(CHANNEL_CK, 1, 3); /* Define Emios Channel as Opwm, define A and B parameters as 0 and 2 to generate CK signal for the camera */ vfnInit_Emios_0_Opwm(CHANNEL_DCK, 0, 2); /* Define Emios Channel as Opwm, define A and B parameters as 0 and 2 to generate DCK signal for the camera */ vfnInit_Emios_0_Opwm(CHANNEL_SI, 0, 2); /* Define Emios Channel as Opwm, define A and B parameters as 1 and 3 to generate SI signal for the camera */ }
/*****************************************************************/ /* TO BE CONTINUED /*****************************************************************/
*/
<dMOTOR.c>
#include "MPC5604B_M27V.h" #include "dMPC5604B.h" #define MAX_VEL #define MIN_VEL 1000 200
/** Main Configuration Register to setup Emios **/ void vfnSetup_Emios_1(void) { EMIOS_1.MCR.B.GPRE= 15; MHz sysclk by 15+1 = 16 for 4MHz eMIOS clk*/ EMIOS_1.MCR.B.GTBE = 1; global time base */ EMIOS_1.MCR.B.FRZ = 1; stopping channels when in debug mode */ } /** Main Configuration Register to initialize Emios **/ void vfnInit_Emios_1(void)
/* Divide 64 /* Enable /* Enable
36

{ EMIOS_1.MCR.B.GPREN = 1; eMIOS clock, start counting */ }

/* Enable
/** Define Emios Channel as Modulus Counter **/ void vfnInit_Emios_1_Mcb(uint8_t u8Channel, uint16_t u16Period) { EMIOS_1.CH[u8Channel].CADR.R = u16Period; /* Period will be u16Period clocks (usec) */ EMIOS_1.CH[u8Channel].CCR.B.MODE = 0x50; /* Set as Modulus Up Counter Buffered (MCB) */ EMIOS_1.CH[u8Channel].CCR.B.UCPRE = 3; /* Set channel prescaler divide by 3+1=4 (to 1Mhz) */ EMIOS_1.CH[u8Channel].CCR.B.UCPEN = 1; /* Enable prescaler*/ EMIOS_1.CH[u8Channel].CCR.B.FREN = 1; /* Freeze channel counting when in debug mode */ } /** Define Emios Channel as Opwm **/ void vfnInit_Emios_1_Opwm(uint8_t u8Channel, uint16_t u16A, uint16_t u16B) { EMIOS_1.CH[u8Channel].CADR.R = u16A; /* Leading edge when channel counter bus=u16A */ EMIOS_1.CH[u8Channel].CBDR.R = u16B; /* Trailing edge when channel's counter bus=u16B */ EMIOS_1.CH[u8Channel].CCR.B.BSL = 0x1; /* Use counter bus B,C,D,or E */ EMIOS_1.CH[u8Channel].CCR.B.EDPOL = 1; /* Polarity-leading edge sets output/trailing clears */ EMIOS_1.CH[u8Channel].CCR.B.MODE = 0x60; /* Mode is OPWM Buffered */ }
void initMOTOR (void){ vfnSetup_Emios_1(); vfnInit_Emios_1(); vfnInit_Emios_Output_Pad(PCR_EMIOS_1_1); MOTOR signal */ vfnInit_Emios_1_Mcb(0,1000); Counter for MOTOR pulse*/ vfnSetup_Emios_1(); Emios */
/* Path configuration, Emios 1 Output for /* Define Emios Channel as Modulus /* Main Configuration Register to initialize
vfnInit_Emios_1_Opwm(1, 0, 750); /* Define Emios Channel as Opwm, define A and B parameters as 0 and 2 to generate CK signal for the camera */ }
void setMOTORspeed (uint16_t vel){ if ( vel > MAX_VEL ){ vel = MAX_VEL; } /* Never more than maximum servo position (harmless) */ if ( vel < MIN_VEL ){ vel = MIN_VEL; } /* Never less than minimum servo position (harmless) */ EMIOS_1.CH[1].CBDR.R = MAX_VEL - vel; /* Set OPWM duty from MIN_LEV to MAX_LEV (no more than 0-1000) */ }
<dSERVO.c>
37

#include "MPC5604B_M27V.h" #include "dMPC5604B.h" #define #define #define #define SERVOCH 20 MAX_POS 700 MIN_POS 0 CENTER 350
void initSERVO (void){ vfnInit_Emios_Output_Pad(PCR_EMIOS_0_20); SERVOPWM signal */ vfnInit_Emios_0_Mcb(16,20000); Modulus Counter for SERVO pulse*/ vfnSetup_Emios_0(); Emios */
/* Path configuration, Emios Output for /* Define Emios Channel as /* Main Configuration Register to initialize
vfnInit_Emios_0_Opwm(SERVOCH, 0, 1590); define A and B parameters for servo at CENTER position */ } void setSERVOpos (uint16_t pos){ if ( pos > MAX_POS ) { pos = MAX_POS; } position (harmless) */ if ( pos < MIN_POS ){ pos = MIN_POS; } position (harmless) */ EMIOS_0.CH[SERVOCH].CBDR.R = 1240 + pos; */ }
/* Define Emios Channel as Opwm,
/* Never more than maximum servo /* Never less than minimum servo /* Set OPWM duty to 1.1(ms) + pos(us)
<dSIU.c>
#include "MPC5604B_M27V.h" #include "dMPC5604B.h" /** Initialize an Emios Channel as an output pad */ void vfnInit_Emios_Output_Pad(uint8_t u8PcrVal) { SIU.PCR[u8PcrVal].R = 0x0600; /* Assign PCR_EMIOS_0_channel as an output emios pad */ } /** Initialize an Emios Channel as an input pad */ void vfnInit_Emios_Input_Pad(uint8_t u8PcrVal) { SIU.PCR[u8PcrVal].R = 0x0500; /* Assign PCR_EMIOS_0_channel as an input emios pad */ } /** Initialize an Adc Channel pad */ void vfnInit_Adc_Pad(uint8_t u8PcrVal) { SIU.PCR[u8PcrVal].R = 0x2000; /* Assign PCR_ADC_channel as an input adc pad */ } /////////////////////////////////////////// void vfnInit_Gpio_Out(uint8_t u8PcrVal) { SIU.PCR[u8PcrVal].R = 0x0200; /* Init GPIO as output */ } void vfnSet_Gpio(uint8_t u8PcrVal, uint8_t u8Val) { SIU.GPDO[u8PcrVal].B.PDO = u8Val; /* Set the value of the GPIO (0 or 1) */
38

}
<dPID.c>
/* varibles & constants for PID unsigned int MaxSpeed=350; const unsigned int Kp=70, Ki=3, Kd=9; // PID Parameters */ // Hold Motor Maximum Speed
const float // Divide all the PID parameters for decimal value KP=Kp * 0.1, KI=Ki * 0.01, KD=Kd * 0.01; float cont_res; int prev_res=0, prev_err_1=0, prev_err_2=0; // PID Control Variables int motor_res,err_func,servo_res; int TARGET_VAL=60;
//codigo PID/// int controlpid(int sensor_val){ // Get the Error Function err_func= sensor_val - TARGET_VAL; // Calculate the Motor Response using PID Control Equation cont_res=(float)(prev_res + KP * (err_func - prev_err_1) + KI * (err_func + prev_err_1)/2.0 + KD * (err_func - 2.0 * prev_err_1 + prev_err_2));
// Limit the control response to the Maximum of servo PWM Value motor_res=(int)cont_res; servo_res = motor_res+350; if (servo_res >= 700) //notice that 700 is the max servo position value {servo_res = 700;} else if (servo_res <= 0) {servo_res = 0;} else{servo_res = servo_res;}
// Save the Motor Response and Error Function Result prev_res=motor_res; prev_err_2=prev_err_1; prev_err_1=err_func; return(servo_res); //thats it! }
39

FREERUN ITCH Final Report2 Vguadalajara

Uploaded by

Document Information

Original Description:

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

FREERUN ITCH Final Report2 Vguadalajara

Uploaded by

Copyright:

Available Formats

The Freescale Cup V1

Project Final Report

School: ITCH (Instituto Tecnlogico de Chihuahua) Team: FREERUN

Final Project Report

Chihuahua Mexico 2011

Final Project Report

Chihuahua Mexico 2011

Figure 1 Placing the Servomotor

Final Project Report

Chihuahua Mexico 2011

Figure 2 Differential mechanism adjustment

Final Project Report

Chihuahua Mexico 2011

Figure 3 Motor control representations

Final Project Report

Chihuahua Mexico 2011

Figure 4 Half H Bridge Schematic

Figure 5 a) schematic using EAGLE CAD

b) Resulting PCB footprint

Final Project Report

Chihuahua Mexico 2011

Figure 6 the Half H Bridge assembly

Final Project Report

Chihuahua Mexico 2011

Figure 7 The interconnection of the custom modules

Final Project Report

Chihuahua Mexico 2011

Figure 8 IR line detection board block diagram

Figure 9 IR board schematics

Figure 10 Layout of the IR sensors PCB (bottom view)

Final Project Report

Chihuahua Mexico 2011

Figure 11 IR analog sensors board location

Figure 12 a) Tachometer Schematic

b) Tacho PCB layout

Final Project Report

Chihuahua Mexico 2011

Figure 13 tachometer sensor board location

Figure 14 Interconnection interface circuit. Software: Orcad Capture

Final Project Report

Chihuahua Mexico 2011

Figure 15 Interconnection interface PCB. Program: Orcad Layout

Final Project Report

Chihuahua Mexico 2011

Looking down to control

Final Project Report

Chihuahua Mexico 2011

Figure 16 Determination of "middle" value

Final Project Report

Chihuahua Mexico 2011

Figure 17 Line detection representation (line at sensor 5.5)

Final Project Report

Chihuahua Mexico 2011

//printf the result (remove)

FREERUN's line detection algorithm source

Figure 18 FREERUN's line detection algorithm console debug

Time to get some turns

Final Project Report

Chihuahua Mexico 2011

Figure 19 Using direct control (error*k to servo)

Proportional Integral Derivative

Figure 20 Proportional controller diagram