Keisuke Fujii
Dependencies: mbed Motordriver QEI
motor.h@1:19d647492936, 2011-11-10 (annotated)
- Committer:
- Keisuke_Fujii
- Date:
- Thu Nov 10 13:29:43 2011 +0000
- Revision:
- 1:19d647492936
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| Keisuke_Fujii | 1:19d647492936 | 1 | void qei( void ) |
| Keisuke_Fujii | 1:19d647492936 | 2 | { |
| Keisuke_Fujii | 1:19d647492936 | 3 | char key; |
| Keisuke_Fujii | 1:19d647492936 | 4 | double d1 = 0.5, d2 = 0.5; |
| Keisuke_Fujii | 1:19d647492936 | 5 | device.printf("\nf:forward, b:back, r:turn_right\n" |
| Keisuke_Fujii | 1:19d647492936 | 6 | "l:turn_left, a:duty+=0.01, m:duty-=0.01\n"); |
| Keisuke_Fujii | 1:19d647492936 | 7 | |
| Keisuke_Fujii | 1:19d647492936 | 8 | while( 1 ) |
| Keisuke_Fujii | 1:19d647492936 | 9 | { |
| Keisuke_Fujii | 1:19d647492936 | 10 | /*device.printf( "Right_Pulses : %d\n", r_enc.getPulses() ); |
| Keisuke_Fujii | 1:19d647492936 | 11 | device.printf( "Right_Rotate : %f\n", (double)r_enc.getRevolutions() ); |
| Keisuke_Fujii | 1:19d647492936 | 12 | device.printf( " Left_Pulses : %d\n", l_enc.getPulses() ); |
| Keisuke_Fujii | 1:19d647492936 | 13 | device.printf( " Left_Rotate : %f\n", (double)l_enc.getRevolutions() );*/ |
| Keisuke_Fujii | 1:19d647492936 | 14 | key = device.getc(); |
| Keisuke_Fujii | 1:19d647492936 | 15 | switch( key ) |
| Keisuke_Fujii | 1:19d647492936 | 16 | { |
| Keisuke_Fujii | 1:19d647492936 | 17 | case 'f': |
| Keisuke_Fujii | 1:19d647492936 | 18 | move_f( d1, d2 ); |
| Keisuke_Fujii | 1:19d647492936 | 19 | break; |
| Keisuke_Fujii | 1:19d647492936 | 20 | case 'b': |
| Keisuke_Fujii | 1:19d647492936 | 21 | move_b( d1, d2 ); |
| Keisuke_Fujii | 1:19d647492936 | 22 | break; |
| Keisuke_Fujii | 1:19d647492936 | 23 | case 'r': |
| Keisuke_Fujii | 1:19d647492936 | 24 | turn_r( d1, d2 ); |
| Keisuke_Fujii | 1:19d647492936 | 25 | break; |
| Keisuke_Fujii | 1:19d647492936 | 26 | case 'l': |
| Keisuke_Fujii | 1:19d647492936 | 27 | turn_l( d1, d2 ); |
| Keisuke_Fujii | 1:19d647492936 | 28 | break; |
| Keisuke_Fujii | 1:19d647492936 | 29 | case 's': |
| Keisuke_Fujii | 1:19d647492936 | 30 | stop(); |
| Keisuke_Fujii | 1:19d647492936 | 31 | break; |
| Keisuke_Fujii | 1:19d647492936 | 32 | case 'a': |
| Keisuke_Fujii | 1:19d647492936 | 33 | d1 += 0.01; |
| Keisuke_Fujii | 1:19d647492936 | 34 | d2 += 0.01; |
| Keisuke_Fujii | 1:19d647492936 | 35 | break; |
| Keisuke_Fujii | 1:19d647492936 | 36 | case 'm': |
| Keisuke_Fujii | 1:19d647492936 | 37 | d1 -= 0.01; |
| Keisuke_Fujii | 1:19d647492936 | 38 | d2 -= 0.01; |
| Keisuke_Fujii | 1:19d647492936 | 39 | break; |
| Keisuke_Fujii | 1:19d647492936 | 40 | default: |
| Keisuke_Fujii | 1:19d647492936 | 41 | break; |
| Keisuke_Fujii | 1:19d647492936 | 42 | } |
| Keisuke_Fujii | 1:19d647492936 | 43 | device.printf("duty1 = %1.2f, duty2 = %1.2f\r", d1, d2); |
| Keisuke_Fujii | 1:19d647492936 | 44 | wait_ms( 10 ); |
| Keisuke_Fujii | 1:19d647492936 | 45 | } |
| Keisuke_Fujii | 1:19d647492936 | 46 | } |
| Keisuke_Fujii | 1:19d647492936 | 47 | |
| Keisuke_Fujii | 1:19d647492936 | 48 | void move_f( double dr, double dl ) |
| Keisuke_Fujii | 1:19d647492936 | 49 | { |
| Keisuke_Fujii | 1:19d647492936 | 50 | right_P.write( 1 ); |
| Keisuke_Fujii | 1:19d647492936 | 51 | right_N.write( 0 ); |
| Keisuke_Fujii | 1:19d647492936 | 52 | left_P.write( 0 ); |
| Keisuke_Fujii | 1:19d647492936 | 53 | left_N.write( 1 ); |
| Keisuke_Fujii | 1:19d647492936 | 54 | right_pwm.write( dr ); |
| Keisuke_Fujii | 1:19d647492936 | 55 | left_pwm.write( dl ); |
| Keisuke_Fujii | 1:19d647492936 | 56 | /*right_pwm.pulsewidth_us( d ); |
| Keisuke_Fujii | 1:19d647492936 | 57 | left_pwm.pulsewidth_us( d );*/ |
| Keisuke_Fujii | 1:19d647492936 | 58 | } |
| Keisuke_Fujii | 1:19d647492936 | 59 | |
| Keisuke_Fujii | 1:19d647492936 | 60 | void move_b( double dr, double dl ) |
| Keisuke_Fujii | 1:19d647492936 | 61 | { |
| Keisuke_Fujii | 1:19d647492936 | 62 | right_P.write( 0 ); |
| Keisuke_Fujii | 1:19d647492936 | 63 | right_N.write( 1 ); |
| Keisuke_Fujii | 1:19d647492936 | 64 | left_P.write( 1 ); |
| Keisuke_Fujii | 1:19d647492936 | 65 | left_N.write( 0 ); |
| Keisuke_Fujii | 1:19d647492936 | 66 | right_pwm.write( dr ); |
| Keisuke_Fujii | 1:19d647492936 | 67 | left_pwm.write( dl ); |
| Keisuke_Fujii | 1:19d647492936 | 68 | /*right_pwm.pulsewidth_us( d ); |
| Keisuke_Fujii | 1:19d647492936 | 69 | left_pwm.pulsewidth_us( d );*/ |
| Keisuke_Fujii | 1:19d647492936 | 70 | } |
| Keisuke_Fujii | 1:19d647492936 | 71 | |
| Keisuke_Fujii | 1:19d647492936 | 72 | void turn_r( double dr, double dl ) |
| Keisuke_Fujii | 1:19d647492936 | 73 | { |
| Keisuke_Fujii | 1:19d647492936 | 74 | right_P.write( 1 ); |
| Keisuke_Fujii | 1:19d647492936 | 75 | right_N.write( 0 ); |
| Keisuke_Fujii | 1:19d647492936 | 76 | left_P.write( 1 ); |
| Keisuke_Fujii | 1:19d647492936 | 77 | left_N.write( 0 ); |
| Keisuke_Fujii | 1:19d647492936 | 78 | right_pwm.write( dr ); |
| Keisuke_Fujii | 1:19d647492936 | 79 | left_pwm.write( dl ); |
| Keisuke_Fujii | 1:19d647492936 | 80 | /*right_pwm.pulsewidth_us( d ); |
| Keisuke_Fujii | 1:19d647492936 | 81 | left_pwm.pulsewidth_us( d );*/ |
| Keisuke_Fujii | 1:19d647492936 | 82 | } |
| Keisuke_Fujii | 1:19d647492936 | 83 | |
| Keisuke_Fujii | 1:19d647492936 | 84 | void turn_l( double dr, double dl ) |
| Keisuke_Fujii | 1:19d647492936 | 85 | { |
| Keisuke_Fujii | 1:19d647492936 | 86 | right_P.write( 0 ); |
| Keisuke_Fujii | 1:19d647492936 | 87 | right_N.write( 1 ); |
| Keisuke_Fujii | 1:19d647492936 | 88 | left_P.write( 0 ); |
| Keisuke_Fujii | 1:19d647492936 | 89 | left_N.write( 1 ); |
| Keisuke_Fujii | 1:19d647492936 | 90 | right_pwm.write( dr ); |
| Keisuke_Fujii | 1:19d647492936 | 91 | left_pwm.write( dl ); |
| Keisuke_Fujii | 1:19d647492936 | 92 | /*right_pwm.pulsewidth_us( d ); |
| Keisuke_Fujii | 1:19d647492936 | 93 | left_pwm.pulsewidth_us( d );*/ |
| Keisuke_Fujii | 1:19d647492936 | 94 | } |
| Keisuke_Fujii | 1:19d647492936 | 95 | |
| Keisuke_Fujii | 1:19d647492936 | 96 | void stop( void ) |
| Keisuke_Fujii | 1:19d647492936 | 97 | { |
| Keisuke_Fujii | 1:19d647492936 | 98 | right_P.write( 0 ); |
| Keisuke_Fujii | 1:19d647492936 | 99 | right_N.write( 0 ); |
| Keisuke_Fujii | 1:19d647492936 | 100 | left_P.write( 0 ); |
| Keisuke_Fujii | 1:19d647492936 | 101 | left_N.write( 0 ); |
| Keisuke_Fujii | 1:19d647492936 | 102 | right_pwm.write( 0 ); |
| Keisuke_Fujii | 1:19d647492936 | 103 | left_pwm.write( 0 ); |
| Keisuke_Fujii | 1:19d647492936 | 104 | } |