Diff: main.cpp

Revision:

0:c9d69c701daf

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Oct 11 00:56:33 2014 +0000
@@ -0,0 +1,103 @@
+// For the KL25Z
+#include "mbed.h"
+
+#define MOVE_FWD 'w'
+#define MOVE_REV 's'
+#define MOVE_LEF 'a'
+#define MOVE_RIG 'd'
+#define MOVE_STO 'q'
+
+DigitalOut myled(LED2);
+DigitalOut M1DIR(D7);
+DigitalOut M2DIR(D8);
+PwmOut M1PWM(D9);
+PwmOut M2PWM(D10);
+DigitalOut M_ON(D2);
+Serial bt(D14, D15);
+Serial pc(USBTX, USBRX);
+
+bool move_motors(char c, int _power);
+void motor_fwd(int _power);
+void motor_rev(int _power);
+void motor_left(int _power);
+void motor_right(int _power);
+void motor_stop();
+
+
+int main() {
+    myled = 0;
+    wait(3);
+    bt.baud(9600);
+    M_ON = 1;
+    while(1) {
+        if (bt.readable()){
+            char c = bt.getc();
+            bt.printf("%c\n\r", c);
+            move_motors(c, 100);
+        }
+    }
+}
+
+bool move_motors(char c, int _power){
+    switch (c){
+        case MOVE_FWD:
+            motor_fwd(_power);
+            break;
+        case MOVE_REV:
+            motor_rev(_power);
+            break;
+        case MOVE_LEF:
+            motor_left(_power);
+            break;
+        case MOVE_RIG:
+            motor_right(_power);
+            break;
+        case MOVE_STO:
+            motor_stop();
+            break;
+        default:
+            motor_stop();
+            //bt.printf("%f\r\n", get_mag_angle());
+    }
+    return 1;
+}
+
+void motor_fwd(int _power){
+    M1DIR = 1;
+    M1PWM = 1;
+    M2DIR = 1;
+    M2PWM = 1;
+    pc.printf("FWD\n");
+}
+
+void motor_rev(int _power){
+    M1DIR = 0;
+    M1PWM = 1;
+    M2DIR = 0;
+    M2PWM = 1;
+    pc.printf("REV\n");
+}
+
+void motor_left(int _power){
+    M1DIR = 0;
+    M1PWM = 1;
+    M2DIR = 1;
+    M2PWM = 1;
+    pc.printf("LEFT\n");  
+}
+
+void motor_right(int _power){
+    M1DIR = 1;
+    M1PWM = 1;
+    M2DIR = 0;
+    M2PWM = 1;
+    pc.printf("RIGHT\n");
+}
+
+void motor_stop(){
+    M1DIR = 1;
+    M1PWM = 0;
+    M2DIR = 1;
+    M2PWM = 0;
+    pc.printf("STOP\n");
+}