Bram Stoelinga
Projectvroemba
Revision 0:dc7bdd0a1b32, committed 2016-12-14
- Comitter:
- bramstoelinga
- Date:
- Wed Dec 14 21:18:17 2016 +0000
- Commit message:
- ii
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
| mbed.bld | Show annotated file Show diff for this revision Revisions of this file |
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Dec 14 21:18:17 2016 +0000
@@ -0,0 +1,429 @@
+#include "mbed.h"
+#include <algorithm>
+
+double voltageToDistance(uint16_t);
+void setPwmBoth(float);
+
+double FilterRechts(double);
+double FilterVoor(double);
+double FilterBeneden(double);
+double FilterLinks(double);
+
+AnalogIn sensorA1(A2); //Sensor 1
+AnalogIn sensorA2(A3); //Sensor 2
+AnalogIn sensorA3(A4); //Sensor 3
+AnalogIn sensorA4(A5); //Sensor 4
+
+//H-brug poorten (pwm)
+PwmOut pwmL1(PC_8);
+PwmOut pwmL3(PB_4);
+
+//Duty cycle
+float dutyCycle1 = 0.0f;
+float dutyCycle2 = 0.0f;
+
+void setPwmBoth(float pwmFloat) {
+ dutyCycle1 = pwmFloat;
+ dutyCycle2 = pwmFloat;
+ pwmL1 = dutyCycle1;
+ pwmL3 = dutyCycle2;
+}
+
+double FilterRechts(double sensorValue) {
+ static double sensorArray[10] = {20, 20, 20, 20, 20, 20, 20, 20, 20, 20};
+ static int arrayPosition = 0;
+ double average = 0;
+ sensorArray[arrayPosition] = sensorValue;
+ arrayPosition ++;
+ for(int i = 0; i < 10; i++) {
+ average += sensorArray[i];
+ }
+ average /= 10;
+ if(arrayPosition == 10) {
+ arrayPosition = 0;
+ }
+ printf("%f\n", average);
+ return average;
+}
+double FilterVoor(double sensorValue) {
+ static double sensorArray[10] = {20, 20, 20, 20, 20, 20, 20, 20, 20, 20};
+ static int arrayPosition = 0;
+ double average = 0;
+ sensorArray[arrayPosition] = sensorValue;
+ arrayPosition ++;
+ for(int i = 0; i < 10; i++)
+ {
+ average += sensorArray[i];
+ }
+ average /= 10;
+ if(arrayPosition == 10)
+ {
+ arrayPosition = 0;
+ }
+ printf("%f\n", average);
+ return average;
+}
+double FilterBeneden(double sensorValue) {
+ static double sensorArray[10] = {14, 14, 14, 14, 14, 14, 14, 14, 14, 14};
+ static int arrayPosition = 0;
+ double average = 0;
+ sensorArray[arrayPosition] = sensorValue;
+ arrayPosition ++;
+ for(int i = 0; i < 10; i++)
+ {
+ average += sensorArray[i];
+ }
+ average /= 10;
+ if(arrayPosition == 10)
+ {
+ arrayPosition = 0;
+ }
+ printf("%f\n", average);
+ return average;
+}
+double FilterLinks(double sensorValue) {
+ static double sensorArray[10] = {20, 20, 20, 20, 20, 20, 20, 20, 20, 20};
+ static int arrayPosition = 0;
+ double average = 0;
+ sensorArray[arrayPosition] = sensorValue;
+ arrayPosition ++;
+ for(int i = 0; i < 10; i++)
+ {
+ average += sensorArray[i];
+ }
+ average /= 10;
+ if(arrayPosition == 10)
+ {
+ arrayPosition = 0;
+ }
+ printf("%f\n", average);
+ return average;
+}
+
+double voltageToDistance(uint16_t voltage) {
+ if (voltage < 16)
+ {
+ voltage = 16;
+ }
+ double distance = 0;
+ distance = 2076.0 / (voltage - 11.0);
+ return distance * 100;
+}
+
+int main(){
+ enum State
+ {
+ initialize,
+ wait,
+ autonomous,
+ HmoveForward,
+ HturnLeft,
+ HturnRight,
+ Hreverse,
+ neutral,
+ AmoveForward,
+ Aleft,
+ Aright,
+ AturnLeft,
+ AturnRight
+ };
+
+ //Digitale H-brug poorten
+ DigitalOut L2(PB_8);
+ DigitalOut E1(PC_9);
+ DigitalOut L4(PB_14);
+ DigitalOut E2(PB_15);
+
+ //Statemachine
+ State stateMachine;
+ stateMachine = initialize;
+
+ //Serieele poorten
+ Serial pc(USBTX, USBRX);
+ Serial bluetooth(PA_11, PA_12); // TX, RX
+
+ //Booleans om afgelopen statussen te onthouden
+ bool fromForward = false;
+ bool fromReverse = false;
+ bool status = false; //status false = autonoom, true = handmatig
+
+ //Timer voor het uitrijden van de bluetooth
+ Timer bluetoothTimer;
+ Timer autoTimer;
+
+ int movingDelay = 25;
+ int cutoff = 30;
+ int detection = 25;
+
+ while (1)
+ {
+ switch(stateMachine)
+ {
+ case initialize:
+ pwmL1.period_ms(10);
+ pwmL3.period_ms(10);
+ pwmL1 = dutyCycle1;
+ pwmL3 = dutyCycle2;
+ bluetooth.baud(115200);
+ pc.baud(115200);
+ pc.printf("Bluetooth Start\r\n");
+ stateMachine = wait;
+ break;
+
+ case wait:
+ if (bluetooth.readable())
+ {
+ switch(bluetooth.getc())
+ {
+ case 30:
+ status = true;
+ bluetoothTimer.start();
+ break;
+
+ case 40:
+ status = false;
+ stateMachine = autonomous;
+ break;
+
+ case 49:
+ bluetoothTimer.reset();
+ if(status == true)
+ {
+ stateMachine = HmoveForward;
+ }
+ break;
+
+ case 47:
+ bluetoothTimer.reset();
+ if(status == true)
+ {
+ stateMachine = HturnLeft;
+ }
+ break;
+
+ case 51:
+ bluetoothTimer.reset();
+ if(status == true)
+ {
+ stateMachine = HturnRight;
+ }
+ break;
+
+ case 55:
+ bluetoothTimer.reset();
+ if(status == true) {
+ stateMachine = Hreverse;
+ }
+ break;
+ }
+ }
+ else if (status == false)
+ {
+ stateMachine = autonomous;
+ }
+ else if (status == true)
+ {
+ stateMachine = neutral;
+ }
+ break;
+
+ case autonomous:
+ double SensorRechts = voltageToDistance(sensorA1.read_u16());
+ double SensorVoor = voltageToDistance(sensorA2.read_u16());
+ double SensorBeneden = voltageToDistance(sensorA3.read_u16());
+ double SensorLinks = voltageToDistance(sensorA4.read_u16());
+
+ if(SensorRechts > cutoff) {SensorRechts = cutoff;}
+ if(SensorVoor > cutoff) {SensorVoor = cutoff;}
+ if(SensorBeneden > cutoff) {SensorBeneden = cutoff;}
+ if(SensorLinks > cutoff) {SensorLinks = cutoff;}
+
+ SensorRechts = FilterRechts(SensorRechts);
+ SensorVoor = FilterVoor(SensorVoor);
+ SensorBeneden = FilterBeneden(SensorBeneden);
+ SensorLinks = FilterLinks(SensorLinks);
+
+ if(SensorBeneden > 17 || SensorBeneden < 11)
+ {
+ stateMachine = AturnLeft; //Draai naar links als er een afgrond is of een obstakel ligt
+ }
+ else if(SensorRechts > detection && SensorVoor > detection && SensorLinks > detection)
+ {
+ stateMachine = AmoveForward; //Naar voren als sensoren niks zien.
+ }
+ else if(SensorRechts < detection && SensorVoor > detection && SensorLinks > detection)
+ {
+ stateMachine = Aleft; //Naar links afbuigen als een schuine sensor een object ziet.
+ }
+ else if(SensorRechts > detection && SensorVoor > detection && SensorLinks < detection)
+ {
+ stateMachine = Aright; //Naar rechts afbuigen als een schuine sensor een object ziet.
+ }
+ else if(SensorVoor < detection && SensorLinks > detection)
+ {
+ stateMachine = AturnLeft; //Naar links draaien als de sensor naar voren iets ziet (rechts maakt niet uit)
+ }
+ else if(SensorVoor < detection && SensorLinks < detection)
+ {
+ stateMachine = AturnRight; //Naar rechts draaien als de sensoren naar voor en links iets zien
+ }
+ break;
+
+ case HmoveForward:
+ fromForward = true;
+ if(fromReverse == true)
+ {
+ setPwmBoth(0.0f);
+ fromReverse = false;
+ }
+ if(dutyCycle1 <= 0.99f && status == true)
+ {
+ E2 = true;
+ L4 = false;
+ E1 = true;
+ L2 = false;
+ dutyCycle1 += 0.05f;
+ dutyCycle2 += 0.05f;
+ pwmL1 = dutyCycle1;
+ pwmL3 = dutyCycle2;
+ }
+ stateMachine = wait;
+ break;
+
+ case HturnLeft:
+ E1 = true;
+ L2 = true;
+ E2 = true;
+ L4 = false;
+ dutyCycle1 = dutyCycle2 = 0.5f;
+ pwmL1 = pwmL3 = dutyCycle1;
+ stateMachine = wait;
+ break;
+
+ case HturnRight:
+ E1 = true;
+ L2 = false;
+ E2 = true;
+ L4 = true;
+ dutyCycle1 = dutyCycle2 = 0.5f;
+ pwmL1 = pwmL3 = dutyCycle1;
+ stateMachine = wait;
+ break;
+
+ case Hreverse:
+ fromReverse = true;
+ E2 = true;
+ L4 = true;
+ E1 = true;
+ L2 = true;
+ if(fromForward == true)
+ {
+ setPwmBoth(1.0f);
+ fromForward = false;
+ }
+ dutyCycle1 -= 0.05f;
+ dutyCycle2 -= 0.05f;
+ pwmL1 = dutyCycle1;
+ pwmL3 = dutyCycle2;
+ stateMachine = wait;
+ break;
+
+ case neutral:
+ if(bluetoothTimer.read_ms() > 100) {
+ E2 = false;
+ L4 = false;
+ pwmL1 = false;
+ pwmL3 = false;
+ E1 = false;
+ L2 = false;
+ if (fromForward == true){setPwmBoth(0.0f);}
+ else if(fromReverse == true){setPwmBoth(1.0f);}
+ }
+ stateMachine = wait;
+ break;
+
+ case AmoveForward:
+ autoTimer.start();
+ while(autoTimer.read_ms() < movingDelay)
+ {
+ E1 = true;
+ L2 = false;
+ E2 = true;
+ L4 = false;
+ setPwmBoth(0.60f);
+ }
+ autoTimer.stop();
+ autoTimer.reset();
+ stateMachine = wait;
+ break;
+
+ case Aright:
+ autoTimer.start();
+ while(autoTimer.read_ms() < movingDelay)
+ {
+ E1 = true;
+ L2 = false;
+ E2 = false;
+ L4 = false;
+ pwmL3 = false;
+ dutyCycle1 = 0.60f;
+ pwmL1 = dutyCycle1;
+ }
+ autoTimer.stop();
+ autoTimer.reset();
+ stateMachine = wait;
+ break;
+
+ case Aleft:
+ autoTimer.start();
+ while(autoTimer.read_ms() < movingDelay)
+ {
+ E1 = false;
+ L2 = false;
+ pwmL1 = false;
+ E2 = true;
+ L4 = false;
+ dutyCycle2 = 0.60f;
+ pwmL3 = dutyCycle2;
+ }
+ autoTimer.stop();
+ autoTimer.reset();
+ stateMachine = wait;
+ break;
+
+ case AturnRight:
+ autoTimer.start();
+ while(autoTimer.read_ms() < movingDelay)
+ {
+ E1 = true;
+ L2 = false;
+ E2 = true;
+ L4 = true;
+ setPwmBoth(0.50f);
+ }
+ autoTimer.stop();
+ autoTimer.reset();
+ stateMachine = wait;
+ break;
+
+ case AturnLeft:
+ autoTimer.start();
+ while(autoTimer.read_ms() < movingDelay)
+ {
+ E1 = true;
+ L2 = true;
+ E2 = true;
+ L4 = false;
+ setPwmBoth(0.50f);
+ }
+ autoTimer.stop();
+ autoTimer.reset();
+ stateMachine = wait;
+ break;
+
+ default:
+ stateMachine = wait;
+ break;
+ }
+ }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Wed Dec 14 21:18:17 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/6c34061e7c34 \ No newline at end of file