Diff: main.cpp

Revision:

0:1d374dc2ea76

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Apr 25 22:21:44 2016 +0000
@@ -0,0 +1,62 @@
+#include "mbed.h"
+#define MAX_IMU_SIZE 29
+#include <vector>
+#include <sstream>
+// transmitter side
+// initialize input sourse
+DigitalOut myled(LED1);
+//active low
+DigitalIn leftW(p21);
+DigitalIn leftR(p22);
+DigitalIn rightW(p23);
+DigitalIn rightR(p24);
+AnalogIn base(p20);
+AnalogIn lower(p19);
+AnalogIn upper(p18);
+AnalogIn cutter(p17);
+//initialize seiral communication
+
+Serial xbee_t(p13, p14);  // tx, rx  send
+
+//received data 
+double D_IMU_Y=0;
+double D_IMU_P=0;
+double D_IMU_R=0;
+//value to be sent through serial
+//s1 base, s2 lower joint, s3 upper joint, s4 cutter
+int s1,s2,s3,s4;
+// m1 left motor, m2 right motor
+// 0 stop, 1 forward mid speed, 2 reverse mid speed
+int m1,m2;
+//pack data into three double number
+double syn_1,syn_2,syn_3;
+
+
+int main()
+{
+    while(1) {
+        //translate controller reading
+        // into integer 0 min 90 max
+        s1=(int)(base*90.0);
+        s2=(int)(lower*90.0);
+        s3=(int)(upper*90.0);
+        s4=(int)(cutter*90.0);
+        //left motor
+        if (leftW == 0 && leftR ==1) {m1 = 1;}
+        if (leftW == 1 && leftR ==0) {m1 = 2;}
+        if (leftW == 1 && leftR ==1) {m1 = 0;}
+        //right motoer
+        if (rightW == 0 && rightR ==1) {m2 = 1;}
+        if (rightW == 1 && rightR ==0) {m2 = 2;}
+        if (rightW == 1 && rightR ==1) {m2 = 0;}
+        // encode data for sending
+        syn_1=s1+s2/100.0;
+        syn_2=s3+s4/100.0;
+        syn_3=m1+m2/100.0;
+        //send massage
+        xbee_t.printf("#YPR,%2.2f,%2.2f,%2.2f\n",syn_1,syn_2,syn_3);
+        myled = !myled;
+        wait(0.1);
+    }
+}
+ 
\ No newline at end of file