Revision 0:6eeba4b67932, committed 2016-04-25

Comitter:

yichaodou

Date:

Mon Apr 25 22:23:04 2016 +0000

Commit message:

Explosives Deactivate Robot Receiver

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motor.lib	Mon Apr 25 22:23:04 2016 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/simon/code/Motor/#f265e441bcd9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo.lib	Mon Apr 25 22:23:04 2016 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/simon/code/Servo/#36b69a7ced07

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Apr 25 22:23:04 2016 +0000
@@ -0,0 +1,148 @@
+#include "mbed.h"
+#define MAX_IMU_SIZE 29
+#include <vector>
+#include <sstream>
+#include "Servo.h"
+//Receiver side
+DigitalOut myled(LED1);
+DigitalOut led2(LED2);
+Servo base(p23);   //PWM out
+Servo jointLow(p24); //lower joint
+Servo jointUp(p25); //uppter joint
+Servo cutter(p26); //cutting servo
+AnalogIn pod(p15); // Pod reading
+
+//serial port communication
+Serial pc(USBTX, USBRX);
+Serial IMU(p13,p14);  // tx, rx  recv
+char IMU_message[256];
+int  IMU_message_counter=0;
+string IMU_Y="N/A";
+string IMU_P="N/A";
+string IMU_R="N/A";
+//received data 
+double D_IMU_Y=0;
+double D_IMU_P=0;
+double D_IMU_R=0;
+// Global variables
+int s1_received,s2_received,s3_received,s4_received;  //0-90 deg  [0,90]
+int m1_received,m2_received;   //0%-99% thruttle [0,99]
+
+//motor driver
+PwmOut m1_S(p21);
+DigitalOut m1_A(p18);
+DigitalOut m1_B(p19);
+PwmOut m2_S(p22);
+DigitalOut m2_A(p16);
+DigitalOut m2_B(p17);
+
+void drive_motor_1(float spd){
+    if (spd<=0){
+        m1_A=1;
+        m1_B=0;
+        m1_S= abs(spd);   
+    }else{
+        m1_A=0;
+        m1_B=1;
+        m1_S= abs(spd); 
+    }    
+}
+
+void drive_motor_2(float spd){
+    if (spd<=0){
+        m2_A=1;
+        m2_B=0;
+        m2_S= abs(spd);   
+    }else{
+        m2_A=0;
+        m2_B=1;
+        m2_S= abs(spd); 
+    }    
+}
+
+
+vector<string> split(const string &s, char delim) {
+    stringstream ss(s);
+    string item;
+    vector<string> tokens;
+    while (getline(ss, item, delim)) {
+        if (item.empty()) {
+            item = "N/A";
+        }
+        tokens.push_back(item);
+    }
+    return tokens;
+}
+
+void updateIMU(string IMU_data) {
+    string IMU_data_string(IMU_data);
+    if (IMU_data_string.substr(0,4) == "#YPR" and IMU_data_string.size() <= MAX_IMU_SIZE) {
+        IMU_data_string = IMU_data_string.substr(5);
+        vector<string> result = split(IMU_data_string, ',');
+        IMU_Y = result.at(0);
+        D_IMU_Y = strtod(IMU_Y.c_str(), NULL);
+        IMU_P = result.at(1);
+        D_IMU_P = strtod(IMU_P.c_str(), NULL);       
+        IMU_R = result.at(2).substr(0, result.at(2).size()-1);
+        D_IMU_R = strtod(IMU_R.c_str(), NULL);  
+        
+        s1_received=(int)D_IMU_Y;
+        s2_received=(int)(D_IMU_Y*100-s1_received*100);
+        s3_received=(int)D_IMU_P;
+        s4_received=(int)(D_IMU_P*100-s3_received*100);
+        m1_received=(int)D_IMU_R;
+        m2_received=(int)(D_IMU_R*100-m1_received*100);
+        
+        }
+}
+
+void printStateIMU() {                 
+     pc.printf("received & decoded data: %d,  %d,   %d,  %d,   %d,  %d\n",s1_received, s2_received,s3_received, s4_received,m1_received, m2_received);                  
+ }
+ 
+void IMU_serial_ISR() {
+    char buf;
+    led2 = !led2;
+     while (IMU.readable()) {
+        buf = IMU.getc();
+
+        IMU_message[IMU_message_counter]=buf;
+        IMU_message_counter+=1;
+        
+        if (buf=='\n'){
+            string IMU_copy(IMU_message, IMU_message_counter);
+            updateIMU(IMU_copy);
+            IMU_message_counter=0; 
+            IMU_copy[0] = '\0';
+        }
+
+    }
+}
+
+int main() {
+    myled =1;
+    led2 = 1;
+    IMU.attach(&IMU_serial_ISR);
+    while(1){
+        
+        //control four servos
+        base = (float)s1_received/90.0;
+        jointLow = (float)s2_received/90.0;
+        jointUp = (float)s3_received/90.0;
+        cutter = (float)s4_received/90.0;
+        
+        //control left motor
+        if(m1_received==1){drive_motor_1(1.0);}
+        if(m1_received==2){drive_motor_1(-1.0);}
+        if(m1_received==0){drive_motor_1(0.0);}
+        //control right motor
+        if(m2_received==1){drive_motor_2(1.0);}
+        if(m2_received==2){drive_motor_2(-1.0);}
+        if(m2_received==0){drive_motor_2(0.0);}
+        
+        myled = !myled;
+        wait(0.1);
+        
+    }
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Mon Apr 25 22:23:04 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/99a22ba036c9
\ No newline at end of file