Diff: main.cpp

Revision:

1:719ac5a42c19

Parent:

0:c871d5355b99

Child:

2:fae63e87900e

--- a/main.cpp	Sun Feb 07 06:14:32 2016 +0000
+++ b/main.cpp	Mon Feb 08 11:13:44 2016 +0000
@@ -1,40 +1,139 @@
 #include <mbed.h>
+#include "rtos.h"
 #include "Periphery/SupportSystem.h"
 #include "Misc/SystemTimer.h"
-#include "rtos.h"
-
-#define PI 3.14159265
 
 Serial serialXbee(p28,p27), serialMinnow(p13, p14);
 PwmOut drivePWM(p22), steerPWM(p21);
-I2C i2c(p9, p10);
+
 
 DigitalOut serialLED(LED1), ledTwo(LED2), ledThree(LED3), heartbeatLED(LED4);
 
 DigitalIn buttonOne(p25), buttonTwo(p26), buttonThree(p29), buttonFour(p30);
 DigitalIn redlight(p16);
 
+
 IMU_RegisterDataBuffer_t *IMU_registerDataBuffer;
+Timer timer;
 
 void hearbeat_thread(void const *args);
 
 int main() {
+    serialMinnow.baud(115200);
+    I2C *i2c = new I2C(p9, p10);
+    
     drivePWM.period_ms(20);
     steerPWM.period_ms(20);
 
     SystemTimer *millis = new SystemTimer();
-    SupportSystem *supportSystem = new SupportSystem(0x80, &i2c);
+    SupportSystem *supportSystem = new SupportSystem(0x80, i2c);
+    
     Thread hearbeatThread(hearbeat_thread);
+
+    steerPWM.pulsewidth_us(1500);
+    float strecke_anfang, strecke_ende, size_parking_space, zeit_anfang, zeit_ende, zeit_parking_space;
+    timer.start();
     
-    ledTwo =1;
-    ledThree = 1;
-    steerPWM.pulsewidth_us(1500);
-    //float strecke_anfang, strecke_ende, size_parking_space;
+    // Konfiguration AMF-IMU
+    // [0]: Conversation Factor
+    // [1]: Sensor Position X
+    // [2]: Sensor Position Y
+    // [3]: Sensor Position Angle
+    //float configData[4] = {0.002751114f, 167.0f, 0.0f, 271.5f};
+    //supportSystem->writeImuConfig(configData);
+
+/*    while(true){
+    
+        ledTwo = !ledTwo;
+        IMU_registerDataBuffer = supportSystem->getImuRegisterDataBuffer();
+        
+        
+        serialMinnow.printf("X %f\n\r", IMU_registerDataBuffer->distanceXRegister); 
+        serialMinnow.printf("S %f\n\r", IMU_registerDataBuffer->squal);     
+        serialMinnow.printf("Uptime %d\n\r", supportSystem->readMaintenanceUptimeMillis());
+        serialMinnow.printf("Error: %d\n\r", supportSystem->getTransmissionErrorCount());
+        
+        wait(0.100);
+    }
+*/
 
     while(true){
-        //IMU_registerDataBuffer = supportSystem->getImuRegisterDataBuffer();
+        ledTwo = !ledTwo;
+        IMU_registerDataBuffer = supportSystem->getImuRegisterDataBuffer();
+        
         if(buttonFour == 1){
-        ledTwo = 0;
+            
+            drivePWM.pulsewidth_us(1700);
+            while(true){
+                if(redlight == 1){
+                    ledThree = 1;
+                
+                    /*do{
+                    
+                    }while(redlight == 1);
+                    strecke_anfang = IMU_registerDataBuffer->distanceXRegister;
+                    ledThree = 1;
+                    do{
+
+                    }while(redlight == 0);*/
+                    
+                    /*strecke_anfang = IMU_registerDataBuffer->distanceXRegister;
+                    wait_ms(2000);
+                    strecke_ende = IMU_registerDataBuffer->distanceXRegister;
+                    size_parking_space = strecke_ende - strecke_anfang;
+                    serialMinnow.printf("%f, ", IMU_registerDataBuffer->distanceXRegister); 
+                    serialMinnow.printf("%f\r\n", size_parking_space);*/ 
+                    
+                    do{
+                    
+                    }while(redlight == 1);
+                    zeit_anfang = timer.read_ms();
+                    do{
+
+                    }while(redlight == 0);
+                    zeit_ende = timer.read_ms();
+                    zeit_parking_space = zeit_ende - zeit_anfang;
+                    serialMinnow.printf("Differenz: %f\r\n", zeit_parking_space);
+                }
+                if(zeit_parking_space < 1350 && zeit_parking_space > 1100){
+                    
+                    drivePWM.pulsewidth_us(1700);   //Stück nach vorne fahren
+                    wait_ms(200);
+                    drivePWM.pulsewidth_us(1500);   //Stehen
+                    wait_ms(200);
+                    drivePWM.pulsewidth_us(1325);   //1. Einschlag
+                    steerPWM.pulsewidth_us(2000);
+                    wait_ms(970);
+                    steerPWM.pulsewidth_us(1000);   //2. Einschlag
+                    wait_ms(950);
+                    drivePWM.pulsewidth_us(1700);   //Korrektur nach vorne
+                    steerPWM.pulsewidth_us(1700);
+                    wait_ms(420);
+                    drivePWM.pulsewidth_us(1500);   //Ende
+                    steerPWM.pulsewidth_us(1500);
+                    break;
+                }
+            }
+        }
+        if(buttonOne == 1){
+            ledTwo = 0;
+            ledThree = 0;
+            drivePWM.pulsewidth_us(1500);
+        }
+        wait_ms(100);
+    }
+}
+
+
+void hearbeat_thread(void const *args) {
+    while(true) {
+        heartbeatLED = !heartbeatLED;
+        Thread::wait(100);
+    }
+}
+
+
+        
 //-------   1   ----------------
   /*      drivePWM.pulsewidth_us(1700); //Vorwärts
         wait_ms(2500);
@@ -50,7 +149,7 @@
         wait_ms(420);
         drivePWM.pulsewidth_us(1500);   //Ende
         steerPWM.pulsewidth_us(1500);
-        ledTwo = 1;*/
+        ledTwo = 1;
 //-------   2   ----------------        
         drivePWM.pulsewidth_us(1800); //Vorwärts
         wait_ms(825);
@@ -66,42 +165,4 @@
         wait_ms(420);
         drivePWM.pulsewidth_us(1500);   //Ende
         steerPWM.pulsewidth_us(1500);       
-        
-        
-        }
-        if(buttonOne == 1){
-            ledTwo = 0;
-            ledThree = 0;
-            drivePWM.pulsewidth_us(1500);
-        }
-    }
-        
-        
-        /*if(buttonFour == 1){
-            drivePWM.pulsewidth_us(1700);
-        
-            if(redlight == 1){
-                ledTwo = 1;
-                
-                do{
-                ledThree = 1;
-                }while(redlight == 1);
-                strecke_anfang = IMU_registerDataBuffer->distanceXRegister;
-                do{
-                    ledThree = 0;
-                }while(redlight == 0);
-                strecke_ende = IMU_registerDataBuffer->distanceXRegister;
-                size_parking_space = strecke_ende - strecke_anfang;
-                
-                }
-        }
-        */
-}
-
-
-void hearbeat_thread(void const *args) {
-    while(true) {
-        heartbeatLED = !heartbeatLED;
-        Thread::wait(100);
-    }
-}
+        */
\ No newline at end of file