Diff: RobotController.cpp

Revision:

8:bfa4bf23522c

Parent:

5:77c6821ae418

Child:

11:531208aca075

--- a/RobotController.cpp	Mon Apr 15 19:26:56 2019 +0000
+++ b/RobotController.cpp	Mon Apr 22 23:55:45 2019 +0000
@@ -2,20 +2,49 @@
 
 RobotController::RobotController(PinName leftWheelPwm, PinName leftWheelFwd,
     PinName leftWheelRev, PinName rightWheelPwm, PinName rightWheelFwd,
-    PinName rightWheelRev, PinName leftEncoderIn, PinName rightEncoderIn) :
+    PinName rightWheelRev, PinName leftEncoder, PinName rightEncoder) :
     leftWheel(leftWheelPwm, leftWheelFwd, leftWheelRev),
     rightWheel(rightWheelPwm, rightWheelFwd, rightWheelRev),
-    leftEncoder(leftEncoderIn), rightEncoder(rightEncoderIn) {
+    leftEncoder(leftEncoder), rightEncoder(rightEncoder) {
+}
+
+void RobotController::detectObstacles() {
+    led = 0b0001;
+    for (int i = 0; i < 360; i++) {
+        leftEncoder.reset();
+        rightEncoder.reset();
+        leftWheel.speed(0.2);
+        rightWheel.speed(-0.2);
+        while((leftEncoder.read() < COUNTPERDEG) && (rightEncoder.read() < COUNTPERDEG));
+        leftWheel.speed(0);
+        rightWheel.speed(0);
+        obstacles[i] = 0; // TODO ping lidar
+    }
+    led = 0;
 }
 
-float* RobotController::obstacleDetection() {
-    
-}
-
-void RobotController::followTrajectory(float* trajectory) {
-    
-}
-
-void RobotController::toOrigin(float* trajectory) {
-    
+void RobotController::followTrajectory() {
+    led = 0b1000;
+    while(!pb);
+    for (int i = 0; i < trajectoryLength; i = i + 2) {
+        leftEncoder.reset();
+        rightEncoder.reset();
+        leftWheel.speed(0.2);
+        rightWheel.speed(-0.2);
+        int angleCount = (int)(trajectory[i]*COUNTPERDEG);
+        while((leftEncoder.read() < angleCount) && (rightEncoder.read() < angleCount));
+        leftWheel.speed(0);
+        rightWheel.speed(0);
+        leftEncoder.reset();
+        rightEncoder.reset();
+        int distanceCount = (int)(trajectory[i + 1]*COUNTPERIN);
+        leftWheel.speed(0.2);
+        rightWheel.speed(0.2);
+        pc.putc(distanceCount);
+        while((leftEncoder.read() < distanceCount) && (rightEncoder.read() < distanceCount));
+        leftWheel.speed(0);
+        rightWheel.speed(0);
+    }
+    delete []trajectory;
+    led = 0;
 }
\ No newline at end of file