Yelfie / Mbed 2 deprecated TDP_main_BartFork

Don't want to screw something by updating the code as Ivelin updated himself while I was writing so I am forking it.

Dependencies:   LocalPositionSystem MMA8451Q Motor_Driver Sensors mbed

Fork of TDP_main by Yelfie

sensor_measure.h@25:8be440e10126, 2015-03-22 (annotated)

Committer:
Bartas
Date:
Sun Mar 22 23:55:20 2015 +0000
Revision:
25:8be440e10126
Parent:
23:902c3086394e 
All the cases and turns theoretical code. Some could be missing and needs experimentation. Got rid of "goto" call.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Reckstyle 0:5ca0450111f3 1 /*
Reckstyle 0:5ca0450111f3 2
Reckstyle 0:5ca0450111f3 3 Sensor measurement header file.
Reckstyle 0:5ca0450111f3 4 Contains the pin declarations and variables for all sensors-related work
Reckstyle 0:5ca0450111f3 5
Reckstyle 11:9e56d52485d1 6 TESTED AND FUNCTIONAL!(24/02)
Reckstyle 11:9e56d52485d1 7 TESTED (/27/02)
Reckstyle 9:718987b106a8 8 NEEDS POPULATION FOR ALL THE SENSORS!
Reckstyle 11:9e56d52485d1 9
Reckstyle 11:9e56d52485d1 10 Need testing on implementation of the array of pointers
Reckstyle 11:9e56d52485d1 11 Need implementation of third value for gray and a way to distinguish between it - will big structures affect speed?
Reckstyle 11:9e56d52485d1 12 Need to revise struct sensors - are all values reallly needed???
Reckstyle 11:9e56d52485d1 13 LAST REVISED 27/02
Reckstyle 0:5ca0450111f3 14 */
Reckstyle 0:5ca0450111f3 15
Reckstyle 0:5ca0450111f3 16 #ifndef _SENSOR_MEASURE_H
Reckstyle 0:5ca0450111f3 17 #define _SENSOR_MEASURE_H
Reckstyle 0:5ca0450111f3 18
Reckstyle 11:9e56d52485d1 19 #define NUMBER_SAMPLES 20 // NUMBER OF SAMPLES FOR SENSOR TESTING
Reckstyle 23:902c3086394e 20 #define NUMBER_SENSORS_REGULAR 8 // number for the array of sensors
Reckstyle 11:9e56d52485d1 21 #define NUMBER_SENSORS_SQUARE 0
Reckstyle 0:5ca0450111f3 22
Reckstyle 0:5ca0450111f3 23 //define pinout for all the sensors
Reckstyle 23:902c3086394e 24 DigitalIn pin_right_right_up(PTE29);
Reckstyle 23:902c3086394e 25 DigitalIn pin_right_left_up(PTC1);
Reckstyle 23:902c3086394e 26 DigitalIn pin_right_right_down(PTC2);
Reckstyle 23:902c3086394e 27 DigitalIn pin_right_left_down(PTE30);
Reckstyle 23:902c3086394e 28 DigitalIn pin_left_right_down(PTB2);
Reckstyle 23:902c3086394e 29 DigitalIn pin_left_left_down(PTE22); //pte22
Reckstyle 23:902c3086394e 30 DigitalIn pin_left_right_up(PTB3); //pte29
Reckstyle 23:902c3086394e 31 DigitalIn pin_left_left_up(PTE23);
Reckstyle 15:6453cd351452 32
Reckstyle 0:5ca0450111f3 33
Reckstyle 0:5ca0450111f3 34 //timer used by the sensor
Reckstyle 0:5ca0450111f3 35 Timer sensorTimer;
Reckstyle 0:5ca0450111f3 36
Reckstyle 9:718987b106a8 37
Reckstyle 11:9e56d52485d1 38 Serial pc(USBTX, USBRX); //used for connection to PC/debugging
Reckstyle 9:718987b106a8 39
Reckstyle 0:5ca0450111f3 40 //structure for sensors
Reckstyle 0:5ca0450111f3 41 typedef struct sensor_data {
Reckstyle 9:718987b106a8 42 DigitalIn* pin;
Reckstyle 11:9e56d52485d1 43 int blackValue;
Reckstyle 11:9e56d52485d1 44 int whiteValue; //maybe it can be removed ???
Reckstyle 11:9e56d52485d1 45 int grayValue; // if sensor is used for squares will have a gray value;
Reckstyle 11:9e56d52485d1 46 int state; //can be removed as well?
Reckstyle 0:5ca0450111f3 47 }sensor_data;
Reckstyle 0:5ca0450111f3 48
Reckstyle 9:718987b106a8 49
Reckstyle 15:6453cd351452 50 sensor_data right_right_up;
Reckstyle 15:6453cd351452 51 sensor_data right_left_up;
Reckstyle 15:6453cd351452 52 sensor_data right_right_down;
Reckstyle 15:6453cd351452 53 sensor_data right_left_down;
Reckstyle 15:6453cd351452 54 sensor_data left_right_down;
Reckstyle 15:6453cd351452 55 sensor_data left_left_down;
Reckstyle 15:6453cd351452 56 sensor_data left_right_up;
Reckstyle 15:6453cd351452 57 sensor_data left_left_up;
Reckstyle 1:eace997e9a93 58 // and so on....
Reckstyle 0:5ca0450111f3 59
orsharp 12:bb21b76b6375 60 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 61
Reckstyle 1:eace997e9a93 62 //Initialise values of all sensors
Reckstyle 1:eace997e9a93 63 void sensor_initialise () {
Reckstyle 11:9e56d52485d1 64 int arrayBuilder = 0; //integer solely used for populating the array
Reckstyle 1:eace997e9a93 65 //right_right
Reckstyle 15:6453cd351452 66 right_right_up.pin = &pin_right_right_up;
Reckstyle 17:de8b3111ddc5 67 right_right_up.blackValue = 10000;
Reckstyle 17:de8b3111ddc5 68 right_right_up.whiteValue = 48000;
Reckstyle 15:6453cd351452 69 right_right_up.grayValue = 0; // 0 for all the non-square sensors
Reckstyle 15:6453cd351452 70 right_right_up.state = 0; //setting all sensors as black at begging
Reckstyle 15:6453cd351452 71 sensorArray [arrayBuilder++] = &right_right_up; //Array goes from rightmost to left, then centre?
Reckstyle 15:6453cd351452 72 right_left_up.pin = &pin_right_left_up;
Reckstyle 17:de8b3111ddc5 73 right_left_up.blackValue = 30000;
Reckstyle 17:de8b3111ddc5 74 right_left_up.whiteValue = 115000;
Reckstyle 15:6453cd351452 75 right_left_up.grayValue = 0;
Reckstyle 17:de8b3111ddc5 76 right_left_up.state = 1;
Reckstyle 15:6453cd351452 77 sensorArray [arrayBuilder++] = &right_left_up;
Reckstyle 16:3649eb1a056d 78 right_right_down.pin = &pin_right_right_down;
Reckstyle 17:de8b3111ddc5 79 right_right_down.blackValue = 7000;
Reckstyle 17:de8b3111ddc5 80 right_right_down.whiteValue = 20000;
Reckstyle 17:de8b3111ddc5 81 right_right_down.grayValue = 0;
Reckstyle 17:de8b3111ddc5 82 right_right_down.state = 0;
Reckstyle 16:3649eb1a056d 83 sensorArray [arrayBuilder++] = &right_right_down;
Reckstyle 16:3649eb1a056d 84 right_left_down.pin = &pin_right_left_down;
Reckstyle 17:de8b3111ddc5 85 right_left_down.blackValue = 8000;
Reckstyle 17:de8b3111ddc5 86 right_left_down.whiteValue = 25000;
Reckstyle 17:de8b3111ddc5 87 right_left_down.grayValue = 0;
Reckstyle 17:de8b3111ddc5 88 right_left_down.state = 1;
Reckstyle 16:3649eb1a056d 89 sensorArray[arrayBuilder++] = &right_left_down;
Reckstyle 17:de8b3111ddc5 90 left_right_down.pin = &pin_left_right_down;
Reckstyle 17:de8b3111ddc5 91 left_right_down.blackValue = 2500;
Reckstyle 17:de8b3111ddc5 92 left_right_down.whiteValue = 21000;
Reckstyle 17:de8b3111ddc5 93 left_right_down.grayValue = 0;
Reckstyle 17:de8b3111ddc5 94 left_right_down.state = 1;
Reckstyle 17:de8b3111ddc5 95 sensorArray [arrayBuilder++] = &left_right_down;
Reckstyle 17:de8b3111ddc5 96 left_left_down.pin = &pin_left_left_down;
Reckstyle 17:de8b3111ddc5 97 left_left_down.blackValue = 6500;
Reckstyle 17:de8b3111ddc5 98 left_left_down.whiteValue = 21000;
Reckstyle 17:de8b3111ddc5 99 left_left_down.grayValue = 0;
Reckstyle 17:de8b3111ddc5 100 left_left_down.state = 0;
Reckstyle 17:de8b3111ddc5 101 sensorArray [arrayBuilder++] = &left_left_down;
Reckstyle 17:de8b3111ddc5 102 left_right_up.pin = &pin_left_right_up;
Reckstyle 19:d277614084bc 103 left_right_up.blackValue = 20000; //??? it's a trick ;)
Reckstyle 17:de8b3111ddc5 104 left_right_up.whiteValue = 79000;
Reckstyle 17:de8b3111ddc5 105 left_right_up.grayValue = 0;
Reckstyle 17:de8b3111ddc5 106 left_right_up.state = 1;
Reckstyle 17:de8b3111ddc5 107 sensorArray [arrayBuilder++] = &left_right_up;
Reckstyle 17:de8b3111ddc5 108 left_left_up.pin = &pin_left_left_up;
Reckstyle 17:de8b3111ddc5 109 left_left_up.blackValue = 27000;
Reckstyle 17:de8b3111ddc5 110 left_left_up.whiteValue = 120000;
Reckstyle 17:de8b3111ddc5 111 left_left_up.grayValue = 0;
Reckstyle 17:de8b3111ddc5 112 left_left_up.state = 0;
Reckstyle 17:de8b3111ddc5 113 sensorArray [arrayBuilder++] = &left_left_up;
Reckstyle 1:eace997e9a93 114 }
Reckstyle 1:eace997e9a93 115
Reckstyle 16:3649eb1a056d 116 int counter1 = 0;
Reckstyle 0:5ca0450111f3 117 //measuring function - returning whether it is black or white line
Reckstyle 11:9e56d52485d1 118 //"0" - black, "1" - white
Reckstyle 16:3649eb1a056d 119 int measure (sensor_data *sensor){
Reckstyle 1:eace997e9a93 120
Reckstyle 9:718987b106a8 121 sensorTimer.reset(); //reset the timer
Reckstyle 9:718987b106a8 122 double freq,period = 0.0;
Reckstyle 0:5ca0450111f3 123 int n =0; //number of samples
Reckstyle 9:718987b106a8 124 int sensor_old = 0; //variable to remember old sensor state
Reckstyle 9:718987b106a8 125 int sensor_new = 0;
Reckstyle 9:718987b106a8 126 //double time_1 = sensorTimer.read(); used for debugging
Reckstyle 0:5ca0450111f3 127 while (n < NUMBER_SAMPLES){
Reckstyle 16:3649eb1a056d 128 sensor_new = sensor->pin->read();
Reckstyle 9:718987b106a8 129 if ( sensor_new== 1 && sensor_old == 0){ // detect on rising edge
Reckstyle 0:5ca0450111f3 130 n++;
Reckstyle 0:5ca0450111f3 131 }
Reckstyle 9:718987b106a8 132 sensor_old = sensor_new;
Reckstyle 0:5ca0450111f3 133 }
Reckstyle 9:718987b106a8 134 double time_2 = sensorTimer.read();
Reckstyle 9:718987b106a8 135 // pc.printf(" delta time is %f , time 2 is %f " , (time_2 - time_1), time_2); //Used for debugging
Reckstyle 9:718987b106a8 136
Reckstyle 9:718987b106a8 137 period = time_2/((double)NUMBER_SAMPLES); // Get time
Reckstyle 9:718987b106a8 138 freq = (1/period); // Convert period (in us) to frequency (Hz).
Reckstyle 16:3649eb1a056d 139 //pc.printf(" period is %f and freq is %f ", period,freq); // Used for debugging
Reckstyle 16:3649eb1a056d 140
Reckstyle 21:e8da3b351cd0 141 sensor->state = freq;
Reckstyle 21:e8da3b351cd0 142 return 0;
Reckstyle 0:5ca0450111f3 143
Reckstyle 16:3649eb1a056d 144 // if (sensor->grayValue != 0 && freq < (sensor->grayValue + 1000) && freq > (sensor.grayValue - 1000)) { //definitely not sure about that!
Reckstyle 16:3649eb1a056d 145 // //this is a gray value sensor in its gray region
Reckstyle 16:3649eb1a056d 146 // sensor->state = 2;
Reckstyle 16:3649eb1a056d 147 // return 2;
Reckstyle 16:3649eb1a056d 148 // }
Reckstyle 21:e8da3b351cd0 149 // if (freq < (2*sensor->blackValue)){
Reckstyle 21:e8da3b351cd0 150 // sensor->state = 0; //if it's less than black value it is black
Reckstyle 21:e8da3b351cd0 151 // return 0;
Reckstyle 21:e8da3b351cd0 152 // }
Reckstyle 21:e8da3b351cd0 153 // else {
Reckstyle 21:e8da3b351cd0 154 // sensor->state = 1;
Reckstyle 21:e8da3b351cd0 155 // }
Reckstyle 21:e8da3b351cd0 156 // return 1;
Reckstyle 16:3649eb1a056d 157 //(freq < sensor->black_value*2)? 1 : 0; //freq
Reckstyle 0:5ca0450111f3 158 }
Reckstyle 0:5ca0450111f3 159 #endif