Diff: lib/motors.cpp

Revision:

0:3a322aad8c88

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/lib/motors.cpp	Thu Oct 16 01:59:21 2014 +0000
@@ -0,0 +1,80 @@
+// ----- Libraries ------------------------------------------------------------------
+#include "mbed.h"
+#include "motors.h"
+
+// ----- I/O Pins -------------------------------------------------------------------
+// MOTORS (Control)
+DigitalOut dir_left(D7);
+DigitalOut dir_right(D8);
+PwmOut pwm_left(D9);
+PwmOut pwm_right(D10);
+
+// ----- Functions ------------------------------------------------------------------
+void move_motors(char _move_command, int _power_left, int _power_right){
+    switch (_move_command){
+        case MOVE_FWD:
+            motor_fwd(_power_left, _power_right);
+            break;
+        case MOVE_REV:
+            motor_rev(_power_left, _power_right);
+            break;
+        case MOVE_LEF:
+            motor_left(_power_left, _power_right);
+            break;
+        case MOVE_RIG:
+            motor_right(_power_left, _power_right);
+            break;
+        case MOVE_STO:
+            motor_stop();
+            break;
+        //case MOVE_90L:
+        //    move_90(1);
+        //    break;
+        //case MOVE_90R:
+        //    move_90(2);
+        //    break;
+        default:
+        //    bt.printf("%f\r\n", get_mag_angle());
+        break;
+    }
+}
+
+void motor_fwd(int _power_left, int _power_right){
+    dir_left = 1;
+    dir_right = 1;
+    pwm_left = (float)_power_left/100;
+    pwm_right = (float)_power_right/100;
+    //pc.printf("FWD\n");
+}
+
+void motor_rev(int _power_left, int _power_right){
+    dir_left = 0;
+    dir_right = 0;
+    pwm_left = (float)_power_left/100;
+    pwm_right = (float)_power_right/100;
+    //pc.printf("REV\n");
+}
+
+void motor_left(int _power_left, int _power_right){
+    dir_left = 0;
+    dir_right = 1;
+    pwm_left = (float)_power_left/100;
+    pwm_right = (float)_power_right/100;
+    //pc.printf("LEFT\n");
+}
+
+void motor_right(int _power_left, int _power_right){
+    dir_left = 1;
+    dir_right = 0;
+    pwm_left = (float)_power_left/100;
+    pwm_right = (float)_power_right/100;
+    //pc.printf("RIGHT\n");
+}
+
+void motor_stop(){
+    dir_left = 1;
+    dir_right = 1;
+    pwm_left = 0;
+    pwm_right = 0;
+    //pc.printf("STOP\n");
+}
\ No newline at end of file