Diff: PiControlThread.cpp

Revision:

19:4fbffc755ed7

Parent:

17:1184df616383

Child:

20:9118203f7c9c

--- a/PiControlThread.cpp	Tue Mar 13 22:14:24 2018 +0000
+++ b/PiControlThread.cpp	Fri Mar 23 19:03:35 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.0125);   // 12.5ms sampling period -> 80kHz
      
 }
 
@@ -107,8 +107,8 @@
         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, 140);
+        sensors.dp_left = SaturateValue(sensors.dp_left, 140);
         
         // Maximum velocity at dPostition = 560 is vel = 703
         velR = (float)((6135.92 * sensors.dp_right) / sensors.dt_right) ;
@@ -116,34 +116,6 @@
         // Maximum velocity at dPostition = 560 is vel = 703
         velL = (float)((6135.92 * sensors.dp_left) / sensors.dt_left) ;
         
-        /*********************Differential Start*******************************/
-        // Inputs are Speed Setpoint and Steering Setpoint
-        // The Inputs for the Steering are specified in the CameraThread
-        // and set at the center.
-        // The distance between the object and the image should be set to 1 meter   
-        // If distance decrease then speed should stop.
-        // If distance increase then speed should increase.
-        
-        // If object is moving away from the the robot increase robot speed
-        /*cameraData_mutex.lock();
-        if(DistanceError > 0)
-        {
-            // Proportional controller
-            Setpoint = (Kd*DistanceError);
-            Setpoint = SaturateValue(Setpoint,560);
-        }
-        // If object is at the set distance limit or less then do not move.
-        else if(DistanceError <= 0)
-        {
-            Setpoint = 0;
-        }
-        
-        // Decoupled drive  
-        setpointR = Setpoint + (Ks*SteeringError);
-        setpointL = Setpoint - (Ks*SteeringError);
-        cameraData_mutex.unlock();
-        */
-        
         /********************Manual Setpoint and Steering**********************/
         mutexSetpoint.lock();
         setpointR = Setpoint + (Ks*SteeringError);
@@ -151,8 +123,8 @@
         mutexSetpoint.unlock();    
             
         // Make sure our limit is not exceeded
-        setpointR = SaturateValue(setpointR, 560);
-        setpointL = SaturateValue(setpointL, 560);
+        setpointR = SaturateValue(setpointR, 140);
+        setpointL = SaturateValue(setpointL, 140);
        
         /*********************Differential End*********************************/