Diff: main.cpp

Revision:

1:df4118878dc4

Parent:

0:425791fe4b42

--- a/main.cpp	Sun May 01 11:12:52 2011 +0000
+++ b/main.cpp	Fri Mar 29 09:05:28 2013 +0000
@@ -23,44 +23,66 @@
 #include "mbed.h"
 #include "USBHost.h"
 #include "Utils.h"
-#include "BD6211F.h"
 #include "Wiimote.h"
+#include "motordriver.h"
 
+AnalogIn L_IR1(p15);      // Analog In Pin
+AnalogIn L_IR2(p16);      // Analog In Pin
+AnalogIn L_IR3(p17);      // Analog In Pin
+AnalogIn R_IR3(p18);      // Analog In Pin
+AnalogIn R_IR2(p19);      // Analog In Pin
+AnalogIn R_IR1(p20);      // Analog In Pin
 
-// ----- Wallbot I/O Setting ----- 
-// Motor
-BD6211F      RightMotor(p21,p22);
-BD6211F      LeftMotor(p23,p24);
+DigitalOut LLED(p7);   // Digital Out Pin
+DigitalOut RLED(p8);
+
+DigitalOut L_IRLED1(p9);    // Digital Out Pin
+DigitalOut L_IRLED2(p10);   // Digital Out Pin
+DigitalOut L_IRLED3(p11);   // Digital Out Pin
+DigitalOut R_IRLED3(p12);   // Digital Out Pin
+DigitalOut R_IRLED2(p13);   // Digital Out Pin
+DigitalOut R_IRLED1(p14);   // Digital Out Pin
+
+Motor L_Motor(p25, p22, p21, 1); // pwm, fwd, rev, can break
+Motor R_Motor(p26, p24, p23, 1); // pwm, fwd, rev, can break
+
 
 // Direct control mode
 int DirectMode( Wiimote* wii, int stat )
 {
+    LLED=1; 
+    RLED=1; // LED Init    
+    
     int ret = stat;
     
     if( wii->left )
     {
-        RightMotor = 1.0;
-        LeftMotor = -1.0;
+        L_Motor.speed(-1.0);
+        R_Motor.speed(1.0);
+        LLED = 1; RLED = 0;  
     }
     else if( wii->right )
     {
-        RightMotor = -1.0;
-        LeftMotor = 1.0;
+        L_Motor.speed(1.0);
+        R_Motor.speed(-1.0);
+        LLED = 0; RLED = 1;
     }    
     else if( wii->up )
     {
-        RightMotor = 1.0;
-        LeftMotor = 1.0;
+        L_Motor.speed(1.0);
+        R_Motor.speed(1.0);
+        LLED = 1; RLED = 1;  
     }
     else if( wii->down )
     {
-        RightMotor = -1.0;
-        LeftMotor = -1.0;
+        L_Motor.speed(-1.0);
+        R_Motor.speed(-1.0);
+        LLED = 0; RLED = 0;  
     }
     else
     {
-        RightMotor = 0.0;
-        LeftMotor = 0.0;
+        L_Motor.stop(0);
+        R_Motor.stop(0);
     }
 
     float factor = wii->wheel / 150.0f; 
@@ -70,13 +92,13 @@
     
     if( wii->one )
     {
-        RightMotor = right_factor;
-        LeftMotor = left_factor;
+        L_Motor.speed(left_factor);
+        R_Motor.speed(right_factor);
     }
     if( wii->two )
     {
-        RightMotor = -left_factor;
-        LeftMotor = -right_factor;
+        L_Motor.speed(-right_factor);
+        R_Motor.speed(-left_factor);
     }
     
     return(ret);