FREERUN ITCH Final Report2 vGuadalajara

The Freescale Cup V1

School: ITCH (Instituto Tecnólogico de Chihuahua)

Team: FREERUN

Abel Félix PérezVíctor Hugo Velasco Fernández

Edgar Abraham González Romero

Project Final ReportSmart Car Design

Final Project Report Chihuahua Mexico 2011

Contents

3

4

6

9

14

22

23

2


24

26

27

28

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

3


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

4


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 couldn’t 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,

5


by software, adjust the power on each one when turning so the wheels won’t drift on every turn giving a more controllable and smooth movement.

Figure 2 Differential mechanism adjustment

6


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.

TRK_MPC5604Bhalf

H bridge

Schematic for a DC Motor control

PWM

+V

GND

Tachometer

+V

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.

7


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 OrCad’s 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 OUTHigh (or high Z) Motor onLow Motor off

8


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

9


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.

SENSORS MOTOR DRIVERMODULE

FREESCALETRK-MPC5604B

BOARD

Aditional HARDWARE

FREESCALE STREET SMART KIT

AnalogSignals

AnalogSignals OPWM

AmplifiedPWM

DigitalSignals

MOTIONSENSORS

DC & STEPMOTORS

TACHOMETERCOMPARATORAND FILTER

MODULE

VISIONSYSTEM

AnalogSignals

General Structure

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 weren’t used, as we preferred to use software to save more space and have a self adjustment method for the reference value.

10


TRK_MPC5604B

IRSensor

5v

GND

11 out

InterconnectionModule

Schematic for IR Sensor

Figure 8 IR line detection board block diagram

Figure 9 IR board schematics

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

11


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 car’s speed.

12


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 motor’s 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 can’t fly yet so we don’t expect the gear to spin that fast.

Interconnection interface

This board’s 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

13


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.

14


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, we’ll 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 controlFirst 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 it’s still kind of useless because we only care if it’s either black or white so a reference value is essential.

15


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 it’s 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 it’s time to finally get the line position, there were many options to determine line’s 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

16


method, it doesn’t 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 positionconst 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 counterint 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 linei++; //next i

}

return 0; //done

17


}

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 foundif (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 foundif (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 line’s 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 line’s 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

18


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 there’s vast information about PID control on discrete time, we’ll 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 line’s 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 don’t want that.

19


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 (Bräunl, 2006):

Rn = Rn–1 + KP · (en – en–1) + KI · (en + en–1)/2 + KD · (en - 2·en–1 + en–2)

Formula Symbol DescriptionImplemented

VariablesType

Rn Current servo motor Response servo_res int

Rn-1 Previous servo motor Response prev_res int

en Current Error err_ func int

en-1 Previous Error prec_err_1 int

en-2 Previous Error 2 prec_err_2 int

Kp Proportional Parameter Factor Kp int

Ki Integral Parameter Factor Ki int

Kd Derivative Parameter Factor Kd int

The Complete PID Formula

Now we know what to control, how to control, also the controller knows where the line is and it’s able to turn its wheels and change the speed of the back dc motor. There’s 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. It’s a time-consuming procedure but it worth it.

20


Figure 21 PID direction control diagram

21


Proportional

Integral

Derivative

SE(t)Target value

+

- ControlResponse

PID CONTROL

DCMotor

Tachometer

FeedBack

Speed Control Loop

Figure 22 PID speed control diagram

22


These are the nominal values, real values may change

23


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 Description Output Datasheet

QRD1113 REFLECTIVE OBJECT SENSOR Analog (Fairchild Semiconductors)

SFH9240 Reflective Interrupter with Schmitt-Trigger

Digital (OSRAM)

ADXL335 Small, Low Power, 3-Axis ±3 g Accelerometer

Analog (Analog Devices)

IDG500 Integrated Dual-Axis Gyro Analog (InvenSense)

TSL1401-DB Linescan Camera Module Analog (Parallax)

24


Weight = 1.1Kg

Height: 9.7cm

Length: 30cm

25


Width: 19cm

26


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

27


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.

28


Analog Devices. (n.d.). adxl335.pdf. Retrieved from http://www.sparkfun.com/datasheets/Components/SMD/adxl335.pdf

Bräunl, 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.osram-os.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

29


APENDIX A: SOURCE CODE<main.c>

/*************************************MAIN.c*****************************************//*FREERUN source code for Street smarts competition

*//*FREERUN team members:

*//* Abel Félix Pérez

*//* Víctor Hugo Velasco Fernández

*//* Edgar Abraham González 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 */

30


for (;;) {

lectura = binSENSORS (); //if (lectura != 0){ //if the line is lost... we prayline = 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; /* Write keys to clear soft lock bit */ SWT.SR.R = 0x0000d928; SWT.CR.R = 0x8000010A; /* Clear watchdog enable (WEN) */}

/** Initializes the peripheral clock */void vfnInit_Peri_Clk_Gen(void) { CGM.SC_DC[2].R = 0x80; /* MPC56xxB/S: Enable peri set 3 sysclk divided by 1 */}

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

31


ME.PCTL[73].R = 0x01; /* MPC56xxB/S EMIOS 1: select ME.RUNPC[0] */ //////////////// ME.PCTL[92].R = 0x01; /* PIT, RTI: select ME_RUN_PC[1] */ /* Mode Transition to enter RUN0 mode: */ ME.MCTL.R = 0x40005AF0; /* Enter RUN0 Mode & Key */ ME.MCTL.R = 0x4000A50F; /* Enter RUN0 Mode & Inverted Key */ while (ME.GS.B.S_MTRANS) {} /* Wait for mode transition to complete */ /* Note: could wait here using timer and/or I_TC IRQ */ while(ME.GS.B.S_CURRENTMODE != 4) {} /* Verify RUN0 is the current mode */}

/** Calls the initialization functions */void vfnInit_All(void){ vfnInit_Modes_And_Clock(); /* Initialize mode entries and system clock */ vfnDisable_Watchdog(); /* Disable watchdog */ vfnInit_Peri_Clk_Gen(); /* 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; /* Initialize ADC0 for scan mode */ ADC.NCMR[0].R = 0x00007FF0; /* Select ANP4:14 inputs for conversion */ ADC.CTR[0].R = 0x00008606; /* Conversion times for 32MHz ADClock */ ADC.MCR.B.NSTART=1; /* Trigger normal conversions for ADC0 */}

void initSENSORS (void) {uint16_t i,j=0,value;

ADC.NCMR[0].R = 0x00007FF0; /* Select ANP4:14 inputs for conversion */

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 sensorsvalue = readSENS(i);if (value > top_ref){top_ref = value;}if (value < bot_ref){bot_ref = value;}

}

32


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;){ //downcountif (array & (0 + (1<<i))) {/*printf("1");*/ downcount=i+1; break;}i--;

}

for (i=0;i<=10;){ //upcountif (array & (0 + (1<<i))) {/*printf("1");*/ upcount=i+1; break;}i++;

}

linepos = (upcount + downcount) * 5; //finally!

return linepos; //woop!

}

<dADC.c>


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

33


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

}

<dBOARD.c>

#include "MPC5604B_M27V.h"#include "dMPC5604B.h"#include "hBOARD.h"

void setBOARDIOS (void){

PE0_IN; // Definicion de PEx_IN para escritura al registro de PE1_IN; // control PCR como entradas en los PUSH ButtonsPE2_IN; // El registro PCR para configuracion del pad de entrada/salidaPE3_IN; // El bit IBE es para configuracion como salida // Pagina 214 del manual

del bolero, pr PE4_OUT; // Definicion de PEx_OUT para escritura al registro dePE5_OUT; // control PCR como salidas en los LedsPE6_OUT;PE7_OUT;

PG6_IN; // Definicion de PPGx_IN para escritura al registro de PG7_IN; // control PCR como entradas en los SwitchesPG8_IN;PG9_IN;

}

<hBOARD.h>

/*TRK-MPC5604B PINPAD DEFINITIONS*/

34


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


#define PE0_IN SIU.PCR[64].B.IBE=1 // Definicion de PEx_IN para escritura al registro de #define PE1_IN SIU.PCR[65].B.IBE=1 // control PCR como entradas en los PUSH Buttons#define PE2_IN SIU.PCR[66].B.IBE=1 // El registro PCR para configuracion del pad de entrada/salida#define PE3_IN SIU.PCR[67].B.IBE=1 // El bit IBE es para configuracion como salida // Pagina 214 del manual del bolero, pr

#define PE4_OUT SIU.PCR[68].B.OBE=1 // Definicion de PEx_OUT para escritura al registro de#define PE5_OUT SIU.PCR[69].B.OBE=1 // control PCR como salidas en los Leds#define PE6_OUT SIU.PCR[70].B.OBE=1#define PE7_OUT SIU.PCR[71].B.OBE=1

#define PG6_IN SIU.PCR[102].B.IBE=1 // Definicion de PPGx_IN para escritura al registro de #define PG7_IN SIU.PCR[103].B.IBE=1 // control PCR como entradas en los Switches#define PG8_IN SIU.PCR[104].B.IBE=1#define PG9_IN SIU.PCR[105].B.IBE=1

/* Definicion de identificadores para lectura o escitura a los pins*/

#define PUSH1 SIU.GPDI[64].B.PDI // identificadores para lectura a los pins de push buttons#define PUSH2 SIU.GPDI[65].B.PDI#define PUSH3 SIU.GPDI[66].B.PDI#define PUSH4 SIU.GPDI[67].B.PDI

#define LED1 SIU.GPDO[68].B.PDO // identificadores para escritura a los pins de leds#define LED2 SIU.GPDO[69].B.PDO#define LED3 SIU.GPDO[70].B.PDO#define LED4 SIU.GPDO[71].B.PDO

#define SWITCH1 SIU.GPDI[102].B.PDI // identificadores para lectura a los pins de switches#define SWITCH2 SIU.GPDI[103].B.PDI#define SWITCH3 SIU.GPDI[104].B.PDI#define SWITCH4 SIU.GPDI[105].B.PDI

<dEMIOS.c>


/****************************************** SETUP FUNCTIONS ****************************************/

/** Main Configuration Register to setup Emios **/void vfnSetup_Emios_0(void){

EMIOS_0.MCR.B.GPRE= 15; /* Divide 64 MHz sysclk by 15+1 = 16 for 4MHz eMIOS clk*/ EMIOS_0.MCR.B.GTBE = 1; /* Enable global time base */ EMIOS_0.MCR.B.FRZ = 1; /* Enable stopping channels when in debug mode */}

35


/** Main Configuration Register to initialize Emios **/void vfnInit_Emios_0(void){ EMIOS_0.MCR.B.GPREN = 1; /* Enable eMIOS clock, start counting */}

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


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

36


/*************************************************************/

/************** 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); /* Path configuration, Emios Output for CK signal */

vfnInit_Emios_Output_Pad(PCR_EMIOS_0_6); /* Path configuration, Emios Output for DCK signal */

vfnInit_Emios_Output_Pad(PCR_EMIOS_0_9); /* Path configuration, Emios Output for SI signal */

vfnSetup_Emios_0(); /* Main Configuration Register to initialize Emios */ vfnInit_Emios_0_Mcb(0,4); /* Define Emios Channel as Modulus Counter for CK pulse*/ vfnInit_Emios_0_Mcb(8,2000); /* Define Emios Channel as Modulus Counter for SI pulse*/

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


#define MAX_VEL 1000#define MIN_VEL 200

/** Main Configuration Register to setup Emios **/void vfnSetup_Emios_1(void){

EMIOS_1.MCR.B.GPRE= 15; /* Divide 64 MHz sysclk by 15+1 = 16 for 4MHz eMIOS clk*/ EMIOS_1.MCR.B.GTBE = 1; /* Enable global time base */ EMIOS_1.MCR.B.FRZ = 1; /* Enable stopping channels when in debug mode */}

37


/** Main Configuration Register to initialize Emios **/void vfnInit_Emios_1(void){ EMIOS_1.MCR.B.GPREN = 1; /* Enable eMIOS clock, start counting */}

/** 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); /* Path configuration, Emios 1 Output for

MOTOR signal */vfnInit_Emios_1_Mcb(0,1000); /* Define Emios Channel as Modulus

Counter for MOTOR pulse*/vfnSetup_Emios_1(); /* Main Configuration Register to initialize

Emios */

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

38


<dSERVO.c>


#define SERVOCH 20#define MAX_POS 700#define MIN_POS 0#define CENTER 350

void initSERVO (void){vfnInit_Emios_Output_Pad(PCR_EMIOS_0_20); /* Path configuration, Emios Output for

SERVOPWM signal */vfnInit_Emios_0_Mcb(16,20000); /* Define Emios Channel as

Modulus Counter for SERVO pulse*/vfnSetup_Emios_0(); /* Main Configuration Register to initialize

Emios */

vfnInit_Emios_0_Opwm(SERVOCH, 0, 1590); /* Define Emios Channel as Opwm, define A and B parameters for servo at CENTER position */}

void setSERVOpos (uint16_t pos){if ( pos > MAX_POS ) { pos = MAX_POS; } /* Never more than maximum servo

position (harmless) */if ( pos < MIN_POS ){ pos = MIN_POS; } /* Never less than minimum servo

position (harmless) */EMIOS_0.CH[SERVOCH].CBDR.R = 1240 + pos; /* Set OPWM duty to 1.1(ms) + pos(us)

*/}

<dSIU.c>


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

39


}void vfnSet_Gpio(uint8_t u8PcrVal, uint8_t u8Val){

SIU.GPDO[u8PcrVal].B.PDO = u8Val; /* Set the value of the GPIO (0 or 1) */}

<dPID.c>

/* varibles & constants for PID */unsigned int MaxSpeed=350; // Hold Motor Maximum Speedconst unsigned int Kp=70,Ki=3,Kd=9; // PID Parameters

const float // Divide all the PID parameters for decimal valueKP=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 Variablesint motor_res,err_func,servo_res;

int TARGET_VAL=60;

//codigo PID///

int controlpid(int sensor_val){ // Get the Error Functionerr_func= sensor_val - TARGET_VAL;

// Calculate the Motor Response using PID Control Equationcont_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 Valuemotor_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 Resultprev_res=motor_res;prev_err_2=prev_err_1;prev_err_1=err_func;

return(servo_res); //thats it!

}

40

Documents

FREERUN ITCH Final Report2 vGuadalajara