Diff: main.cpp

Revision:

0:5e7a5b5b6922

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Apr 11 08:57:27 2015 +0000
@@ -0,0 +1,223 @@
+#include "mbed.h"
+#include "tsi_sensor.h"
+#include <string>
+#include "HCSR04.h"
+
+/* This defines will be replaced by PinNames soon
+#if defined (TARGET_KL25Z) || defined (TARGET_KL46Z)
+#define ELEC0 9
+#define ELEC1 10
+#elif defined (TARGET_KL05Z)
+#define ELEC0 9
+#define ELEC1 8
+#else
+#error TARGET NOT DEFINED
+#endif*/
+
+PwmOut LED(LED1);
+PwmOut leftForward(PTD5);
+PwmOut leftBackward(PTA13);
+PwmOut rightForward(PTA5);
+PwmOut rightBackward(PTA4);
+
+PwmOut grabIn(PTE30);
+PwmOut grabOut(PTC1);
+PwmOut heightUp(PTD4);
+PwmOut heightDown(PTA12);
+
+// Defines the sensors (Left and Right)
+HCSR04 sensorLEFT(PTC12, PTC13);
+HCSR04 sensorRIGHT(PTC16, PTC17);
+
+Serial pc(USBTX, USBRX);
+Serial BT(PTC4, PTC3);
+char command = 0;
+char store;
+float frontSpeed = 0;
+float backSpeed = 0;
+float rightSpeed = 0;
+float leftSpeed = 0;
+bool pilot = false;
+
+
+int distLeft(int dLEFT){
+    return dLEFT;    
+}
+
+// Get the right distance variable
+int distRight(int dRIGHT){
+    return dRIGHT;    
+}
+
+//SELVKØRENDE I MØRKET!
+void selfDrive(){
+    
+    int dLEFT = sensorLEFT.distance(CM);
+    int dRIGHT = sensorRIGHT.distance(CM);
+    /*
+    // Writes the left and right distance variable
+       pc.printf("SENSOR Left: %d \n\r\v",distLeft(dLEFT));
+       pc.printf("SENSOR Right: %d \n\r\v",distRight(dRIGHT));
+       */
+       // Break out of loop if 'p'
+       if(BT.getc() == 'p' && pilot == true){
+            pilot == false;      
+        }
+        
+    //Drej til venstre for at rette op    
+    if(dRIGHT > 35){
+      leftForward = 0.75;
+      leftBackward = 0.25;
+      rightForward = 0.25;
+      rightBackward = 0.75;
+    }
+    
+    else if(dLEFT > 35){
+      leftForward = 0.25;
+      leftBackward = 0.75;
+      rightForward = 0.75;
+      rightBackward = 0.25;
+    }
+    
+    else if(dRIGHT > 50){
+      leftForward = 0.1;
+      leftBackward = 0.9;
+      rightForward = 0.9;
+      rightBackward = 0.1;
+    }     
+    
+    else if(dLEFT > 50){
+      leftForward = 0.1;
+      leftBackward = 0.9;
+      rightForward = 0.9;
+      rightBackward = 0.1;
+      }else{
+      leftForward = 1;
+      leftBackward = 0;
+      rightForward = 1;
+      rightBackward = 0;
+    }         
+}     
+
+void motorControl(char input) {
+  switch (input) {
+    case 'w':
+    BT.printf("FORWARDS!");
+      if (frontSpeed < 0.99) {
+        frontSpeed = frontSpeed + 0.25f;
+}
+      if (backSpeed > 0.1) {
+        backSpeed = backSpeed - 0.25f;
+      }
+      leftForward = frontSpeed;
+      leftBackward = backSpeed;
+      rightForward = frontSpeed;
+      rightBackward = backSpeed;
+      break;
+
+    case 's':
+    if (frontSpeed > 0.1) {
+        frontSpeed = frontSpeed - 0.25f;
+      }
+    if (backSpeed < 0.99) {
+        backSpeed = backSpeed + 0.25f;
+      }
+      leftForward = frontSpeed;
+      leftBackward = backSpeed;
+      rightForward = frontSpeed;
+      rightBackward = backSpeed;
+      break;
+
+    case 'a':
+      if (leftSpeed > 0.1) {
+        leftSpeed = leftSpeed - 0.25f;
+      }
+      if (rightSpeed < 0.99) {
+        rightSpeed = rightSpeed + 0.25f;
+      }
+      leftForward = rightSpeed;
+      leftBackward = leftSpeed;
+      rightForward = leftSpeed;
+      rightBackward = rightSpeed;
+      break;
+
+    case 'd':
+      if (leftSpeed < 0.99) {
+        leftSpeed = leftSpeed + 0.25f;
+      }
+      if (rightSpeed > 0.1) {
+        rightSpeed = rightSpeed - 0.25f;
+      }
+      leftForward = rightSpeed;
+      leftBackward = leftSpeed;
+      rightForward = leftSpeed;
+      rightBackward = rightSpeed;
+      break;
+      
+    case 'r':
+      // Go up
+      heightDown = 0;
+      heightUp.write(0.5);
+      wait(0.25);
+      heightUp = 0;
+      break;
+      
+    case 'f':
+      // Down
+      heightUp = 0;
+      heightDown.write(0.5);
+      wait(0.25);
+      heightDown = 0;
+      break;
+      
+    case 'g':
+      // Close the grab
+      grabOut = 0;
+      grabIn.write(0.5);
+      wait(0.3);
+      grabIn = 0;
+      break;
+    case 'h': 
+      // Open the grab
+      grabIn = 0;
+      grabOut.write(0.5);
+      wait(0.3);
+      grabOut = 0;
+      break;
+    case 'x':
+      frontSpeed = 0;
+      backSpeed = 0;
+      rightSpeed = 0;
+      leftSpeed = 0;
+      leftForward = 0;
+      rightForward = 0;
+      leftBackward = 0;
+      rightBackward = 0;
+      grabIn = 0;
+      grabOut = 0;
+      heightUp = 0;
+      heightDown = 0;
+      break;
+    case 'p':
+        if(pilot == false){
+            pilot = true;
+            while(1){ 
+            selfDrive();
+            }
+        }
+    break;
+  }
+}
+int main(void) {
+  LED = 0.1;
+  leftForward = 0;
+  rightForward = 0;
+  leftBackward = 0;
+  rightBackward = 0;
+  while (1) {
+
+        command = BT.getc();
+        BT.putc(command);
+      motorControl(command);
+    }
+  }