Yelfie / Mbed 2 deprecated TDP_main

Team Design project 3 main file

Dependencies:   mbed Motor_Driver Sensors MMA8451Q LocalPositionSystem

Fork of TDP_main by Ivelin Kozarev

sensor_measure.h@38:02ef89edd828, 2015-03-25 (annotated)

Committer:
Bartas
Date:
Wed Mar 25 10:45:08 2015 +0000
Revision:
38:02ef89edd828
Parent:
37:c0fddc75c862 
asd

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Reckstyle 0:5ca0450111f3 1 /*
Reckstyle 0:5ca0450111f3 2 Sensor measurement header file.
Reckstyle 0:5ca0450111f3 3 */
Reckstyle 0:5ca0450111f3 4
Reckstyle 0:5ca0450111f3 5 #ifndef _SENSOR_MEASURE_H
Reckstyle 0:5ca0450111f3 6 #define _SENSOR_MEASURE_H
Reckstyle 0:5ca0450111f3 7
Reckstyle 11:9e56d52485d1 8 #define NUMBER_SAMPLES 20 // NUMBER OF SAMPLES FOR SENSOR TESTING
Reckstyle 22:902c3086394e 9 #define NUMBER_SENSORS_REGULAR 8 // number for the array of sensors
Reckstyle 11:9e56d52485d1 10 #define NUMBER_SENSORS_SQUARE 0
Reckstyle 23:9b53c72fcd38 11 #define BLACK_THRESHOLD 2
Reckstyle 0:5ca0450111f3 12
Reckstyle 0:5ca0450111f3 13 //define pinout for all the sensors
Reckstyle 30:307b45a52401 14 DigitalIn pin_right_right_up(PTC2);
Reckstyle 30:307b45a52401 15 DigitalIn pin_right_left_up(PTE30);
Reckstyle 30:307b45a52401 16 DigitalIn pin_right_right_down(PTE29);
Reckstyle 30:307b45a52401 17 DigitalIn pin_right_left_down(PTC1);
Reckstyle 30:307b45a52401 18 DigitalIn pin_left_right_down(PTB3);
Reckstyle 22:902c3086394e 19 DigitalIn pin_left_left_down(PTE22); //pte22
Reckstyle 30:307b45a52401 20 DigitalIn pin_left_right_up(PTE23); //pte29
Reckstyle 30:307b45a52401 21 DigitalIn pin_left_left_up(PTB2);
Reckstyle 15:6453cd351452 22
Reckstyle 0:5ca0450111f3 23
Reckstyle 0:5ca0450111f3 24 //timer used by the sensor
Reckstyle 0:5ca0450111f3 25 Timer sensorTimer;
Reckstyle 11:9e56d52485d1 26 Serial pc(USBTX, USBRX); //used for connection to PC/debugging
Bartas 37:c0fddc75c862 27 int counter1 = 0;
Reckstyle 9:718987b106a8 28
Reckstyle 0:5ca0450111f3 29 //structure for sensors
Reckstyle 0:5ca0450111f3 30 typedef struct sensor_data {
Reckstyle 9:718987b106a8 31 DigitalIn* pin;
Reckstyle 11:9e56d52485d1 32 int blackValue;
Bartas 37:c0fddc75c862 33 int whiteValue;
Reckstyle 11:9e56d52485d1 34 int grayValue; // if sensor is used for squares will have a gray value;
Bartas 37:c0fddc75c862 35 int state;
Reckstyle 0:5ca0450111f3 36 }sensor_data;
Reckstyle 0:5ca0450111f3 37
Reckstyle 9:718987b106a8 38
Reckstyle 15:6453cd351452 39 sensor_data right_right_up;
Reckstyle 15:6453cd351452 40 sensor_data right_left_up;
Reckstyle 15:6453cd351452 41 sensor_data right_right_down;
Reckstyle 15:6453cd351452 42 sensor_data right_left_down;
Reckstyle 15:6453cd351452 43 sensor_data left_right_down;
Reckstyle 15:6453cd351452 44 sensor_data left_left_down;
Reckstyle 15:6453cd351452 45 sensor_data left_right_up;
Reckstyle 15:6453cd351452 46 sensor_data left_left_up;
Bartas 37:c0fddc75c862 47
Reckstyle 0:5ca0450111f3 48
orsharp 12:bb21b76b6375 49 sensor_data *sensorArray [NUMBER_SENSORS_REGULAR]; //array just used for getting value out of the sensors, helps for iteration(see main program)
Reckstyle 11:9e56d52485d1 50
Reckstyle 1:eace997e9a93 51 //Initialise values of all sensors
Bartas 37:c0fddc75c862 52 void sensor_initialise ()
Bartas 37:c0fddc75c862 53 {
Reckstyle 11:9e56d52485d1 54 int arrayBuilder = 0; //integer solely used for populating the array
Reckstyle 1:eace997e9a93 55 //right_right
Reckstyle 15:6453cd351452 56 right_right_up.pin = &pin_right_right_up;
Reckstyle 32:d1dc27f4a18d 57 right_right_up.blackValue = 13000*BLACK_THRESHOLD;
Reckstyle 32:d1dc27f4a18d 58 right_right_up.whiteValue = 72000;
Reckstyle 15:6453cd351452 59 right_right_up.grayValue = 0; // 0 for all the non-square sensors
Reckstyle 15:6453cd351452 60 right_right_up.state = 0; //setting all sensors as black at begging
Reckstyle 15:6453cd351452 61 sensorArray [arrayBuilder++] = &right_right_up; //Array goes from rightmost to left, then centre?
Reckstyle 15:6453cd351452 62 right_left_up.pin = &pin_right_left_up;
Reckstyle 32:d1dc27f4a18d 63 right_left_up.blackValue = 11500*BLACK_THRESHOLD;
Reckstyle 32:d1dc27f4a18d 64 right_left_up.whiteValue = 70000;
Reckstyle 15:6453cd351452 65 right_left_up.grayValue = 0;
Reckstyle 17:de8b3111ddc5 66 right_left_up.state = 1;
Reckstyle 15:6453cd351452 67 sensorArray [arrayBuilder++] = &right_left_up;
Reckstyle 16:3649eb1a056d 68 right_right_down.pin = &pin_right_right_down;
Reckstyle 32:d1dc27f4a18d 69 right_right_down.blackValue = 2000*BLACK_THRESHOLD;
Reckstyle 32:d1dc27f4a18d 70 right_right_down.whiteValue = 12000;
Reckstyle 17:de8b3111ddc5 71 right_right_down.grayValue = 0;
Reckstyle 17:de8b3111ddc5 72 right_right_down.state = 0;
Reckstyle 16:3649eb1a056d 73 sensorArray [arrayBuilder++] = &right_right_down;
Reckstyle 16:3649eb1a056d 74 right_left_down.pin = &pin_right_left_down;
Reckstyle 30:307b45a52401 75 right_left_down.blackValue = 2500*BLACK_THRESHOLD;
Reckstyle 32:d1dc27f4a18d 76 right_left_down.whiteValue = 17000;
Reckstyle 17:de8b3111ddc5 77 right_left_down.grayValue = 0;
Reckstyle 17:de8b3111ddc5 78 right_left_down.state = 1;
Reckstyle 16:3649eb1a056d 79 sensorArray[arrayBuilder++] = &right_left_down;
Reckstyle 17:de8b3111ddc5 80 left_right_down.pin = &pin_left_right_down;
Reckstyle 30:307b45a52401 81 left_right_down.blackValue = 1800*BLACK_THRESHOLD;
Reckstyle 30:307b45a52401 82 left_right_down.whiteValue = 14000;
Reckstyle 17:de8b3111ddc5 83 left_right_down.grayValue = 0;
Reckstyle 17:de8b3111ddc5 84 left_right_down.state = 1;
Reckstyle 17:de8b3111ddc5 85 sensorArray [arrayBuilder++] = &left_right_down;
Reckstyle 17:de8b3111ddc5 86 left_left_down.pin = &pin_left_left_down;
Reckstyle 32:d1dc27f4a18d 87 left_left_down.blackValue = 3000*BLACK_THRESHOLD;
Reckstyle 32:d1dc27f4a18d 88 left_left_down.whiteValue = 10000;
Reckstyle 17:de8b3111ddc5 89 left_left_down.grayValue = 0;
Reckstyle 17:de8b3111ddc5 90 left_left_down.state = 0;
Reckstyle 17:de8b3111ddc5 91 sensorArray [arrayBuilder++] = &left_left_down;
Reckstyle 17:de8b3111ddc5 92 left_right_up.pin = &pin_left_right_up;
Reckstyle 32:d1dc27f4a18d 93 left_right_up.blackValue = 9500*BLACK_THRESHOLD; //??? it's a trick ;)
Reckstyle 32:d1dc27f4a18d 94 left_right_up.whiteValue = 35000;
Reckstyle 17:de8b3111ddc5 95 left_right_up.grayValue = 0;
Reckstyle 17:de8b3111ddc5 96 left_right_up.state = 1;
Reckstyle 17:de8b3111ddc5 97 sensorArray [arrayBuilder++] = &left_right_up;
Reckstyle 17:de8b3111ddc5 98 left_left_up.pin = &pin_left_left_up;
Reckstyle 32:d1dc27f4a18d 99 left_left_up.blackValue = 12000*BLACK_THRESHOLD;
Reckstyle 32:d1dc27f4a18d 100 left_left_up.whiteValue = 46000;
Reckstyle 17:de8b3111ddc5 101 left_left_up.grayValue = 0;
Reckstyle 17:de8b3111ddc5 102 left_left_up.state = 0;
Reckstyle 17:de8b3111ddc5 103 sensorArray [arrayBuilder++] = &left_left_up;
Reckstyle 1:eace997e9a93 104 }
Reckstyle 1:eace997e9a93 105
Bartas 37:c0fddc75c862 106
Bartas 37:c0fddc75c862 107 //measuring function - returning whether it is black or white line: "0" - black, "1" - white
Bartas 37:c0fddc75c862 108 int measure (sensor_data *sensor)
Bartas 37:c0fddc75c862 109 {
Bartas 37:c0fddc75c862 110 sensorTimer.reset(); //reset the timer
Bartas 37:c0fddc75c862 111 double freq, period = 0.0;
Bartas 37:c0fddc75c862 112 int n = 0; //number of samples
Bartas 37:c0fddc75c862 113 int sensor_new, sensor_old = 0; //variable to remember old and new sensor states
Reckstyle 9:718987b106a8 114 //double time_1 = sensorTimer.read(); used for debugging
Bartas 37:c0fddc75c862 115 while (n < NUMBER_SAMPLES) {
Reckstyle 16:3649eb1a056d 116 sensor_new = sensor->pin->read();
Bartas 37:c0fddc75c862 117 if ( sensor_new== 1 && sensor_old == 0) { // detect on rising edge
Reckstyle 0:5ca0450111f3 118 n++;
Reckstyle 0:5ca0450111f3 119 }
Reckstyle 9:718987b106a8 120 sensor_old = sensor_new;
Reckstyle 0:5ca0450111f3 121 }
Reckstyle 9:718987b106a8 122 double time_2 = sensorTimer.read();
Bartas 37:c0fddc75c862 123 // pc.printf(" delta time is %f , time 2 is %f " , (time_2 - time_1), time_2); //Used for debugging
Bartas 37:c0fddc75c862 124
Bartas 37:c0fddc75c862 125 period = time_2/((double)NUMBER_SAMPLES); // Get time
Bartas 37:c0fddc75c862 126 freq = (1/period); // Convert period (in us) to frequency (Hz).
Reckstyle 16:3649eb1a056d 127 //pc.printf(" period is %f and freq is %f ", period,freq); // Used for debugging
Bartas 37:c0fddc75c862 128
Bartas 37:c0fddc75c862 129 // sensor->state = freq;
Reckstyle 32:d1dc27f4a18d 130 // return 0;
Bartas 37:c0fddc75c862 131
Reckstyle 16:3649eb1a056d 132 // if (sensor->grayValue != 0 && freq < (sensor->grayValue + 1000) && freq > (sensor.grayValue - 1000)) { //definitely not sure about that!
Reckstyle 16:3649eb1a056d 133 // //this is a gray value sensor in its gray region
Reckstyle 16:3649eb1a056d 134 // sensor->state = 2;
Reckstyle 16:3649eb1a056d 135 // return 2;
Reckstyle 16:3649eb1a056d 136 // }
Bartas 37:c0fddc75c862 137 if (freq < (sensor->blackValue)) {
Reckstyle 30:307b45a52401 138 sensor->state = 0; //if it's less than black value it is black
Reckstyle 30:307b45a52401 139 return 0;
Bartas 37:c0fddc75c862 140 } else {
Reckstyle 30:307b45a52401 141 sensor->state = 1;
Bartas 37:c0fddc75c862 142 }
Bartas 37:c0fddc75c862 143 return 1;
Bartas 37:c0fddc75c862 144 //(freq < sensor->black_value*2)? 1 : 0; //freq
Reckstyle 0:5ca0450111f3 145 }
Reckstyle 0:5ca0450111f3 146 #endif