Diff: main.cpp

Revision:

7:cead0e7b0c1e

Parent:

6:b8a1fa98707e

Child:

8:347b10e6bdf8

--- a/main.cpp	Wed Aug 15 17:05:59 2018 +0000
+++ b/main.cpp	Mon Aug 27 04:48:25 2018 +0000
@@ -3,36 +3,35 @@
 AnalogIn x(A0);
 AnalogIn y(A1);
 
-DigitalOut off(D0);
-DigitalOut on(D1);
+DigitalOut on(D12);
+DigitalOut off(D11);
 DigitalOut up(D2);
 DigitalOut down(D3);
 
+Serial pc(USBTX, USBRX, 57600);
+Timer t;
+Thread thread;
+EventQueue queue;
+Wheelchair smart(xDir,yDir, &pc, &t);
+
 char c;
 bool manual = false;
-bool received = false;
-Serial pc(USBTX, USBRX, 57600);
-
-Timer t;
-EventQueue queue;
-
-//MPU9250 imu(D14, D15);
-Wheelchair smart(xDir,yDir, &pc, &t);
-Thread thread;
 
 ros::NodeHandle nh;
-geometry_msgs::Twist commandRead;
-ros::Publisher chatter("chatter", &commandRead);
+//geometry_msgs::Twist commandRead;
+std_msgs::String commandRead;
+std_msgs::String out;
 
-void handlerFunction(const geometry_msgs::Twist& command)
+ros::Publisher chatter("chatter", &commandRead);
+bool received = false;
+
+void handlerFunction(const std_msgs::String& commandIn)
 {
-    if(command.linear.x >0) {
-        c = 'w';
-    }
+    commandRead = commandIn;
     received = true;
 }
 
-ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &handlerFunction);
+ros::Subscriber<std_msgs::String> sub("cmd_vel", &handlerFunction);
 
 
 void setupNode()
@@ -40,20 +39,21 @@
     nh.initNode();
     nh.subscribe(sub);
     nh.advertise(chatter);
+    char initialVal[2] = "z";
+    commandRead.data = initialVal;
 }
 
 int main(void)
 {
-    on = 0;
-    off = 0;
     
     queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::compass_thread);
     t.reset();
     thread.start(callback(&queue, &EventQueue::dispatch_forever));
     setupNode();
+    
     while(1) {
         if( received) {
-
+        c = commandRead.data[0];
             if( c == 'w') {
                 pc.printf("up \n");
                 smart.forward();
@@ -75,11 +75,11 @@
             }
 
             else if( c == 'r') {
-                smart.turn_right(90);
+                smart.pid_right(90);
             }
 
             else if( c == 'l') {
-                smart.turn_left(90);
+                smart.pid_left(90);
             }
 
             else if( c == 't') {
@@ -99,11 +99,13 @@
                 on = 1;
                 wait(1);
                 on = 0;
+                c = 'z';
             } else if(c == 'f') {
                 pc.printf("turning off\n");
                 off = 1;
                 wait(1);
                 off = 0;
+                c = 'z';
             }
 
             else if( c == 'm' || manual) {
@@ -126,6 +128,7 @@
                 pc.printf("none \n");
                 smart.stop();
             }
+            received = false;
         }
 
         else {