ObjectFollower

Dependencies:   TPixy-Interface

Fork of PlayBack by ECE4333 - 2018 - Ahmed & Brandon

Revision:
19:4fbffc755ed7
Parent:
18:db6d9fc1ebd0
Child:
20:9118203f7c9c
--- 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);
+    
 }