Bartas Abaravicius / Mbed 2 deprecated TDP_main

Lol smth

Dependencies:   LocalPositionSystem MMA8451Q Motor_Driver Sensors mbed

Fork of TDP_main_BartFork by Yelfie

main.cpp@16:3649eb1a056d, 2015-03-13 (annotated)

Committer:
Reckstyle
Date:
Fri Mar 13 18:19:01 2015 +0000
Revision:
16:3649eb1a056d
Parent:
15:6453cd351452
Child:
17:de8b3111ddc5
fixed pointers problem,still needs testing though

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"
orsharp 12:bb21b76b6375 24 #include "shooter.h"
Reckstyle 9:718987b106a8 25 //#include "Sensors.h"
Reckstyle 0:5ca0450111f3 26
Joseph_Penikis 5:2fc7bf0135d4 27 #define PWM_PERIOD_US 10000
Reckstyle 0:5ca0450111f3 28
Joseph_Penikis 7:14af656b721f 29
orsharp 12:bb21b76b6375 30 //DigitalOut led(LED1);
orsharp 12:bb21b76b6375 31
Reckstyle 16:3649eb1a056d 32 Serial HC06(PTE0,PTE1); //TX,RX
Joseph_Penikis 10:c9212bbfeae6 33 //Serial pc(USBTX, USBRX);
Reckstyle 9:718987b106a8 34
Joseph_Penikis 10:c9212bbfeae6 35 //Timer MeasureTimer; //Timer used for measurement
Joseph_Penikis 10:c9212bbfeae6 36
orsharp 12:bb21b76b6375 37 //Motor_Driver motors(PTD4, PTA12, PTA4, PTA5, PTC9, PTC8, PWM_PERIOD_US);
Reckstyle 9:718987b106a8 38
Reckstyle 11:9e56d52485d1 39 typedef enum mode {REGULAR,SQUARE} mode; //enumeration for different states
Reckstyle 11:9e56d52485d1 40 mode driveMode; //declaring the variable for the states
Reckstyle 11:9e56d52485d1 41 int sensorsCheckSum; //varibale used for sensors mapping access
Reckstyle 11:9e56d52485d1 42
Reckstyle 11:9e56d52485d1 43
Reckstyle 11:9e56d52485d1 44
Reckstyle 11:9e56d52485d1 45 void measureSensors () {
Reckstyle 11:9e56d52485d1 46 sensorsCheckSum = 0; //zero it when first going into the routine
Reckstyle 11:9e56d52485d1 47 int iterationNumber = NUMBER_SENSORS_REGULAR;
Reckstyle 11:9e56d52485d1 48 if (driveMode == SQUARE) {
orsharp 12:bb21b76b6375 49 iterationNumber += NUMBER_SENSORS_SQUARE;
Reckstyle 11:9e56d52485d1 50 }
Reckstyle 11:9e56d52485d1 51 for (int i = 0; i < iterationNumber;i++){
Reckstyle 16:3649eb1a056d 52 if (measure(sensorArray[i]) == 1) {//if sensor is white
Reckstyle 11:9e56d52485d1 53 sensorsCheckSum += (i+1)*(i+1);
Reckstyle 11:9e56d52485d1 54 }
Reckstyle 11:9e56d52485d1 55 }
Reckstyle 11:9e56d52485d1 56 //pc.printf("sensorsCheckSum is %i",sensorsCheckSum);
Reckstyle 11:9e56d52485d1 57 }
Reckstyle 11:9e56d52485d1 58
Reckstyle 15:6453cd351452 59 void printBluetooth() { //for debugging
Reckstyle 16:3649eb1a056d 60
Reckstyle 16:3649eb1a056d 61 HC06.printf("rru%i rlu%i rrd%i rld%i\n",sensorArray[0]->state,sensorArray[1]->state,sensorArray[2]->state,sensorArray[3]->state);
Reckstyle 16:3649eb1a056d 62 //HC06.printf("%i %i %i %i",sensorArray[NUMBER_SENSORS_REGULAR-1]->state,sensorArray[NUMBER_SENSORS_REGULAR-2]->state,sensorArray[1]->state,sensorArray[0]->state);
Reckstyle 16:3649eb1a056d 63 //HC06.printf("%i %i %i %i",sensorArray[NUMBER_SENSORS_REGULAR-3]->state,sensorArray[NUMBER_SENSORS_REGULAR-4]->state,sensorArray[3]->state,sensorArray[2]->state);
Reckstyle 15:6453cd351452 64 //HC06.printf("%i %i/n%i %i,sensorArray[NUMBER_SENSORS_REGULAR]->state,sensorArray[NUMBER_SENSORS_REGULAR+1]->state,sensorArray[NUMBER_SENSORS_REGULAR+2]->state,sensorArray[NUMBER_SENSORS_REGULAR+3]->state);
Reckstyle 15:6453cd351452 65 //HC06.printf("%f %f",motor.getLeftSpeed(),motor.getRightSpeed());
Reckstyle 16:3649eb1a056d 66 //HC06.printf("/n/n");
Reckstyle 15:6453cd351452 67 }
Reckstyle 15:6453cd351452 68
Reckstyle 11:9e56d52485d1 69 int main() {
Reckstyle 11:9e56d52485d1 70
orsharp 12:bb21b76b6375 71
Reckstyle 11:9e56d52485d1 72 /*
Reckstyle 11:9e56d52485d1 73 Motor_Driver motors(PTD4, PTA12, PTA4, PTA5, PTC9, PTC8, PWM_PERIOD_US);
Reckstyle 11:9e56d52485d1 74
Reckstyle 11:9e56d52485d1 75 motors.setSpeed(1.0f);
Reckstyle 11:9e56d52485d1 76 motors.forward();
Reckstyle 11:9e56d52485d1 77 motors.start();
Reckstyle 11:9e56d52485d1 78
Reckstyle 11:9e56d52485d1 79 wait(3);
Reckstyle 11:9e56d52485d1 80
Reckstyle 11:9e56d52485d1 81 motors.stop()
Reckstyle 11:9e56d52485d1 82 motors.reverse();
Reckstyle 11:9e56d52485d1 83 wait(200ms);
Reckstyle 11:9e56d52485d1 84
Reckstyle 11:9e56d52485d1 85 motors.start();
Reckstyle 11:9e56d52485d1 86 */
Reckstyle 11:9e56d52485d1 87 // setup_counter(1000, 0);
Reckstyle 11:9e56d52485d1 88 // float frequency = measure_frequency(CHANNEL_1);
Reckstyle 11:9e56d52485d1 89
Reckstyle 15:6453cd351452 90 driveMode = REGULAR; //initialise drivemoder
Reckstyle 11:9e56d52485d1 91 sensor_initialise(); // initialise sensor values
Reckstyle 11:9e56d52485d1 92 wait(1); //give time to set up the system
Reckstyle 11:9e56d52485d1 93
Reckstyle 11:9e56d52485d1 94 sensorTimer.start(); // start timer for sensors
Reckstyle 11:9e56d52485d1 95
Reckstyle 15:6453cd351452 96 HC06.baud(9600);
Reckstyle 16:3649eb1a056d 97 HC06.printf("working..");
Reckstyle 16:3649eb1a056d 98 while(1){
Reckstyle 16:3649eb1a056d 99 measureSensors();
Reckstyle 16:3649eb1a056d 100 printBluetooth();
Reckstyle 16:3649eb1a056d 101 wait(0.5);
Reckstyle 16:3649eb1a056d 102 }
Reckstyle 16:3649eb1a056d 103
Reckstyle 15:6453cd351452 104
Reckstyle 15:6453cd351452 105 //HC06.printf("AT");
Reckstyle 15:6453cd351452 106 //HC06.printf("AT+PIN5555");
Reckstyle 15:6453cd351452 107
Reckstyle 15:6453cd351452 108
Reckstyle 14:3844d1dacece 109 // pc.printf("Start...");
Reckstyle 14:3844d1dacece 110 //
Reckstyle 14:3844d1dacece 111 //
Reckstyle 14:3844d1dacece 112 // while (1) {
Reckstyle 14:3844d1dacece 113 //
Reckstyle 14:3844d1dacece 114 // if (driveMode == REGULAR) {
Reckstyle 14:3844d1dacece 115 // measureSensors();
Reckstyle 14:3844d1dacece 116 // switch (sensorsCheckSum) {
Reckstyle 14:3844d1dacece 117 // case 1: //right right is 1*1 + 0 + 0+ 0+0 +0
Reckstyle 14:3844d1dacece 118 // pc.printf ("only Right is white \n");
Reckstyle 14:3844d1dacece 119 // break;
Reckstyle 14:3844d1dacece 120 // case 4 : //0*0 + 2*2 + 0 + 0 + 0
Reckstyle 14:3844d1dacece 121 // pc.printf ("only Left is white \n");
Reckstyle 14:3844d1dacece 122 // break;
Reckstyle 14:3844d1dacece 123 // case 5 : //1*1 + 2*2 + 0 + 0
Reckstyle 14:3844d1dacece 124 // pc.printf ("both are white \n");
Reckstyle 14:3844d1dacece 125 // break;
Reckstyle 14:3844d1dacece 126 //// case 91:
Reckstyle 14:3844d1dacece 127 //// driveMode = SQUARE; //if all sensors are white you're in the square
Reckstyle 14:3844d1dacece 128 //// break;
Reckstyle 14:3844d1dacece 129 // default: //checksum is zero , all are black
Reckstyle 14:3844d1dacece 130 // pc.printf ("BLACK BLACK");
Reckstyle 11:9e56d52485d1 131 // break;
Reckstyle 14:3844d1dacece 132 // }
Reckstyle 14:3844d1dacece 133 //
Reckstyle 14:3844d1dacece 134 //
Reckstyle 14:3844d1dacece 135 //
Reckstyle 14:3844d1dacece 136 // }
Reckstyle 14:3844d1dacece 137 // else { //if (driveMode == SQUARE}
Reckstyle 14:3844d1dacece 138 // //implement the square searching thing..
Reckstyle 14:3844d1dacece 139 //
Reckstyle 14:3844d1dacece 140 // }
Reckstyle 14:3844d1dacece 141 //
Reckstyle 14:3844d1dacece 142 //
Reckstyle 14:3844d1dacece 143 // }
Reckstyle 14:3844d1dacece 144 //
Reckstyle 11:9e56d52485d1 145
Reckstyle 11:9e56d52485d1 146 }
Reckstyle 11:9e56d52485d1 147
Reckstyle 11:9e56d52485d1 148
Reckstyle 11:9e56d52485d1 149
Reckstyle 11:9e56d52485d1 150
Reckstyle 11:9e56d52485d1 151
Reckstyle 11:9e56d52485d1 152 /* TOBE deleted
Reckstyle 9:718987b106a8 153 double measure1 (){
Reckstyle 9:718987b106a8 154
Reckstyle 9:718987b106a8 155
Reckstyle 9:718987b106a8 156
Reckstyle 9:718987b106a8 157 sensorTimer.reset();
Reckstyle 9:718987b106a8 158 double freq,period = 0.0;
Reckstyle 9:718987b106a8 159 int n =0; //number of samples
Reckstyle 9:718987b106a8 160 int sensor_old = 0;
Reckstyle 9:718987b106a8 161 int sensor_new = 0;//variable to remember old sensor state
Reckstyle 9:718987b106a8 162 double time_us = sensorTimer.read();
Reckstyle 9:718987b106a8 163 while (n < NUMBER_SAMPLES){
Reckstyle 9:718987b106a8 164 sensor_new = pin_right_right.read();
Reckstyle 9:718987b106a8 165 if ( sensor_new== 1 && sensor_old == 0){ // detect on rising edge ,sensor_old
Reckstyle 9:718987b106a8 166 n++;
Reckstyle 9:718987b106a8 167 }
Reckstyle 9:718987b106a8 168 sensor_old = sensor_new;
Reckstyle 9:718987b106a8 169
Reckstyle 9:718987b106a8 170 }
Reckstyle 9:718987b106a8 171 double time_us2 = sensorTimer.read();
Reckstyle 9:718987b106a8 172 // pc.printf(" delta time is %f , time 2 is %f " , (time_us2 - time_us), time_us2);
Reckstyle 9:718987b106a8 173 period = time_us2/((double)NUMBER_SAMPLES); // Get time
Reckstyle 9:718987b106a8 174 freq = (1/period); // Convert period (in us) to frequency (Hz). Works up to 100kHz.
Reckstyle 9:718987b106a8 175 // pc.printf(" period is %f ", period);
Reckstyle 9:718987b106a8 176
Reckstyle 9:718987b106a8 177 return freq;//(freq < sensor.black_value*2)? 1 : 0;
Reckstyle 9:718987b106a8 178 }
Reckstyle 9:718987b106a8 179 */