Diff: PiControlThread.cpp

Revision:

24:e88753f090b8

Parent:

16:58ec2b891a25

Child:

25:f2a1bd6591fb

--- a/PiControlThread.cpp	Sun Mar 11 00:37:58 2018 +0000
+++ b/PiControlThread.cpp	Tue Apr 03 16:00:10 2018 +0000
@@ -86,7 +86,7 @@
 
     // Specify address of the PeriodicInt ISR as PiControllerISR, specify the interval
     // in seconds between interrupts, and start interrupt generation:
-    PeriodicInt.attach(&PeriodicInterruptISR, 0.05);   // 50ms sampling rate
+    PeriodicInt.attach(&PeriodicInterruptISR, 0.01);   // 10ms sampling rate
      
 }
 
@@ -107,14 +107,14 @@
         DE0_read(&sensors);
         
         // might not be needed
-        sensors.dp_right = SaturateValue(sensors.dp_right, 560);
-        sensors.dp_left = SaturateValue(sensors.dp_left, 560);
+        sensors.dp_right = SaturateValue(sensors.dp_right, 112);
+        sensors.dp_left = SaturateValue(sensors.dp_left, 112);
         
         // Maximum velocity at dPostition = 560 is vel = 703
-        velR = (float)((6135.92 * sensors.dp_right) / sensors.dt_right) ;
+        //velR = (float)((6135.92 * sensors.dp_right) / sensors.dt_right) ;
         
         // Maximum velocity at dPostition = 560 is vel = 703
-        velL = (float)((6135.92 * sensors.dp_left) / sensors.dt_left) ;
+        //velL = (float)((6135.92 * sensors.dp_left) / sensors.dt_left) ;
         
         /*********************Differential Start*******************************/
         // Inputs are Speed Setpoint and Steering Setpoint
@@ -130,7 +130,7 @@
         {
             // Proportional controller
             Setpoint = (Kd*DistanceError);
-            Setpoint = SaturateValue(Setpoint,560);
+            Setpoint = SaturateValue(Setpoint,112);
         }
         // If object is at the set distance limit or less then do not move.
         else if(DistanceError <= 0)
@@ -144,8 +144,8 @@
         cameraData_mutex.unlock();
         
         // Make sure our limit is not exceeded
-        setpointR = SaturateValue(setpointR, 560);
-        setpointL = SaturateValue(setpointL, 560);
+        setpointR = SaturateValue(setpointR, 112);
+        setpointL = SaturateValue(setpointL, 112);
        
         /*********************Differential End*********************************/