Revision 19:4fbffc755ed7, committed 2018-03-23

Comitter:

asobhy

Date:

Fri Mar 23 19:03:35 2018 +0000

Parent:

18:db6d9fc1ebd0

Child:

20:9118203f7c9c

Commit message:

increasing sampling rate manual control

Changed in this revision

--- 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*********************************/
 

--- a/main.cpp	Tue Mar 13 22:14:24 2018 +0000
+++ b/main.cpp	Fri Mar 23 19:03:35 2018 +0000
@@ -45,6 +45,6 @@
         }
         
         consoleUI(); 
-        Thread::wait(100); // Go to sleep for 500 ms  
+        Thread::wait(20); // Go to sleep for 20 ms  
     }
 }

--- a/ui.cpp	Tue Mar 13 22:14:24 2018 +0000
+++ b/ui.cpp	Fri Mar 23 19:03:35 2018 +0000
@@ -186,12 +186,10 @@
         // by incrementing u
         else if(x == 'w') {
             setpointR_mutex.lock();
-            if ( setpointR < 560 ) 
-            {
-                //setpoint = setpoint + SPEED_STEP;
-                setpointR = 280;
-            }
+            //setpoint = setpoint + SPEED_STEP;
+            setpointR = 25;
             setpointR_mutex.unlock();
+            
             bluetooth.printf("\r\n %5d", setpointR);
         }
 
@@ -200,43 +198,39 @@
         else if(x == 's') {
 
             setpointR_mutex.lock();
-            if (setpointR > -560) 
-            {
-                setpointR = -280;
-                //setpoint = setpoint - SPEED_STEP;
-            }
-                
+
+            setpointR = -25;
+            //setpoint = setpoint - SPEED_STEP;
+
             setpointR_mutex.unlock();
 
             // display speed
-            bluetooth.printf("\r\n %5d", setpointR);
+            //bluetooth.printf("\r\n %5d", setpointR);
         }
 
 /******************************LEFT MOTOR**************************************/        
         else if (x=='i')
         {
             setpointL_mutex.lock();
-            if ( setpointL < 560 ) 
-            {
+
+ 
                 //setpoint = setpoint + SPEED_STEP;
-                setpointL = 280;
-            }
+                setpointL = 25;
+
             setpointL_mutex.unlock();
-            bluetooth.printf("\r\n %5d", setpointL);
+            //bluetooth.printf("\r\n %5d", setpointL);
         }
         else if (x=='k')
         {
             setpointL_mutex.lock();
-            if (setpointL > -560) 
-            {
-                setpointL = -280;
-                //setpoint = setpoint - SPEED_STEP;
-            }
-                
+
+            setpointL = -25;
+            //setpoint = setpoint - SPEED_STEP;
+  
             setpointL_mutex.unlock();
 
             // display speed
-            bluetooth.printf("\r\n %5d", setpointL);
+            //bluetooth.printf("\r\n %5d", setpointL);
         }
 /******************************END MOTOR SETPOINT******************************/
         
@@ -286,7 +280,7 @@
         // by incrementing u
         else if(x == 'w') {
             mutexSetpoint.lock();
-            Setpoint = 100;
+            Setpoint = -25;
             mutexSetpoint.unlock();
         
         }
@@ -295,7 +289,7 @@
         // by decrementing u
         else if(x == 's') {
             mutexSetpoint.lock();
-            Setpoint = -100;
+            Setpoint = 25;
             mutexSetpoint.unlock();
         
         }
@@ -304,13 +298,13 @@
         else if (x=='a')
         {
             mutexSetpoint.lock();
-            SteeringError = 300;
+            SteeringError = 75;
             mutexSetpoint.unlock();
         }
         else if (x=='d')
         {
             mutexSetpoint.lock();
-            SteeringError = -300;
+            SteeringError = -75;
             mutexSetpoint.unlock();
         }        
         // error wrong input
@@ -325,4 +319,6 @@
            
     }
     
+    //bluetooth.printf("\r\nPos: %d, dP: %d, dT: %d, Kp: %f, Ki: %f, vel: %d, e: %d", position, dPosition, dTime, Kp, Ki, vel, e);
+    
 }