Diff: main.cpp

Revision:

5:47262622a9bf

Parent:

4:e74c06e43485

Child:

6:1f084e6f7e80

--- a/main.cpp	Wed Apr 11 15:26:03 2018 +0000
+++ b/main.cpp	Mon Apr 16 12:44:48 2018 +0000
@@ -5,20 +5,21 @@
 #include "IRSensor.h"
 #include "Motion.h"
 
-    Serial pc (SERIAL_TX, SERIAL_RX);
 //User Button
-    DigitalIn button(PC_13);
+    InterruptIn button(USER_BUTTON);
 
 //Sensors:
     
-    AnalogIn lineSensor(PA_0);
-    AnalogIn distanceL(PA_1);
-    AnalogIn distanceC(PA_4);
-    AnalogIn distanceR(PB_0);
+    AnalogIn lineSensor(PA_4);
+    AnalogIn distance2(PC_3);
+    AnalogIn distance4(PB_1);
+    AnalogIn distance1(PC_2);
+    AnalogIn distance3(PC_5);
 
-    IRSensor irSensorL (distanceL);
-    IRSensor irSensorC (distanceC);
-    IRSensor irSensorR (distanceR);
+    IRSensor irSensorL (distance2);
+    IRSensor irSensorC (distance4);
+    IRSensor irSensorR (distance1);
+    IRSensor irSensorB (distance3);
 
 //Motors:
     DigitalOut myled(LED1);
@@ -28,19 +29,38 @@
     DigitalIn motorDriverFault(PB_14);
     DigitalIn motorDriverWarning(PB_15);
     
-    PwmOut pwmLeft(PA_8);
-    PwmOut pwmRight(PA_10);
+    PwmOut pwmRight(PA_8);
+    PwmOut pwmLeft(PA_10);
     
-    EncoderCounter counterLeft(PB_6, PB_7);
-    EncoderCounter counterRight(PA_1, PA_0);
+    EncoderCounter counterRight(PB_6, PB_7);
+    EncoderCounter counterLeft(PA_0, PA_1);
+    
     
     Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
     
-    Motion motion(controller, counterLeft, counterRight, irSensorL, irSensorC, irSensorR);
+    Motion motion(controller, counterLeft, counterRight, irSensorL, irSensorC, irSensorR, enableMotorDriver);
     
-    int start = 0;
+//------------------------------------------------------------------------------
+    
+    volatile int start = 0;
     short countsLeft = 0;
     short countsRight = 0;
+    short countsLeftOld = 0;
+    short countsRightOld = 0;
+    //den jeweiligen Test auf 1 setzen
+    int testSensor = 0;
+    int testMotion = 0;
+    int testRotation = 0;
+    int testCounts = 0;
+    int testClass = 0;
+    
+//------------------------------------------------------------------------------
+
+//User button toggle
+void press() {
+    start = !start;
+}
+    
     
 //------------------------------------------------------------------------------
 
@@ -50,15 +70,17 @@
     Micromouse Test 
 */
     
-    //den jeweiligen Test auf 1 setzen
-    int testSensor = 0;
-    int testMotion = 0;
-    int testRotation = 0;
-    int testCounts = 0;
+    
+while(1) {
+    
+    button.fall(&press); //User button einlesen
     
-    //User Button einlesen: start Signal
-    if (button.read() && start != 1) start = 1;
-    else if(button.read() && start == 1) start = 0;
+    while(0) {
+        myled = 1;
+        printf("Hai\n");
+        if (start == 0) break;  
+    }
+    
 
 
     /*
@@ -67,85 +89,146 @@
     while(1) {
         
         
-        float distanceL = irSensorL.read();
-        float distanceC = irSensorC.read();
-        float distanceR = irSensorR.read();
-        pc.printf("Rechts: %f  Mitte: %f  Links: %f\n", distanceL, distanceC, distanceR);
+        float distanceL = irSensorL.readL();
+        float distanceC = irSensorC.readC();
+        float distanceR = irSensorR.readR();
+        float distanceB = irSensorB.readC();
+        printf("Links: %f  Mitte: %f  Rechts: %f  Hinten: %f  Line: %f\n", distanceL, distanceC, distanceR, distanceB, lineSensor.read());
         
-        wait(0.5f);
+        //wait(0.5f);
     }
     
     /*
     Motoren Test: Gerade Bewegung
     */
-    while(0) {
-        
-        controller.setDesiredSpeedLeft(50.0f); //Drehzahl in [rpm]
-        controller.setDesiredSpeedRight(-50.0f);
-        enableMotorDriver = 1; //Schaltet  den Leistungstreiber ein
-            while(1) {
+    while(start == 1 && testMotion == 1) {
         
-                myled =! myled;
-                wait(0.1f); //200 ms
-                
-                countsLeft = counterLeft.read();
-                countsRight = counterRight.read();
-                printf("countsLeft: %hd countsRight: %hd\n", countsLeft, countsRight);
-            }     
+        controller.setDesiredSpeedLeft(20.0f); //Drehzahl in [rpm]
+        controller.setDesiredSpeedRight(-20.0f);
+        enableMotorDriver = 1; //Schaltet  den Leistungstreiber ein
+                        
+        countsLeft = counterLeft.read();
+        countsRight = counterRight.read();
+        
+        printf("countsLeft: %hd countsRight: %hd\n", countsLeft, countsRight);
+        
+        if (start == 0) {
+            
+            motion.stop();
+        }
     }
     
     /*
     Motoren Test: 180° Rotation
     */
         
-    while(start && testRotation) {
-        
-        while(countsLeft  < 200 || countsRight > -200) {
+    while(start == 1 && testRotation == 1) {
+            
+            
+        while((countsLeft - countsLeftOld)  > -1614 || (countsRight - countsRightOld) > -1614) {
             
             countsLeft = counterLeft.read();
             countsRight = counterRight.read();
         
-            controller.setDesiredSpeedLeft(50.0f);
+            controller.setDesiredSpeedLeft(-50.0f);
             controller.setDesiredSpeedRight(-50.0f);
             enableMotorDriver = 1;
         }
         
+        motion.stop();
+        enableMotorDriver = 0;
+        printf("countsLeft: %hd countsRight: %hd\n", countsLeft, countsRight);
+        
+        if (start == 0) {
+           countsLeftOld = countsLeft - 100;
+           countsRightOld = countsRight - 100;
+            
+            break;
+        }
+    }
+
+
+/* Benötigte Ticks für 180* Rotation am Ort:
+
+    mm/count = 0.122
+    Dr = 124mm
+    Rr = 60mm
+    180°=> 1597 counts
+    90°=> 798 counts
+    
+        
+    RotationsDistanz Rd= DistanzRäder*pi/2 = 194.8
+    
+    UmfangRad = Drad*pi = 188.5
+    
+    
+    Counts = Rd/mm pro Count
+    
+    => Counts pro Drehung rausfinden */
+    while(start == 1 && testCounts == 1) {
+        
+        while(countsLeft  < 5000 || countsRight > -5000) {
+            
+            countsLeft = counterLeft.read();
+            countsRight = counterRight.read();
+        
+            controller.setDesiredSpeedLeft(20.0f);
+            controller.setDesiredSpeedRight(-20.0f);
+            enableMotorDriver = 1;
+        }
+        
         controller.setDesiredSpeedLeft(0.0f);
         controller.setDesiredSpeedRight(0.0f); 
         enableMotorDriver = 0;
         
     }
-
-
-/* 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 && testCounts) {
+    //Class test
+    if(start == 1 && testClass == 1) {
+        motion.rotate180();
+        counterLeft.reset();
+        counterRight.reset();
+        start = 0;
+        }
         
-        while(countsLeft  < 100 || countsRight < 100) {
-            
-            countsLeft = counterLeft.read();
-            countsRight = counterRight.read();
+    //Sensor calib
+    if(start == 1) {
+        int i;
+        myled = !myled;
+        for (i=0;i<5;i++) {
+            float distanceL = irSensorL.readL();
+            float distanceC = irSensorC.readC();
+            float distanceR = irSensorR.readR();
+            float distanceB = irSensorB.readC();
+            printf("Links: %f  Mitte: %f  Rechts: %f  Hinten: %f  Line: %f\n", distanceL, distanceC, distanceR, distanceB, lineSensor.read());
+       }
+        printf("------------------------\n");
+        start = 0;
         
-            controller.setDesiredSpeedLeft(20.0f);
-            controller.setDesiredSpeedRight(20.0f);
-            enableMotorDriver = 1;
+       /* while((countsLeft - countsLeftOld) > -82 || (countsRight - countsRightOld) < 82) {
+        
+        controller.setDesiredSpeedLeft(-10.0f); //Drehzahl in [rpm]
+        controller.setDesiredSpeedRight(10.0f);
+        enableMotorDriver = 1; //Schaltet  den Leistungstreiber ein
+                        
+        countsLeft = counterLeft.read();
+        countsRight = counterRight.read();
         }
         
         controller.setDesiredSpeedLeft(0.0f);
         controller.setDesiredSpeedRight(0.0f); 
         enableMotorDriver = 0;
         
+        float distanceL = irSensorB.read();
+        printf("Hinten: %f\n", distanceL);
+        
+        countsLeftOld = countsLeft;
+        countsRightOld = countsRight;
+        start = 0;*/
+        
     }
 }
+}