Tony Dou
Receiver
Dependencies: Motor Servo mbed
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