Diff: main.cpp

Revision:

7:3b3a2c158803

Parent:

6:9b34fadd6628

Child:

8:2ae54c6a9dc6

diff -r 9b34fadd6628 -r 3b3a2c158803 main.cpp
--- a/main.cpp	Mon Apr 26 08:33:06 2021 +0000
+++ b/main.cpp	Wed May 12 23:35:22 2021 +0000
@@ -5,6 +5,7 @@
 #include "EncoderCounter.h"
 #include "Servo.h"
 #include "SpeedController.h"
+#include "RangeFinder.h"
 
 using namespace std::chrono;
 
@@ -16,28 +17,38 @@
     Servo  servo_0(PA_6);
     
     
-   //Input 
-    DigitalIn pirrechts(PB_12);
-    DigitalIn pirlinks(PC_7);   //pc_6 puuty besetzt
-    DigitalIn pirvorne(PB_7);  //pc_8
-    DigitalIn pirhinten(PA_1); //pc-9
-    DigitalIn ultrarechts(PB_1);
-    DigitalIn ultralinks(PC_5); //immer eins
-    DigitalIn ultravorne(PC_3); 
-    DigitalIn ultrahinten(PC_2);// immer eins
+   //Infrarotsensor
+    DigitalIn pirlinks(PB_12);
+    DigitalIn pirhinten(PC_7);   //pc_6 puuty besetzt
+    DigitalIn pirrechts(PB_7);  //pc_8
+    DigitalIn pirvorne(PA_1); //pc-9
+    //im moment nich eingebaut
+  //  DigitalIn ultrarechts(PB_1);
+   // DigitalIn ultralinks(PB_13); //immer eins behoben
+   // DigitalIn ultravorne(PC_3); 
+    //DigitalIn ultrahinten(PA_9);// immer eins behoben
     
     //Output
     //Servo
     DigitalOut pumpe(PB_6);
     DigitalOut summer(PA_0);
     
- 
+    // Groove Ultrasonic Ranger V2.0
+    // https://ch.rs-online.com/web/p/entwicklungstools-sensorik/1743238/?cm_mmc=CH-PLA-DS3A-_-google-_-CSS_CH_DE_Raspberry_Pi_%26_Arduino_und_Entwicklungstools_Whoop-_-(CH:Whoop!)+Entwicklungstools+Sensorik-_-1743238&matchtype=&pla-306637898829&gclid=Cj0KCQjwpdqDBhCSARIsAEUJ0hOLQOOaw_2-Ob03u4YGwXthQPeSyjaazFqNuMkTIT8Ie18B1pD7P9AaAn18EALw_wcB&gclsrc=aw.ds 
+    /* create range finder object (ultra sonic distance sensor) */
+    RangeFinder ultrarechts(PB_1, 5782.0f, 0.02f, 17500); // 1/Ts_ms = 20 Hz parametrization
+    RangeFinder ultralinks(PB_13, 5782.0f, 0.02f, 17500); // 1/Ts_ms = 20 Hz parametrization
+    RangeFinder ultravorne(PC_3, 5782.0f, 0.02f, 17500); // 1/Ts_ms = 20 Hz parametrization
+    RangeFinder ultrahinten(PA_9, 5782.0f, 0.02f, 17500); // 1/Ts_ms = 20 Hz parametrization
+    
+
     /* input your stuff here */
     
 Timer          loop_timer;
 int            Ts_ms = 500;
-char           CZustand = 55;
+char           CZustand = 0;
 int            spritz = 3000; //spritzzeit
+float          fhelp = 0.0f;
 
 
 
@@ -77,12 +88,12 @@
     while (true) {
 
      loop_timer.reset();
-     if(user_button == 0)
+   /*  if(user_button == 0)
      {
          printf("nicht aktiv");
          CZustand = 0;
          
-         }
+         }*/
  
         /* ------------- start hacking ------------- -------------*/
     if(CZustand == 0) // Aufstarten
@@ -133,9 +144,11 @@
        led = 1;
        printf("rechts\n");
        
-       servo_0.SetPosition(1250); //1250
-       if(ultrarechts == 1)
+       servo_0.SetPosition(2100); //1250
+       if(ultrarechts.read_cm() < 200)
        {
+        fhelp = ultrarechts.read_cm();
+        printf("%f;",fhelp);
         CZustand = 6;
         }
         else{
@@ -149,9 +162,11 @@
     if(CZustand == 3)//links erkennt
     {
        printf("links\n");
-       servo_0.SetPosition(1750);
-        if(ultralinks == 1)
+       servo_0.SetPosition(1000);
+        if(ultralinks.read_cm() < 200)
        {
+        fhelp = ultralinks.read_cm();
+        printf("%3.3f;\r\n",fhelp);
         CZustand = 6;
         }
         else{
@@ -166,7 +181,7 @@
     {
         printf("vorne\n");
        servo_0.SetPosition(1500);
-       if(ultravorne == 1)
+       if(ultravorne.read_cm() < 200)
        {
         CZustand = 6;
         }
@@ -182,8 +197,8 @@
     if(CZustand == 5)//hinten erkennt
     {
       printf("hinten\n");
-      servo_0.SetPosition(2000);
-       if(ultrahinten == 1) 
+      servo_0.SetPosition(2450);
+       if(ultrahinten.read_cm() < 200) 
        {
         CZustand = 6;
         }