Diff: main.cpp

Revision:

1:1adf5dfcc7bb

Parent:

0:9a3e7847a4be

Child:

2:4ccdc62022c2

--- a/main.cpp	Mon Apr 09 19:26:46 2018 +0000
+++ b/main.cpp	Tue Apr 10 12:24:23 2018 +0000
@@ -3,19 +3,23 @@
 #include "EncoderCounter.h"
 #include "Controller.h"
 #include "IRSensor.h"
+#include "Motion.h"
 
-//Sensor I/O:
+//User Button
+    DigitalIn button(PC_13);
+
+//Sensors:
     
     AnalogIn lineSensor(PA_0);
-    AnalogIn distance1(PA_1);
-    AnalogIn distance2(PA_4);
-    AnalogIn distance3(PB_0);
+    AnalogIn distanceL(PA_1);
+    AnalogIn distanceC(PA_4);
+    AnalogIn distanceR(PB_0);
 
-    IRSensor irSensorL (distance1);
-    IRSensor irSensorC (distance2);
-    IRSensor irSensorR (distance3);
+    IRSensor irSensorL (distanceL);
+    IRSensor irSensorC (distanceC);
+    IRSensor irSensorR (distanceR);
 
-//Motor I/O:
+//Motors:
     DigitalOut myled(LED1);
     
     DigitalOut enableMotorDriver(PB_2);
@@ -30,42 +34,105 @@
     EncoderCounter counterRight(PA_6, PC_7);
     
     Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
+    
+    Motion motion(controller, counterLeft, counterRight, irSensorL, irSensorC, irSensorR);
 
 int main() {
     
-/* Micromouse Test */
+/** Micromouse Test 
+
+    für den jeweiligen Test 0 im while Argument auf 1 setzen
+*/
+    
+    //User Button einlesen: start Signal
+    int start;
+    short countsLeft = 0;
+    short countsRight = 0;
+    
+    if (button.read() || start != 1) start = 1;
+    else if(button.read() || start == 1) start = 0;
 
 
-    //Sensoren
-    while(1) {
+    /*
+    Sensoren Test
+    */
+    while(start && 1) {
         
         float distanceL = irSensorL.read();
         float distanceC = irSensorC.read();
         float distanceR = irSensorR.read();
-        printf("Rechts: %f  Mitte: %f  Links: %f  Line: %f\n", 
-                distanceL, distanceC, distanceR, lineSensor);
+        printf("Rechts: %f  Mitte: %f  Links: %f\n", distanceL, distanceC, distanceR);
         
         wait(0.5f);
     }
     
-    //Motoren
-    while(0) {
+    /*
+    Motoren Test: Gerade Bewegung
+    */
+    while(start && 0) {
         
         controller.setDesiredSpeedLeft(50.0f); //Drehzahl in [rpm]
-        controller.setDesiredSpeedRIght(50.0f);
+        controller.setDesiredSpeedRight(50.0f);
         enableMotorDriver = 1; //Schaltet  den Leistungstreiber ein
             while(1) {
         
                 myled =! myled;
                 wait(0.1f); //200 ms
-            }
+                
+                countsLeft = counterLeft.read();
+                countsRight = counterRight.read();
+                printf("countsLeft: %hd countsRight: %hd\n", countsLeft, countsRight);
+            }     
+    }
+    
+    /*
+    Motoren Test: 180° Rotation
+    */
+        
+    while(start && 0) {
+        
+        while(countsLeft  < 200 || countsRight > -200) {
+            
+            countsLeft = counterLeft.read();
+            countsRight = counterRight.read();
+        
+            controller.setDesiredSpeedLeft(50.0f);
+            controller.setDesiredSpeedRight(-50.0f);
+        }
+        
+        controller.setDesiredSpeedLeft(0.0f);
+        controller.setDesiredSpeedRight(0.0f); 
         
     }
 
-}
+
+/* Benötigte Ticks für 180* Rotation am Ort:
 
-
+    RotationsDistanz Rd= DistanzRäder*pi/2
+    
+    UmfangRad = Drad*pi
+    
+    mm pro Count = UmfangRad/Counts pro Drehung
+    
+    Counts = Rd/mm pro Count
     
+    => Counts pro Drehung rausfinden */
+    while(start && 0) {
+        
+        while(countsLeft  < 100 || countsRight < 100) {
+            
+            countsLeft = counterLeft.read();
+            countsRight = counterRight.read();
+        
+            controller.setDesiredSpeedLeft(50.0f);
+            controller.setDesiredSpeedRight(50.0f);
+        }
+        
+        controller.setDesiredSpeedLeft(0.0f);
+        controller.setDesiredSpeedRight(0.0f); 
+        
+    }
+}