Code for our FYDP -only one IMU works right now -RTOS is working

Dependencies:   mbed

bt_shell/shell/remotecontrol_fnt.h

Committer:
majik
Date:
2015-03-18
Revision:
0:964eb6a2ef00

File content as of revision 0:964eb6a2ef00:

void remotecontrol_fnt(int argc, char **argv)
{
    int speed = 40;
    
    bt.lock();
    bt.printf("Remote Control Mode\r\n");
    bt.printf("Controls:\r\n");
    bt.printf("     wasd & qezc to move\r\n");
    bt.printf("     r/f for speed +/-\r\n");
    bt.printf("     enter key to exit\r\n");
    bt.unlock();
    
    if(argc == 2){
        speed = tinysh_atoxi(argv[1]);        
        if( speed <= 0 || speed > 100)
            speed = 100;
    }        
    
    int Lspeed = 0;
    int Rspeed = 0;

    char c = 0;
    
    while(c != '\r' && c != '\n')
    {
        bt.lock();
        
        if(bt.readable())
        {
        c = bt.getc();
        bt.unlock();
               
        switch ( c ) {
            case 'w':
                Lspeed = speed;
                Rspeed = speed;            
            break;
            case 'a':
                Lspeed = -speed;
                Rspeed = speed;
            break;
            case 's':
                Lspeed = -speed;
                Rspeed = -speed;
            break;
            case 'x':
                Lspeed = -speed;
                Rspeed = -speed;
            break;
            case 'd':
                Lspeed = speed;
                Rspeed = -speed;
            break;
            case 'q':
                Lspeed = 0;
                Rspeed = speed;
            break;
            case 'e':
                Lspeed = speed;
                Rspeed = 0;
            break;      
            case 'c':
                Lspeed = -speed;
                Rspeed = 0;
            break;       
            case 'z':
                Lspeed = 0;
                Rspeed = -speed;
            break;        
            case 'r':
                speed += 10;
                if(speed > 100)
                    speed = 100;
                bt.lock();
                bt.printf("speed = %d\r\n", speed);
                bt.unlock();
                Lspeed = 0;
                Rspeed = 0;
            break;  
            case 'f':
                speed -= 10;
                if(speed < 0)
                    speed = 0;
                bt.lock();
                bt.printf("speed = %d\r\n", speed);      
                bt.unlock();
                Lspeed = 0;
                Rspeed = 0;              
            break;         
            default:
                Lspeed = 0;
                Rspeed = 0;
            break;
        }
        
        moveMotors(Lspeed,Rspeed,100);
        
        }else
            bt.unlock();
            
        Thread::wait(25);   
    }
}