Diff: Drive.cpp

Revision:

1:d9e840c48b1e

Child:

2:592f01278db4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Drive.cpp	Sat Mar 31 16:45:57 2018 +0000
@@ -0,0 +1,51 @@
+#include <cmath>
+#include "Drive.h"
+
+
+
+using namespace std;
+
+const float Drive::DRIVINGSPEED = 1000.0f;//Fahrgeschwindigkeit
+const int Drive::DRIVINGCOUNTS = 10000;  //Entspricht Strecke von 20cm
+
+Drive::Drive(KontrastSensor& kontrastSensor, EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, float distanceRight, float distanceFront, float distanceLeftFront):
+    kontrastSensor(kontrastSensor),
+    counterLeft(counterLeft), 
+    counterRight(counterRight),
+    controller(controller)
+{
+    this->distanceRight = distanceRight;
+    this->distanceFront = distanceFront; 
+    this->distanceLeftFront = distanceLeftFront;
+}
+
+Drive::~Drive() {}
+
+
+void Drive::driving()
+{
+
+    int countsRight = counterRight.read();  //EncoderCounts auslesen  
+    int countsRight0 = countsRight;         //ReferenzCounts setzten
+    int countsLeft = counterLeft.read();
+    int countsLeft0 = countsLeft;
+    
+    printf("Los gehts\n");
+        
+    while((countsRight >= countsRight0 - DRIVINGCOUNTS) && (countsLeft <= countsLeft0 + DRIVINGCOUNTS)){
+    
+        //kontrastSensor.check();
+        controller.setDesiredSpeedRight(DRIVINGSPEED);
+        controller.setDesiredSpeedLeft(-DRIVINGSPEED);
+        countsRight = counterRight.read();
+        countsLeft = counterLeft.read();
+        //printf("CountsRight%d\n", countsRight);
+        //printf("CountsLeft%d\n", countsLeft);
+        
+        //parallelitaet und center pruefen!!!!!!!!!!!!
+        
+    }
+    controller.setDesiredSpeedRight(0.0f);
+    controller.setDesiredSpeedLeft(0.0f);
+
+}