Diff: bt_shell/shell/recon_fnt.h

Revision:

0:964eb6a2ef00

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/bt_shell/shell/recon_fnt.h	Wed Mar 18 22:23:48 2015 +0000
@@ -0,0 +1,92 @@
+void reset_position_fnt(int argc, char **argv)
+{
+    if(echo){
+        bt.lock();    
+        bt.printf("recon position reset to x = 0, y = 0, angle = 0\r\n");
+        bt.unlock();
+    }
+    reckon.reset();    
+}
+
+int Vav = 40;
+float k = 3;
+
+void goto_coord_fnt(int argc, char **argv)
+{   
+//to fix for updated reckon
+/*
+    if(argc != 3){
+        if(echo){
+            bt.lock();
+            bt.printf("Usage: gotocoord [x] [y]\r\n");
+            bt.unlock();
+            return;
+        }
+    }    
+    
+    float x_dest = atof( argv[1] );
+    float y_dest = atof( argv[2] );
+    
+    bt.lock();
+    bt.printf("going to coordinates [%.2f, %.2f]...\r\n", x_dest, y_dest);
+    bt.printf("Press enter to exit\r\n");
+    bt.unlock();
+    
+    float return_angle_offset;
+    int Vt, Lspeed, Rspeed;
+    
+    char c = 0;
+    
+    while(c != '\r' && c != '\n' && reckon.dest_distance(x_dest, y_dest) > 20)
+    {   
+        bt.lock();
+        if(bt.readable())
+        {
+            c = bt.getc();
+            bt.unlock();
+        }else
+            bt.unlock();
+        
+        return_angle_offset = (reckon.dest_angle(x_dest, y_dest) - reckon.angle)*180/3.14159265359;
+        
+        if(return_angle_offset > 90){
+            Vt =(int)(-k*(180-return_angle_offset));
+        }
+        else if(return_angle_offset < -90)
+            Vt =(int)(k*(180+return_angle_offset));
+        else        
+            Vt =(int)(k*return_angle_offset);
+
+        Lspeed = - Vt;
+        Rspeed = + Vt;
+        
+        if(abs(return_angle_offset)> 90)
+        {
+             Lspeed -= Vav;
+             Rspeed -= Vav;
+        }
+        else
+        {
+            Lspeed += Vav;
+            Rspeed += Vav;
+        }
+        
+        if(Lspeed > Vav*2)
+            Lspeed = Vav*2;
+        else if(Lspeed < -Vav*2)
+            Lspeed = -Vav*2;
+            
+        if(Rspeed > Vav*2)
+            Rspeed = Vav*2;
+        else if(Rspeed < -Vav*2)
+            Rspeed = -Vav*2;
+            
+        *motors.Left = Lspeed;
+        *motors.Right = Rspeed;
+        
+        Thread::wait(30);
+    }    
+        *motors.Left = 0;
+        *motors.Right = 0;        
+        */
+}
\ No newline at end of file