Diff: MotorSystemTest.cpp

Revision:

0:9dba1f8fc770

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MotorSystemTest.cpp	Fri Oct 24 22:00:49 2014 +0000
@@ -0,0 +1,70 @@
+#include "mbed.h"
+
+#define MOTOR_SCALE 0.25
+
+void forward();
+void backward();
+void left();
+void right();
+
+PwmOut whl1Spd(PTB0);
+PwmOut whl2Spd(PTB1);
+PwmOut whl3Spd(PTB2);
+PwmOut whl4Spd(PTB3);
+    
+DigitalOut whl1Dir(PTE20);
+DigitalOut whl2Dir(PTE21);
+DigitalOut whl3Dir(PTE22);
+DigitalOut whl4Dir(PTE23);
+
+int main()
+{
+    for(int i = 0; i < 10; i++)
+    {
+        forward();
+        wait(1);
+        backward();
+        wait(1);
+        left();
+        wait(1);
+        right();
+        wait(1);
+    }
+    
+    return 0;
+}
+
+void forward()
+{
+    whl1Dir = whl2Dir = whl3Dir = whl4Dir = 1;
+    whl1Spd = whl2Spd = whl3Spd = whl4Spd = 1*MOTOR_SCALE;
+    wait(2);
+    whl1Spd = whl2Spd = whl3Spd = whl4Spd = 0;
+}
+
+void backward()
+{
+    whl1Dir = whl2Dir = whl3Dir = whl4Dir = 0;
+    whl1Spd = whl2Spd = whl3Spd = whl4Spd = 1*MOTOR_SCALE;
+    wait(2);
+    whl1Spd = whl2Spd = whl3Spd = whl4Spd = 0; 
+}
+
+void left()
+{
+    whl2Dir = whl4Dir = 1;
+    whl1Dir = whl3Dir = 0;
+    whl1Spd = whl2Spd = whl3Spd = whl4Spd = 1*MOTOR_SCALE;
+    wait(2);
+    whl1Spd = whl2Spd = whl3Spd = whl4Spd = 0;
+   
+}
+
+void right()
+{
+    whl2Dir = whl4Dir = 0;
+    whl1Dir = whl3Dir = 1;
+    whl1Spd = whl2Spd = whl3Spd = whl4Spd = 1*MOTOR_SCALE;
+    wait(2);
+    whl1Spd = whl2Spd = whl3Spd = whl4Spd = 0;
+}
\ No newline at end of file