Dependencies:   mbed Motordriver QEI

motor.h

Committer:
Keisuke_Fujii
Date:
2011-11-10
Revision:
1:19d647492936

File content as of revision 1:19d647492936:

void qei( void )
{
    char key;
    double d1 = 0.5, d2 = 0.5;
    device.printf("\nf:forward, b:back, r:turn_right\n"
                  "l:turn_left, a:duty+=0.01, m:duty-=0.01\n");
    
    while( 1 )
    {
        /*device.printf( "Right_Pulses : %d\n", r_enc.getPulses() );
        device.printf( "Right_Rotate : %f\n", (double)r_enc.getRevolutions() );
        device.printf( " Left_Pulses : %d\n", l_enc.getPulses() );
        device.printf( " Left_Rotate : %f\n", (double)l_enc.getRevolutions() );*/
        key = device.getc();
        switch( key )
        {
            case 'f':
                move_f( d1, d2 );
                break;
            case 'b':
                move_b( d1, d2 );
                break;
            case 'r':
                turn_r( d1, d2 );
                break;
            case 'l':
                turn_l( d1, d2 );
                break;
            case 's':
                stop();
                break;
            case 'a':
                d1 += 0.01;
                d2 += 0.01;
                break;
            case 'm':
                d1 -= 0.01;
                d2 -= 0.01;
                break;
            default:
                break;
        }
        device.printf("duty1 = %1.2f, duty2 = %1.2f\r", d1, d2);
        wait_ms( 10 );
    }
}

void move_f( double dr, double dl )
{
    right_P.write( 1 );
    right_N.write( 0 );
    left_P.write( 0 );
    left_N.write( 1 );
    right_pwm.write( dr );
    left_pwm.write( dl );
    /*right_pwm.pulsewidth_us( d );
    left_pwm.pulsewidth_us( d );*/
}

void move_b( double dr, double dl )
{
    right_P.write( 0 );
    right_N.write( 1 );
    left_P.write( 1 );
    left_N.write( 0 );
    right_pwm.write( dr );
    left_pwm.write( dl );
    /*right_pwm.pulsewidth_us( d );
    left_pwm.pulsewidth_us( d );*/
}

void turn_r( double dr, double dl )
{
    right_P.write( 1 );
    right_N.write( 0 );
    left_P.write( 1 );
    left_N.write( 0 );
    right_pwm.write( dr );
    left_pwm.write( dl );
    /*right_pwm.pulsewidth_us( d );
    left_pwm.pulsewidth_us( d );*/
}

void turn_l( double dr, double dl )
{
    right_P.write( 0 );
    right_N.write( 1 );
    left_P.write( 0 );
    left_N.write( 1 );
    right_pwm.write( dr );
    left_pwm.write( dl );
    /*right_pwm.pulsewidth_us( d );
    left_pwm.pulsewidth_us( d );*/
}

void stop( void )
{
    right_P.write( 0 );
    right_N.write( 0 );
    left_P.write( 0 );
    left_N.write( 0 );
    right_pwm.write( 0 );
    left_pwm.write( 0 );
}