Diff: motor.h

Revision:

1:19d647492936

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motor.h	Thu Nov 10 13:29:43 2011 +0000
@@ -0,0 +1,104 @@
+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 );
+}
\ No newline at end of file