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

Dependencies:   mbed

bt_shell/shell/motor_fnt.h

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

File content as of revision 0:964eb6a2ef00:

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();
        }
    }
}