Diff: bt_shell/shell/motor_fnt.h

Revision:

0:964eb6a2ef00

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/bt_shell/shell/motor_fnt.h	Wed Mar 18 22:23:48 2015 +0000
@@ -0,0 +1,117 @@
+void move_fnt(int argc, char **argv)
+{
+    if(echo)
+        bt.printf("move command called\r\n");
+    
+    if(argc == 1){
+        if(echo){
+            bt.lock();
+            bt.printf("Usage: move [L_speed] [R_speed] [duration_ms]\r\n");
+            bt.unlock();
+        }
+    }else if(argc == 2){
+        if(echo){
+            bt.lock();
+            bt.printf("\r\nInvalid Parameters\r\n");
+            bt.unlock();
+        }
+    }else{
+        
+        int param[3];
+            
+        for(int i = 0; i < 2; i++)
+        {
+            param[i] = tinysh_atoxi(argv[i+1]);
+    
+            if(param[i] > 100)
+                param[i] = 100;
+            else if (param[i] < -100)
+                param[i] = -100;
+        }
+        
+        if(echo){
+            bt.lock();
+            bt.printf("Setting motor speeds Left = %d  Right = %d\r\n" ,param[0],param[1]);
+            bt.unlock();
+        }
+            
+        if(argc == 4)
+        {
+            param[2] = tinysh_atoxi(argv[3]);
+            if( param[2] > 0)
+            {
+                if(echo){
+                    bt.lock();
+                    bt.printf("moving duration: %sms\r\n", argv[3]);
+                    bt.unlock();
+                }
+
+                moveMotors(param[0],param[1],param[2]);
+            }
+            else
+            {
+                if(echo){
+                    bt.lock();                
+                    bt.printf("invalid duration: %sms\r\n", argv[3]);    
+                    bt.unlock();
+                }                
+            }
+        }
+        else
+        {   
+            *motors.Left = param[0];
+            *motors.Right = param[1];
+        }
+    }
+}
+
+//////////////////////////////////////////////////////////////////////////
+void stop_motors_fnt(int argc, char **argv)
+{
+    if(echo){
+        bt.lock();
+        bt.printf("motors stopped\r\n");
+        bt.unlock();
+    }
+    *motors.Left = 0;
+    *motors.Right = 0;
+}
+
+///////////////////////////////////////////////////////////////////////
+void flip_motors_fnt(int argc, char **argv)
+{
+    if(echo){
+        bt.lock();
+        bt.printf("L/R motors swapped\r\n");
+        bt.unlock();
+    }
+    motors.flip();    
+}
+
+void motor_scale_fnt(int argc, char **argv)
+{
+    if(echo){
+        bt.lock();
+        bt.printf("motor scale command called\r\n");
+        bt.unlock();
+    }
+    
+    if(argc == 2){
+        float val = atof(argv[1]);
+        
+        motors.Left->scale = val;
+        motors.Right->scale = val;                 
+        
+        if(echo){
+            bt.lock();
+            bt.printf("Scale set to %s\r\n", argv[1]); 
+            bt.unlock();
+        }
+    }else{
+        if(echo){
+            bt.lock();
+            bt.printf("Usage: scale [value]\r\n");
+            bt.unlock();
+        }
+    }
+}
\ No newline at end of file