Diff: Control/drivecontrol.cpp

Revision:

5:d783972dce10

Parent:

4:73510c7fa316

Child:

6:1bcfda49e146

--- a/Control/drivecontrol.cpp	Sat May 13 21:27:32 2017 +0000
+++ b/Control/drivecontrol.cpp	Sat May 13 22:23:23 2017 +0000
@@ -1,5 +1,6 @@
 #include "drivecontrol.h"
 #include "Cell.h"
+//#include "mbed.h"
 #include "ir_sensor.h"
 #include "left_motor.h"
 #include "right_motor.h"
@@ -12,18 +13,20 @@
 LeftMotor * leftMotor;
 RightMotor * rightMotor;
 Cell * curr_cell;
-
 namespace pid_controller
 {
     float motor_speed;
     float LEFT_WALL_THRES = 0.89f, RIGHT_WALL_THRES = 0.91f;
     float SENSOR_ERROR_OFFSET = 0.02f;
     
+    float left_speed;
+    float right_speed;
     float error_p = 0.0f, old_error_p = 0.0f, error_d = 0.0f;
-    float total_error;
-    float P = 0.02f, D = 0.01f;
+  
+    float P = 1.0f, D = 1.0f;
     bool has_left_wall = leftIR.readIR() > LEFT_WALL_THRES;
     bool has_right_wall = rightIR.readIR() > RIGHT_WALL_THRES;
+    float total_error = 0.0f;
     
     
     void start_pid() {
@@ -46,8 +49,21 @@
         total_error = P * error_p + D*error_d;
         old_error_p = error_p;
         // if (DEBUG) { leftMotor->speed(0.15f); rightMotor->speed(0.85f); }
-        leftMotor -> speed(0.15f - total_error);
-        rightMotor -> speed(0.85f + total_error);
+        
+        //adjust to right
+        
+        if(total_error < 0.15f){
+            leftMotor -> speed(0.15f - total_error);
+            rightMotor -> speed(0.85f + total_error);
+        }
+        else{
+            leftMotor->speed(0.15f);
+            rightMotor->speed(0.85f);
+        }
+    }
+    
+    float get_error() {
+        return total_error;    
     }    
 }
 
@@ -105,11 +121,8 @@
 void DriveControl::drive_one_forward() {
     if (DEBUG) { leftMotor->speed(0.15f); rightMotor->speed(0.85f); }
     else {
-        int count = 0;
-        while (count != 10) {
-            pid_controller::start_pid();
-            count ++;
-        }
+        pid_controller::start_pid();
+        total_error = pid_controller::get_error();
     }
 }