Revision 1:4808f55970e8, committed 2018-04-22

Comitter:

wengefa1

Date:

Sun Apr 22 09:55:40 2018 +0000

Parent:

0:e38b500d6e74

Commit message:

Grundfunktionen funktionieren

Changed in this revision

--- a/MotorDriver.cpp	Thu Apr 19 11:31:49 2018 +0000
+++ b/MotorDriver.cpp	Sun Apr 22 09:55:40 2018 +0000
@@ -16,6 +16,8 @@
 EncoderCounter counterLeft(PB_6, PB_7);
 EncoderCounter counterRight(PA_6, PC_7);
 
+DigitalOut myled(LED2);
+
 Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
 
 void resetCounter(void);
@@ -27,44 +29,26 @@
     counterRight.reset();
 }
 
-void driveOne(int NumbField, int Speed)
-{
-    switch(Speed) {
-        case 1:
+void driveOne(int NumbField, int SpeedMode)
 
-            controller.setDesiredSpeedLeft(-30.0f); // Drehzahl in [rpm]
-            controller.setDesiredSpeedRight(30.0f);
-            printf("SpeedSet\n");
-            //Fährt bis Anzahl umdrehungen erreicht sind
-            while(((abs(counterLeft.read())+abs(counterRight.read()))/2)<= 1000*NumbField) {
+{
+    int AnzahlZyklen = 0;
+    float distance;
+    float CorrectFactor;
+    float Speed = 0.0f;
 
-            }
-            printf("number erreicht");
-            stop();
-
+    switch(SpeedMode) {
+        case 1:
+            Speed = 20.0f;
             break;
 
 
         case 2:
-            controller.setDesiredSpeedLeft(-150.0f); // Drehzahl in [rpm] bei 31/1 enspricht es ca. 374/min
-            controller.setDesiredSpeedRight(150.0f);
-            //Fährt bis Anzahl umdrehungen erreicht sind
-            while(((abs(counterLeft.read())+abs(counterRight.read()))/2)<= 2000*NumbField) {
-
-            }
-            stop();
-
+            Speed = 40.0f;
             break;
 
         case 3:
-            controller.setDesiredSpeedLeft(-20.0f); // Drehzahl in [rpm] bei 31/1 enspricht es ca. 374/min
-            controller.setDesiredSpeedRight(20.0f);
-            //Fährt bis Anzahl umdrehungen erreicht sind
-            while(((abs(counterLeft.read())+abs(counterRight.read()))/2)<= 2000*NumbField) {
-                //do nothing
-            }
-            stop();
-
+            Speed = 80.0f;
             break;
 
 
@@ -72,6 +56,25 @@
 
             break;
     }
+
+    //Fährt bis Anzahl umdrehungen erreicht sind
+    while(((abs(counterLeft.read())+abs(counterRight.read()))/2)<= 1000*NumbField) {
+        distance = readSensorValue(1);      //Distanz zur linken Wand wird gemessen
+        AnzahlZyklen = AnzahlZyklen+1;      //Anzahl Regelvorgänge werden gezählt
+        distance = distance - 7.8f;         //Distanz zwischen Position und Ideallinie wird berechnet
+        CorrectFactor = distance*0.8f;      //P Faktor des P-Reglers
+
+        //printf("%f\n",CorrectFactor);
+        if(abs(CorrectFactor) <= 5.0f) {    //erkennt Wand oder Lücke auf linker Seite
+            controller.setDesiredSpeedLeft((Speed * (-1.0f)) - CorrectFactor); //Bei Wand wird korrigiert
+            controller.setDesiredSpeedRight(Speed - CorrectFactor);
+        } else {
+            controller.setDesiredSpeedLeft(Speed * (-1.0f));                 //Bei Lücke wird nach Counter gefahren
+            controller.setDesiredSpeedRight(Speed);
+        }
+    }
+    stop();
+    printf("Das Programm wurde: %d durchlaufen", AnzahlZyklen);
 }
 
 int turnRight(int direction)
@@ -129,17 +132,3 @@
     stop();
 }
 
-void orientate(void)
-{
-    while(ReadSensorValue(1) > ReadSensorValue(3))
-    {
-        driveDist(1,1);
-    }
-    while(ReadSensorValue(1) < ReadSensorValue(3))
-    {
-        driveDist(1,-1);
-    }
-    //Differenz Sensor zu Drehpunkt vorwärts fahren
-    
-}
-

--- a/MotorDriver.h	Thu Apr 19 11:31:49 2018 +0000
+++ b/MotorDriver.h	Sun Apr 22 09:55:40 2018 +0000
@@ -5,7 +5,6 @@
  void driveOne(int NumbField, int Speed);
  void startup(void);
  void stop(void);
- void orientate(void);
  int turnRight(int direction);
  int turnLeft(int direction);
  

--- a/ReadFinalLine.cpp	Thu Apr 19 11:31:49 2018 +0000
+++ b/ReadFinalLine.cpp	Sun Apr 22 09:55:40 2018 +0000
@@ -3,7 +3,7 @@
 AnalogIn LineSens1(PC_0);
 AnalogIn LineSens2(PC_1);
 
-int ReadFinalLine(void)
+int readFinalLine(void)
 {
     int SensorPos = 0;
     float MessureValue[2];

--- a/ReadFinalLine.h	Thu Apr 19 11:31:49 2018 +0000
+++ b/ReadFinalLine.h	Sun Apr 22 09:55:40 2018 +0000
@@ -1,5 +1,5 @@
 #ifndef ReadFinalLine_H
 #define ReadFinalLine_H
 
-int ReadFinalLine(void);   /* Funktionsdeklaration */
+int readFinalLine(void);   /* Funktionsdeklaration */
 #endif
\ No newline at end of file

--- a/ReadSensor.cpp	Thu Apr 19 11:31:49 2018 +0000
+++ b/ReadSensor.cpp	Sun Apr 22 09:55:40 2018 +0000
@@ -14,11 +14,6 @@
 HCSR04 sensor_Front(EchoF, TriggerF);
 HCSR04 sensor_Right(EchoR, TriggerR);
 
-//PwmOut& pwmLeft
-
-//float sampleTime = 0.5f;
-//PT1 filter(1, 2, sampleTime);
-//Ticker ticker_sens;
 float distance;
 float filteredDistance;
 int Glob_Position;
@@ -43,7 +38,7 @@
     }
 }
 
-int ReadSensor(int Position)
+int readSensor(int Position)
 {
     Glob_Position = Position;
     //PT1 filter(1, 2, sampleTime);
@@ -95,7 +90,7 @@
     }
 }
 
-float ReadSensorValue(int Position)
+float readSensorValue(int Position)
 {
     Glob_Position = Position;
     float distancetot = 0;
@@ -105,7 +100,7 @@
     sensor_Front.setRanges(2,400);
     sensor_Right.setRanges(2, 400);
     while(true) {
-        for(i = 0; i < 3; i++) {
+        for(i = 0; i < 2; i++) {
             switch(Position) {
                 case 1:
                     while(!sensor_Left.isNewDataReady()) {
@@ -135,7 +130,8 @@
             distancetot = distancetot + distance;
         }
         //ticker_sens.detach();
-        distancetot = distancetot / i;
+        distancetot = distancetot / (i);
+        printf("%f\n",distancetot);
         return distancetot;
         //filter.in(distance);
         //filteredDistance = filter.out();

--- a/ReadSensor.h	Thu Apr 19 11:31:49 2018 +0000
+++ b/ReadSensor.h	Sun Apr 22 09:55:40 2018 +0000
@@ -1,6 +1,6 @@
 #ifndef ReadSensor_H
 #define ReadSensor_H
 
-int ReadSensor(int Position);   /* Funktionsdeklaration */
-float ReadSensorValue(int Position);
+int readSensor(int Position);   /* Funktionsdeklaration */
+float readSensorValue(int Position);
 #endif
\ No newline at end of file

--- a/UltrasonicSensor/HCSR04.cpp	Thu Apr 19 11:31:49 2018 +0000
+++ b/UltrasonicSensor/HCSR04.cpp	Sun Apr 22 09:55:40 2018 +0000
@@ -1,7 +1,7 @@
 #include "mbed.h"
 #include "HCSR04.h"
 
-float sampleTime = 0.5f;
+float sampleTime = 0.05f;//Ehemals 0.5
 
 HCSR04::HCSR04(InterruptIn& echo, DigitalOut& trigger) : echo(echo), trigger(trigger) {
                                             
@@ -54,7 +54,7 @@
 
 void HCSR04::startMeasurement() {
     trigger = 1;
-    triggerTimeout.attach_us(this, &HCSR04::turnOffTrigger, 10);
+    triggerTimeout.attach_us(this, &HCSR04::turnOffTrigger, 10);//Ehemals 10
     echo.rise(this, &HCSR04::startTimer);
     newDataReady = false;
 }

--- a/main.cpp	Thu Apr 19 11:31:49 2018 +0000
+++ b/main.cpp	Sun Apr 22 09:55:40 2018 +0000
@@ -5,12 +5,23 @@
 #include "ReadSensor.h"
 
 
-DigitalOut myled(LED2);
+
 int answer; 
 
 int main()
 {
     startup();
-    while(1) {
+    /*while(1)
+    {
+    
+    printf("%f\n", ReadSensorValue(1));
+    }*/
+    
+    while(1){       
+    driveOne(3,2);
+    wait(1);
+    turnRight(1);
+    turnRight(1);
     }
 }
+