File content as of revision 0:964eb6a2ef00:

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;        
        */
}