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