ManualControl

Dependencies:   TPixy-Interface

Fork of MbedOS_Robot by ECE4333 - 2018 - Ahmed & Brandon

Files at this revision

API Documentation at this revision

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

PiControlThread.cpp Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
ui.cpp Show annotated file Show diff for this revision Revisions of this file
--- 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);
+    
 }