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

main.cpp@11:9e56d52485d1, 2015-03-06 (annotated)

Committer:
Reckstyle
Date:
Fri Mar 06 11:50:07 2015 +0000
Revision:
11:9e56d52485d1
Parent:
10:c9212bbfeae6
Child:
12:bb21b76b6375
06/03

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Joseph_Penikis 3:0274e082582e 1 // TESTING REPO COMMIT
Joseph_Penikis 3:0274e082582e 2
Reckstyle 2:e2adb7ab2947 3 /*
Reckstyle 2:e2adb7ab2947 4 ****** MAIN PROGRAM ******
Reckstyle 2:e2adb7ab2947 5
Reckstyle 11:9e56d52485d1 6
Reckstyle 2:e2adb7ab2947 7
Reckstyle 2:e2adb7ab2947 8 Please consider that although it is an embedded envrionment we are NOT creating a HARD-TIME real time system - delays can be dealt with
Reckstyle 11:9e56d52485d1 9
Reckstyle 11:9e56d52485d1 10
Reckstyle 11:9e56d52485d1 11 Sensors are mapped on the global variable sensorsCheckSum,
Reckstyle 11:9e56d52485d1 12 which multiplies the sensor number by itself to then decode,
Reckstyle 11:9e56d52485d1 13 which sensors are off and which are on
Reckstyle 11:9e56d52485d1 14 ie. if sensor rightright - sensorChecksum = 1*1 = 1
Reckstyle 11:9e56d52485d1 15 if rightright and rightcentre - sensorChecksum = 1*1 + 2*2 = 5
Reckstyle 11:9e56d52485d1 16 ...
Reckstyle 2:e2adb7ab2947 17 */
Reckstyle 2:e2adb7ab2947 18
Reckstyle 2:e2adb7ab2947 19
Reckstyle 2:e2adb7ab2947 20
Reckstyle 0:5ca0450111f3 21 #include "mbed.h"
Reckstyle 11:9e56d52485d1 22 #include "sensor_measure.h"
Joseph_Penikis 5:2fc7bf0135d4 23 #include "Motor_Driver.h"
Reckstyle 9:718987b106a8 24 //#include "Sensors.h"
Reckstyle 0:5ca0450111f3 25
Joseph_Penikis 5:2fc7bf0135d4 26 #define PWM_PERIOD_US 10000
Reckstyle 0:5ca0450111f3 27
Joseph_Penikis 7:14af656b721f 28
Joseph_Penikis 10:c9212bbfeae6 29 DigitalOut led(LED1);
Joseph_Penikis 6:bea13722aae7 30
Joseph_Penikis 10:c9212bbfeae6 31 //Serial pc(USBTX, USBRX);
Reckstyle 9:718987b106a8 32
Joseph_Penikis 10:c9212bbfeae6 33 //Timer MeasureTimer; //Timer used for measurement
Joseph_Penikis 10:c9212bbfeae6 34
Joseph_Penikis 10:c9212bbfeae6 35 Motor_Driver motors(PTD4, PTA12, PTA4, PTA5, PTC9, PTC8, PWM_PERIOD_US);
Reckstyle 9:718987b106a8 36
Reckstyle 11:9e56d52485d1 37 typedef enum mode {REGULAR,SQUARE} mode; //enumeration for different states
Reckstyle 11:9e56d52485d1 38 mode driveMode; //declaring the variable for the states
Reckstyle 11:9e56d52485d1 39 int sensorsCheckSum; //varibale used for sensors mapping access
Reckstyle 11:9e56d52485d1 40
Reckstyle 11:9e56d52485d1 41
Reckstyle 11:9e56d52485d1 42
Reckstyle 11:9e56d52485d1 43 void measureSensors () {
Reckstyle 11:9e56d52485d1 44 sensorsCheckSum = 0; //zero it when first going into the routine
Reckstyle 11:9e56d52485d1 45 int iterationNumber = NUMBER_SENSORS_REGULAR;
Reckstyle 11:9e56d52485d1 46 if (driveMode == SQUARE) {
Reckstyle 11:9e56d52485d1 47 itearationNumber += NUMBER_SENSORS_SQUARE;
Reckstyle 11:9e56d52485d1 48 }
Reckstyle 11:9e56d52485d1 49 for (int i = 0; i < iterationNumber;i++){
Reckstyle 11:9e56d52485d1 50 if (measure(*sensorArray[i]) == 1) {//if sensor is white
Reckstyle 11:9e56d52485d1 51 sensorsCheckSum += (i+1)*(i+1);
Reckstyle 11:9e56d52485d1 52 }
Reckstyle 11:9e56d52485d1 53 }
Reckstyle 11:9e56d52485d1 54 //pc.printf("sensorsCheckSum is %i",sensorsCheckSum);
Reckstyle 11:9e56d52485d1 55 }
Reckstyle 11:9e56d52485d1 56
Reckstyle 11:9e56d52485d1 57 int main() {
Reckstyle 11:9e56d52485d1 58
Reckstyle 11:9e56d52485d1 59 /*
Reckstyle 11:9e56d52485d1 60 Motor_Driver motors(PTD4, PTA12, PTA4, PTA5, PTC9, PTC8, PWM_PERIOD_US);
Reckstyle 11:9e56d52485d1 61
Reckstyle 11:9e56d52485d1 62 motors.setSpeed(1.0f);
Reckstyle 11:9e56d52485d1 63 motors.forward();
Reckstyle 11:9e56d52485d1 64 motors.start();
Reckstyle 11:9e56d52485d1 65
Reckstyle 11:9e56d52485d1 66 wait(3);
Reckstyle 11:9e56d52485d1 67
Reckstyle 11:9e56d52485d1 68 motors.stop()
Reckstyle 11:9e56d52485d1 69 motors.reverse();
Reckstyle 11:9e56d52485d1 70 wait(200ms);
Reckstyle 11:9e56d52485d1 71
Reckstyle 11:9e56d52485d1 72 motors.start();
Reckstyle 11:9e56d52485d1 73 */
Reckstyle 11:9e56d52485d1 74 // setup_counter(1000, 0);
Reckstyle 11:9e56d52485d1 75 // float frequency = measure_frequency(CHANNEL_1);
Reckstyle 11:9e56d52485d1 76
Reckstyle 11:9e56d52485d1 77 driveMode = REGULAR;
Reckstyle 11:9e56d52485d1 78 sensor_initialise(); // initialise sensor values
Reckstyle 11:9e56d52485d1 79 wait(1); //give time to set up the system
Reckstyle 11:9e56d52485d1 80
Reckstyle 11:9e56d52485d1 81 sensorTimer.start(); // start timer for sensors
Reckstyle 11:9e56d52485d1 82
Reckstyle 11:9e56d52485d1 83
Reckstyle 11:9e56d52485d1 84 pc.printf("Start...");
Reckstyle 11:9e56d52485d1 85
Reckstyle 11:9e56d52485d1 86
Reckstyle 11:9e56d52485d1 87 while (1) {
Reckstyle 11:9e56d52485d1 88
Reckstyle 11:9e56d52485d1 89 if (driveMode == REGULAR) {
Reckstyle 11:9e56d52485d1 90 measureSensors();
Reckstyle 11:9e56d52485d1 91 switch (sensorsCheckSum) {
Reckstyle 11:9e56d52485d1 92 case 1: //right right is 1*1 + 0 + 0+ 0+0 +0
Reckstyle 11:9e56d52485d1 93 pc.printf ("only Right is white \n");
Reckstyle 11:9e56d52485d1 94 break;
Reckstyle 11:9e56d52485d1 95 case 4 : //0*0 + 2*2 + 0 + 0 + 0
Reckstyle 11:9e56d52485d1 96 pc.printf ("only Left is white \n");
Reckstyle 11:9e56d52485d1 97 break;
Reckstyle 11:9e56d52485d1 98 case 5 : //1*1 + 2*2 + 0 + 0
Reckstyle 11:9e56d52485d1 99 pc.printf ("both are white \n");
Reckstyle 11:9e56d52485d1 100 break;
Reckstyle 11:9e56d52485d1 101 // case 91:
Reckstyle 11:9e56d52485d1 102 // driveMode = SQUARE; //if all sensors are white you're in the square
Reckstyle 11:9e56d52485d1 103 // break;
Reckstyle 11:9e56d52485d1 104 default: //checksum is zero , all are black
Reckstyle 11:9e56d52485d1 105 pc.printf ("BLACK BLACK");
Reckstyle 11:9e56d52485d1 106 break;
Reckstyle 11:9e56d52485d1 107 }
Reckstyle 11:9e56d52485d1 108
Reckstyle 11:9e56d52485d1 109
Reckstyle 11:9e56d52485d1 110
Reckstyle 11:9e56d52485d1 111 }
Reckstyle 11:9e56d52485d1 112 else { //if (driveMode == SQUARE}
Reckstyle 11:9e56d52485d1 113 //implement the square searching thing..
Reckstyle 11:9e56d52485d1 114
Reckstyle 11:9e56d52485d1 115 }
Reckstyle 11:9e56d52485d1 116
Reckstyle 11:9e56d52485d1 117
Reckstyle 11:9e56d52485d1 118 }
Reckstyle 11:9e56d52485d1 119
Reckstyle 11:9e56d52485d1 120 }
Reckstyle 11:9e56d52485d1 121
Reckstyle 11:9e56d52485d1 122
Reckstyle 11:9e56d52485d1 123
Reckstyle 11:9e56d52485d1 124
Reckstyle 11:9e56d52485d1 125
Reckstyle 11:9e56d52485d1 126 /* TOBE deleted
Reckstyle 9:718987b106a8 127 double measure1 (){
Reckstyle 9:718987b106a8 128
Reckstyle 9:718987b106a8 129
Reckstyle 9:718987b106a8 130
Reckstyle 9:718987b106a8 131 sensorTimer.reset();
Reckstyle 9:718987b106a8 132 double freq,period = 0.0;
Reckstyle 9:718987b106a8 133 int n =0; //number of samples
Reckstyle 9:718987b106a8 134 int sensor_old = 0;
Reckstyle 9:718987b106a8 135 int sensor_new = 0;//variable to remember old sensor state
Reckstyle 9:718987b106a8 136 double time_us = sensorTimer.read();
Reckstyle 9:718987b106a8 137 while (n < NUMBER_SAMPLES){
Reckstyle 9:718987b106a8 138 sensor_new = pin_right_right.read();
Reckstyle 9:718987b106a8 139 if ( sensor_new== 1 && sensor_old == 0){ // detect on rising edge ,sensor_old
Reckstyle 9:718987b106a8 140 n++;
Reckstyle 9:718987b106a8 141 }
Reckstyle 9:718987b106a8 142 sensor_old = sensor_new;
Reckstyle 9:718987b106a8 143
Reckstyle 9:718987b106a8 144 }
Reckstyle 9:718987b106a8 145 double time_us2 = sensorTimer.read();
Reckstyle 9:718987b106a8 146 // pc.printf(" delta time is %f , time 2 is %f " , (time_us2 - time_us), time_us2);
Reckstyle 9:718987b106a8 147 period = time_us2/((double)NUMBER_SAMPLES); // Get time
Reckstyle 9:718987b106a8 148 freq = (1/period); // Convert period (in us) to frequency (Hz). Works up to 100kHz.
Reckstyle 9:718987b106a8 149 // pc.printf(" period is %f ", period);
Reckstyle 9:718987b106a8 150
Reckstyle 9:718987b106a8 151 return freq;//(freq < sensor.black_value*2)? 1 : 0;
Reckstyle 9:718987b106a8 152 }
Reckstyle 9:718987b106a8 153 */