Diff: main.cpp

Revision:

1:fac3a5bf41dd

Parent:

0:a3da4faf20a8

--- a/main.cpp	Sat Feb 08 18:54:20 2014 +0000
+++ b/main.cpp	Wed Feb 12 23:04:33 2014 +0000
@@ -28,26 +28,10 @@
 */
 
 #include "mbed.h"
-
-/* Remove the comment to get some output on the serial line, however this
-   may result in problems if the serial line's not connected (i.e. MUTT's
-   not USB connected and is free roaming */
-//#define SERIAL_OUTPUT
+#include "ArduinoMotorShield.hpp"
 
-#if defined SERIAL_OUTPUT
-Serial pc(USBTX, USBRX); 
-#define OUTPUT_PERIOD 1000
-#endif
-
-/* Connections to the Motor Shield.  These will need to be changed for boards 
-   other than the KL46Z and it also assumes the presence of the cross-wiring
-   on PWM B (see "Issues" at http://mbed.org/users/johnb/notebook/mutt-mbed-enabled-robot-vehicle/ )
-*/
+ArduinoMotorShield shield;
 AnalogIn irFeedback(PTC2  /* A4 */);
-PwmOut pwm_b( PTA5 );
-PwmOut pwm_a( PTA12 );
-DigitalOut dir_a(PTD7);
-DigitalOut dir_b(PTD5);
 
 /** Determine if the IR sensor is detecting an obstacle */
 bool irSensesObstacle( void )
@@ -58,40 +42,56 @@
     /* If analogue feedback is less than the threshold then there's an obstacle */
     return( irFeedback < ir_thresh );
 }
- 
+
+/* Remove the comment to get some output on the serial line, however this
+   may result in problems if the serial line's not connected (i.e. MUTT's
+   not USB connected and is free roaming) */
+#define SERIAL_OUTPUT
+
+#if defined SERIAL_OUTPUT
+#define OUTPUT_PERIOD 1000
+Serial pc(USBTX, USBRX);
+uint16_t ticker = 0;
+uint16_t next_output = 0;
+
+void serial_status( void )
+{
+    if( next_output == OUTPUT_PERIOD ) 
+    {
+        pc.printf( "MUTT: %04u Obstacle:%u  Current A: %0.2f   Current B: %0.2f\r\n",
+                                            ticker++,irSensesObstacle(),
+                                            shield.GetMotorCurrent(ArduinoMotorShield::MOTOR_A),
+                                            shield.GetMotorCurrent(ArduinoMotorShield::MOTOR_B) );
+        next_output = 0;
+    }
+    next_output++;
+} 
+#endif
+
 int main( void )
 {
-#if defined SERIAL_OUTPUT
-    uint16_t ticker = 0;
-    uint16_t next_output = 0;
-#endif        
-
     /* Set directions on the motors.  What you need here will depend on how 
        you've wired the motors to the motor shield.  If you find that one or 
        both wheels are turning backwards then you can either change the wiring 
        or change these settings */    
-    dir_a = 0;
-    dir_b = 1;
+    shield.SetMotorPolarity( ArduinoMotorShield::MOTOR_A, ArduinoMotorShield::MOTOR_FORWARD );
+    shield.SetMotorPolarity( ArduinoMotorShield::MOTOR_B, ArduinoMotorShield::MOTOR_BACKWARD );
     
     /* Forever is a long time ... */
     while( 1 )
     { 
         /* Determine if there's an obstacle */
         bool obstacle = irSensesObstacle();
+
 #if defined SERIAL_OUTPUT
-        if( next_output == OUTPUT_PERIOD ) 
-        {
-            pc.printf( "MUTT: %04u %u\r\n",ticker++,obstacle);
-            next_output = 0;
-        }
-        next_output++;
+        serial_status();
 #endif        
         
-        /* Set the PWM to both motors to 1 (i.e. 100%) in the case that there's
-           no obstacle, set it to 0 (i.e. 0%) in the case that ab obstacle is
+        /* Set the both motors to 1 (i.e. full power) in the case that there's
+           no obstacle, set it to 0 (i.e. no power) in the case that an obstacle is
            detected */ 
-        pwm_a.write( !obstacle );
-        pwm_b.write( !obstacle );
+        shield.SetMotorPower( ArduinoMotorShield::MOTOR_A, !obstacle );
+        shield.SetMotorPower( ArduinoMotorShield::MOTOR_B, !obstacle );
 
         /* Wait 1 millisecond before goind round the loop again */
         wait_ms( 1 );