Ivelin Kozarev / Mbed 2 deprecated TDP_main_fork

Team Design project 3 main file

Dependencies:   LocalPositionSystem MMA8451Q Motor_Driver Sensors mbed

Fork of TDP_main by Yelfie

main.cpp@35:1819c5a8254a, 2015-03-24 (annotated)

Committer:
Reckstyle
Date:
Tue Mar 24 19:51:28 2015 +0000
Revision:
35:1819c5a8254a
Parent:
34:9e7d192ee06c
Child:
36:a48d57d63630
vis

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Reckstyle 2:e2adb7ab2947 1 /*
Reckstyle 2:e2adb7ab2947 2 ****** MAIN PROGRAM ******
Reckstyle 2:e2adb7ab2947 3
Reckstyle 2:e2adb7ab2947 4 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 5
Reckstyle 11:9e56d52485d1 6 Sensors are mapped on the global variable sensorsCheckSum,
Reckstyle 11:9e56d52485d1 7 which multiplies the sensor number by itself to then decode,
Reckstyle 11:9e56d52485d1 8 which sensors are off and which are on
Reckstyle 11:9e56d52485d1 9 ie. if sensor rightright - sensorChecksum = 1*1 = 1
Reckstyle 11:9e56d52485d1 10 if rightright and rightcentre - sensorChecksum = 1*1 + 2*2 = 5
Reckstyle 11:9e56d52485d1 11 ...
Reckstyle 2:e2adb7ab2947 12 */
Reckstyle 2:e2adb7ab2947 13
Reckstyle 0:5ca0450111f3 14 #include "mbed.h"
Reckstyle 11:9e56d52485d1 15 #include "sensor_measure.h"
Joseph_Penikis 5:2fc7bf0135d4 16 #include "Motor_Driver.h"
orsharp 12:bb21b76b6375 17 #include "shooter.h"
Reckstyle 0:5ca0450111f3 18
Joseph_Penikis 5:2fc7bf0135d4 19 #define PWM_PERIOD_US 10000
Reckstyle 0:5ca0450111f3 20
Reckstyle 28:d1e7daeb240e 21 DigitalOut led(LED_RED);
Bartas 24:ecc3fbaf2824 22 Serial HC06(PTE0,PTE1);
Reckstyle 9:718987b106a8 23
Reckstyle 35:1819c5a8254a 24 void turnLeft ();
Reckstyle 35:1819c5a8254a 25 void turnRight ();
Reckstyle 35:1819c5a8254a 26 void measureSensors ();
Reckstyle 35:1819c5a8254a 27 void printBluetooth();
Reckstyle 35:1819c5a8254a 28
Reckstyle 35:1819c5a8254a 29
Reckstyle 18:d277614084bc 30 Timer measureTimer; //Timer used for measurement
Reckstyle 29:307b45a52401 31 Motor_Driver motors(PTA5, PTC9, PTC8,PTD4, PTA12, PTA4, PWM_PERIOD_US); //motors object
Reckstyle 9:718987b106a8 32
Reckstyle 11:9e56d52485d1 33 typedef enum mode {REGULAR,SQUARE} mode; //enumeration for different states
Reckstyle 11:9e56d52485d1 34 mode driveMode; //declaring the variable for the states
Reckstyle 11:9e56d52485d1 35 int sensorsCheckSum; //varibale used for sensors mapping access
Reckstyle 18:d277614084bc 36 int passedTime1,passedTime2;
Reckstyle 25:bec5dc4c9f5e 37 int oldValues[5] = {0};
Bartas 27:fc0fd2c0eebb 38 int cursor = 0, k = 0;
Reckstyle 35:1819c5a8254a 39 float normalSpeed = 0.15f;
Reckstyle 11:9e56d52485d1 40
Reckstyle 11:9e56d52485d1 41
Reckstyle 35:1819c5a8254a 42 int main() {
Reckstyle 35:1819c5a8254a 43
Reckstyle 35:1819c5a8254a 44 //setup_counter(1000, 0);
Reckstyle 35:1819c5a8254a 45 // float frequency = measure_frequency(CHANNEL_1);
Reckstyle 35:1819c5a8254a 46 led = 0;//start LED with beginning of main program
Reckstyle 35:1819c5a8254a 47
Reckstyle 35:1819c5a8254a 48 measureTimer.start();
Reckstyle 35:1819c5a8254a 49 driveMode = REGULAR; //initialise drivemoder
Reckstyle 35:1819c5a8254a 50 sensor_initialise(); // initialise sensor values
Reckstyle 35:1819c5a8254a 51 wait(1); //give time to set up the system
Reckstyle 35:1819c5a8254a 52
Reckstyle 35:1819c5a8254a 53 sensorTimer.start(); // start timer for sensors
Reckstyle 35:1819c5a8254a 54
Reckstyle 35:1819c5a8254a 55 motors.setSteeringMode (NORMAL);
Reckstyle 35:1819c5a8254a 56 motors.disableHardBraking();
Reckstyle 35:1819c5a8254a 57 motors.forward ();
Reckstyle 35:1819c5a8254a 58
Reckstyle 35:1819c5a8254a 59 motors.start ();
Reckstyle 35:1819c5a8254a 60 motors.setSpeed (normalSpeed);
Reckstyle 35:1819c5a8254a 61
Reckstyle 35:1819c5a8254a 62 //motors.enableSlew();
Reckstyle 35:1819c5a8254a 63
Reckstyle 35:1819c5a8254a 64 // HC06.printf("heello");
Reckstyle 35:1819c5a8254a 65 int testOtherCases = 0;
Reckstyle 35:1819c5a8254a 66 while(1){
Reckstyle 35:1819c5a8254a 67 measureSensors();
Reckstyle 35:1819c5a8254a 68 //printBluetooth();
Reckstyle 35:1819c5a8254a 69 // if (HC06.getc() == 'r') {
Reckstyle 35:1819c5a8254a 70 // measureSensors();
Reckstyle 35:1819c5a8254a 71 // printBluetooth();
Reckstyle 35:1819c5a8254a 72 // }
Reckstyle 35:1819c5a8254a 73
Reckstyle 35:1819c5a8254a 74 switch (sensorsCheckSum) {
Reckstyle 35:1819c5a8254a 75 case 94:
Reckstyle 35:1819c5a8254a 76 motors.setSpeed (normalSpeed*2);
Reckstyle 35:1819c5a8254a 77 wait(0.4);
Reckstyle 35:1819c5a8254a 78 break;
Reckstyle 35:1819c5a8254a 79
Reckstyle 35:1819c5a8254a 80 case 104:
Reckstyle 35:1819c5a8254a 81 turnRight ();
Reckstyle 35:1819c5a8254a 82 break;
Reckstyle 35:1819c5a8254a 83 default:
Reckstyle 35:1819c5a8254a 84 testOtherCases =1;
Reckstyle 35:1819c5a8254a 85 break;
Reckstyle 35:1819c5a8254a 86 }
Reckstyle 35:1819c5a8254a 87 if (testOtherCases == 1) {
Reckstyle 35:1819c5a8254a 88 if (sensorsCheckSum < 96) {
Reckstyle 35:1819c5a8254a 89 motors.setSpeed (0.0);
Reckstyle 35:1819c5a8254a 90 motors.setLeftSpeed(normalSpeed*2);
Reckstyle 35:1819c5a8254a 91 wait(0.1);
Reckstyle 35:1819c5a8254a 92 }
Reckstyle 35:1819c5a8254a 93 else if (sensorsCheckSum > 96) {
Reckstyle 35:1819c5a8254a 94 motors.setSpeed (0.0);
Reckstyle 35:1819c5a8254a 95 motors.setRightSpeed(normalSpeed*2);
Reckstyle 35:1819c5a8254a 96 wait(0.1);
Reckstyle 35:1819c5a8254a 97 }
Reckstyle 35:1819c5a8254a 98
Reckstyle 35:1819c5a8254a 99 }
Reckstyle 35:1819c5a8254a 100 motors.setSpeed(0.0);
Reckstyle 35:1819c5a8254a 101 wait(0.2);
Reckstyle 35:1819c5a8254a 102
Reckstyle 35:1819c5a8254a 103 }
Reckstyle 35:1819c5a8254a 104 } //int main
Reckstyle 35:1819c5a8254a 105
Reckstyle 35:1819c5a8254a 106 void printBluetooth() { //for debugging
Reckstyle 35:1819c5a8254a 107 HC06.printf("LLU%i LRU%i rlu%i rru%i\n",sensorArray[7]->state,sensorArray[6]->state,sensorArray[1]->state,sensorArray[0]->state);
Reckstyle 35:1819c5a8254a 108 HC06.printf("LLD%i LRD%i rld%i rrd%i\n\n",sensorArray[5]->state,sensorArray[4]->state,sensorArray[3]->state,sensorArray[2]->state);
Reckstyle 35:1819c5a8254a 109 //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 35:1819c5a8254a 110 //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 35:1819c5a8254a 111 //HC06.printf("%f %f",motor.getLeftSpeed(),motor.getRightSpeed());
Reckstyle 35:1819c5a8254a 112 HC06.printf("sensorCheckSum%i\n\n",sensorsCheckSum);
Reckstyle 35:1819c5a8254a 113 //HC06.printf("passedTime1 %i passedTime2 \n\n",passedTime1,passedTime2);
Reckstyle 35:1819c5a8254a 114 }
Reckstyle 35:1819c5a8254a 115
Reckstyle 35:1819c5a8254a 116 void turnRight () {
Reckstyle 35:1819c5a8254a 117 motors.setSpeed (normalSpeed);
Reckstyle 35:1819c5a8254a 118 wait(0.7);
Reckstyle 35:1819c5a8254a 119 motors.setSpeed(0.0);
Reckstyle 35:1819c5a8254a 120 motors.setLeftSpeed (normalSpeed);
Reckstyle 35:1819c5a8254a 121 wait (1.4);
Reckstyle 35:1819c5a8254a 122 int exit = 0; //used for exiting the function
Reckstyle 35:1819c5a8254a 123 motors.setSpeed(0.0);
Reckstyle 35:1819c5a8254a 124 wait(0.2);
Reckstyle 35:1819c5a8254a 125 motors.setSpeed (normalSpeed);
Reckstyle 35:1819c5a8254a 126 for (int i = 4; i < NUMBER_SENSORS_REGULAR; i++) {
Reckstyle 35:1819c5a8254a 127 if (measure(sensorArray[i]) == 0) {//if sensor is black
Reckstyle 35:1819c5a8254a 128 exit ++;
Reckstyle 35:1819c5a8254a 129 }
Reckstyle 35:1819c5a8254a 130 if (exit > 1){
Reckstyle 35:1819c5a8254a 131
Reckstyle 35:1819c5a8254a 132 return;
Reckstyle 35:1819c5a8254a 133 }
Reckstyle 35:1819c5a8254a 134 exit = 0;
Reckstyle 35:1819c5a8254a 135 }
Reckstyle 35:1819c5a8254a 136
Reckstyle 35:1819c5a8254a 137 }
Reckstyle 35:1819c5a8254a 138
Reckstyle 11:9e56d52485d1 139 void measureSensors () {
Bartas 24:ecc3fbaf2824 140 sensorsCheckSum = 0; //zero it when first going into the routine
Bartas 24:ecc3fbaf2824 141 int iterationNumber = NUMBER_SENSORS_REGULAR;
Bartas 24:ecc3fbaf2824 142 if (driveMode == SQUARE) {
orsharp 12:bb21b76b6375 143 iterationNumber += NUMBER_SENSORS_SQUARE;
Bartas 24:ecc3fbaf2824 144 }
Bartas 24:ecc3fbaf2824 145 for (int i = 0; i < iterationNumber;i++){
Bartas 24:ecc3fbaf2824 146 //pc.printf("%i iteration%i ",i,iterationNumber);
Bartas 24:ecc3fbaf2824 147 if (measure(sensorArray[i]) == 1) {//if sensor is white
Reckstyle 11:9e56d52485d1 148 sensorsCheckSum += (i+1)*(i+1);
Reckstyle 11:9e56d52485d1 149 }
Bartas 24:ecc3fbaf2824 150 }
Reckstyle 25:bec5dc4c9f5e 151 if (oldValues[cursor] != sensorsCheckSum) { //why only if different ??
Reckstyle 25:bec5dc4c9f5e 152 oldValues[cursor] = sensorsCheckSum;
Reckstyle 23:9b53c72fcd38 153 cursor = (cursor+1)%5;
Bartas 24:ecc3fbaf2824 154 }
Bartas 24:ecc3fbaf2824 155 //pc.printf("sensorsCheckSum is %i",sensorsCheckSum);
Reckstyle 11:9e56d52485d1 156 }