recon_fnt.h

00001 void reset_position_fnt(int argc, char **argv)
00002 {
00003     if(echo){
00004         bt.lock();    
00005         bt.printf("recon position reset to x = 0, y = 0, angle = 0\r\n");
00006         bt.unlock();
00007     }
00008     reckon.reset();    
00009 }
00010 
00011 int Vav = 40;
00012 float k = 3;
00013 
00014 void goto_coord_fnt(int argc, char **argv)
00015 {   
00016 //to fix for updated reckon
00017 /*
00018     if(argc != 3){
00019         if(echo){
00020             bt.lock();
00021             bt.printf("Usage: gotocoord [x] [y]\r\n");
00022             bt.unlock();
00023             return;
00024         }
00025     }    
00026     
00027     float x_dest = atof( argv[1] );
00028     float y_dest = atof( argv[2] );
00029     
00030     bt.lock();
00031     bt.printf("going to coordinates [%.2f, %.2f]...\r\n", x_dest, y_dest);
00032     bt.printf("Press enter to exit\r\n");
00033     bt.unlock();
00034     
00035     float return_angle_offset;
00036     int Vt, Lspeed, Rspeed;
00037     
00038     char c = 0;
00039     
00040     while(c != '\r' && c != '\n' && reckon.dest_distance(x_dest, y_dest) > 20)
00041     {   
00042         bt.lock();
00043         if(bt.readable())
00044         {
00045             c = bt.getc();
00046             bt.unlock();
00047         }else
00048             bt.unlock();
00049         
00050         return_angle_offset = (reckon.dest_angle(x_dest, y_dest) - reckon.angle)*180/3.14159265359;
00051         
00052         if(return_angle_offset > 90){
00053             Vt =(int)(-k*(180-return_angle_offset));
00054         }
00055         else if(return_angle_offset < -90)
00056             Vt =(int)(k*(180+return_angle_offset));
00057         else        
00058             Vt =(int)(k*return_angle_offset);
00059 
00060         Lspeed = - Vt;
00061         Rspeed = + Vt;
00062         
00063         if(abs(return_angle_offset)> 90)
00064         {
00065              Lspeed -= Vav;
00066              Rspeed -= Vav;
00067         }
00068         else
00069         {
00070             Lspeed += Vav;
00071             Rspeed += Vav;
00072         }
00073         
00074         if(Lspeed > Vav*2)
00075             Lspeed = Vav*2;
00076         else if(Lspeed < -Vav*2)
00077             Lspeed = -Vav*2;
00078             
00079         if(Rspeed > Vav*2)
00080             Rspeed = Vav*2;
00081         else if(Rspeed < -Vav*2)
00082             Rspeed = -Vav*2;
00083             
00084         *motors.Left = Lspeed;
00085         *motors.Right = Rspeed;
00086         
00087         Thread::wait(30);
00088     }    
00089         *motors.Left = 0;
00090         *motors.Right = 0;        
00091         */
00092 }