Javier Dardón / test_galileo

An example program that uses FRDM-TFC library written by Eli Hughes.

Dependencies:   FRDM-TFC

Fork of TFC-TEST by Daniel Hadad

main.cpp@1:1dd8d5ffdd02, 2014-10-08 (annotated)

Committer:
SaverioF15
Date:
Wed Oct 08 22:50:52 2014 +0000
Revision:
1:1dd8d5ffdd02
Parent:
0:6432166d0781 
Some tests for the freescale cup

Who changed what in which revision?

UserRevisionLine numberNew contents of line
redxeth 0:6432166d0781 1 #include "mbed.h"
redxeth 0:6432166d0781 2 #include "TFC.h"
redxeth 0:6432166d0781 3
redxeth 0:6432166d0781 4
redxeth 0:6432166d0781 5 //This macro is to maintain compatibility with Codewarrior version of the sample. This version uses the MBED libraries for serial port access
redxeth 0:6432166d0781 6 Serial PC(USBTX,USBRX);
SaverioF15 1:1dd8d5ffdd02 7 #define ref_val 5
redxeth 0:6432166d0781 8 #define TERMINAL_PRINTF PC.printf
redxeth 0:6432166d0781 9
redxeth 0:6432166d0781 10
redxeth 0:6432166d0781 11 //This ticker code is used to maintain compability with the Codewarrior version of the sample. This code uses an MBED Ticker for background timing.
redxeth 0:6432166d0781 12
redxeth 0:6432166d0781 13 #define NUM_TFC_TICKERS 4
redxeth 0:6432166d0781 14
redxeth 0:6432166d0781 15 Ticker TFC_TickerObj;
redxeth 0:6432166d0781 16
redxeth 0:6432166d0781 17 volatile uint32_t TFC_Ticker[NUM_TFC_TICKERS];
SaverioF15 1:1dd8d5ffdd02 18 float servo_angle=0.0;
SaverioF15 1:1dd8d5ffdd02 19 float vision[129];
SaverioF15 1:1dd8d5ffdd02 20 unsigned int n_izq=0;
SaverioF15 1:1dd8d5ffdd02 21 unsigned int n_der=0;
SaverioF15 1:1dd8d5ffdd02 22 int error=0;
redxeth 0:6432166d0781 23 void TFC_TickerUpdate()
redxeth 0:6432166d0781 24 {
redxeth 0:6432166d0781 25 int i;
redxeth 0:6432166d0781 26
redxeth 0:6432166d0781 27 for(i=0; i<NUM_TFC_TICKERS; i++)
redxeth 0:6432166d0781 28 {
redxeth 0:6432166d0781 29 if(TFC_Ticker[i]<0xFFFFFFFF)
redxeth 0:6432166d0781 30 {
redxeth 0:6432166d0781 31 TFC_Ticker[i]++;
redxeth 0:6432166d0781 32 }
redxeth 0:6432166d0781 33 }
redxeth 0:6432166d0781 34 }
redxeth 0:6432166d0781 35
redxeth 0:6432166d0781 36
redxeth 0:6432166d0781 37
redxeth 0:6432166d0781 38
redxeth 0:6432166d0781 39 int main()
redxeth 0:6432166d0781 40 {
redxeth 0:6432166d0781 41 uint32_t i,t = 0;
redxeth 0:6432166d0781 42
redxeth 0:6432166d0781 43 PC.baud(115200);
redxeth 0:6432166d0781 44 TFC_TickerObj.attach_us(&TFC_TickerUpdate,2000);
redxeth 0:6432166d0781 45
redxeth 0:6432166d0781 46 TFC_Init();
redxeth 0:6432166d0781 47
redxeth 0:6432166d0781 48 for(;;)
redxeth 0:6432166d0781 49 {
redxeth 0:6432166d0781 50 //TFC_Task must be called in your main loop. This keeps certain processing happy (I.E. Serial port queue check)
redxeth 0:6432166d0781 51 // TFC_Task();
redxeth 0:6432166d0781 52
redxeth 0:6432166d0781 53 //This Demo program will look at the middle 2 switch to select one of 4 demo modes.
redxeth 0:6432166d0781 54 //Let's look at the middle 2 switches
redxeth 0:6432166d0781 55
redxeth 0:6432166d0781 56
redxeth 0:6432166d0781 57 //Demo Mode 2 will use the Pots to make the motors move
redxeth 0:6432166d0781 58 TFC_HBRIDGE_ENABLE;
redxeth 0:6432166d0781 59
SaverioF15 1:1dd8d5ffdd02 60 TFC_SetMotorPWM(TFC_ReadPot(1),TFC_ReadPot(1));
redxeth 0:6432166d0781 61
SaverioF15 1:1dd8d5ffdd02 62 if(TFC_Ticker[0]>=20)
SaverioF15 1:1dd8d5ffdd02 63 {
SaverioF15 1:1dd8d5ffdd02 64 TFC_Ticker[0] = 0; //reset the Ticker
SaverioF15 1:1dd8d5ffdd02 65 //Every 20 mSeconds, update the Servos
SaverioF15 1:1dd8d5ffdd02 66 TFC_SetServo(0,servo_angle);
SaverioF15 1:1dd8d5ffdd02 67 n_der=0;
SaverioF15 1:1dd8d5ffdd02 68 }
SaverioF15 1:1dd8d5ffdd02 69
SaverioF15 1:1dd8d5ffdd02 70 if(TFC_Ticker[1]>50 && TFC_LineScanImageReady>0)
redxeth 0:6432166d0781 71 {
SaverioF15 1:1dd8d5ffdd02 72 TFC_Ticker[1] = 0;
redxeth 0:6432166d0781 73 TFC_LineScanImageReady=0;
redxeth 0:6432166d0781 74 if(t==0)
redxeth 0:6432166d0781 75 t=4;
redxeth 0:6432166d0781 76 else
redxeth 0:6432166d0781 77 t--;
redxeth 0:6432166d0781 78 TFC_SetBatteryLED_Level(t);
redxeth 0:6432166d0781 79
redxeth 0:6432166d0781 80 // camera 1
redxeth 0:6432166d0781 81 for(i=0;i<128;i++)
SaverioF15 1:1dd8d5ffdd02 82
SaverioF15 1:1dd8d5ffdd02 83 {if(TFC_LineScanImage0[i]<=1250)
SaverioF15 1:1dd8d5ffdd02 84 vision[i]=0;
SaverioF15 1:1dd8d5ffdd02 85 else
SaverioF15 1:1dd8d5ffdd02 86 vision[i]= 1;
redxeth 0:6432166d0781 87 }
redxeth 0:6432166d0781 88
SaverioF15 1:1dd8d5ffdd02 89
SaverioF15 1:1dd8d5ffdd02 90 }
SaverioF15 1:1dd8d5ffdd02 91 for(i=65;i<70;i++)
SaverioF15 1:1dd8d5ffdd02 92 {if(vision[i]==1)
SaverioF15 1:1dd8d5ffdd02 93 n_der++;
SaverioF15 1:1dd8d5ffdd02 94 else break;
redxeth 0:6432166d0781 95 }
SaverioF15 1:1dd8d5ffdd02 96 //error= 5-n_der;
SaverioF15 1:1dd8d5ffdd02 97 //if(error>5) error=(error-5)*-1;
SaverioF15 1:1dd8d5ffdd02 98 //servo_angle=0.09*error;
SaverioF15 1:1dd8d5ffdd02 99
SaverioF15 1:1dd8d5ffdd02 100 if(n_der<=7)
SaverioF15 1:1dd8d5ffdd02 101 servo_angle=0.30;
SaverioF15 1:1dd8d5ffdd02 102 else if(n_der<=12 && n_der>=8)servo_angle=0;
SaverioF15 1:1dd8d5ffdd02 103 else if (n_der>=13)servo_angle=-0.30;
SaverioF15 1:1dd8d5ffdd02 104
SaverioF15 1:1dd8d5ffdd02 105 if(TFC_Ticker[1]>1000&&(vision[63]==0||vision[67]))
SaverioF15 1:1dd8d5ffdd02 106 servo_angle*=-1.0;
SaverioF15 1:1dd8d5ffdd02 107 /*if(n_der<3)
SaverioF15 1:1dd8d5ffdd02 108 servo_angle=0.25;
SaverioF15 1:1dd8d5ffdd02 109 else if(n_der<=7 && n_der>=3)servo_angle=0;
SaverioF15 1:1dd8d5ffdd02 110 else if (n_der>7)servo_angle=-0.25;*/
SaverioF15 1:1dd8d5ffdd02 111 /*if(n_der<4 && n_der>0)
SaverioF15 1:1dd8d5ffdd02 112 servo_angle=-0.3;
SaverioF15 1:1dd8d5ffdd02 113 //else if(n_der<=6 && n_der>=4)servo_angle=0;
SaverioF15 1:1dd8d5ffdd02 114 else if(n_der==5)servo_angle=0;
SaverioF15 1:1dd8d5ffdd02 115 else if (n_der>6 && n_der<10)servo_angle=0.3;*/
redxeth 0:6432166d0781 116 }
redxeth 0:6432166d0781 117 }
redxeth 0:6432166d0781 118