Diff: main.cpp

Revision:

11:12e73437dc9f

Parent:

10:51dd168b5a96

--- a/main.cpp	Mon Jan 06 20:38:07 2020 +0000
+++ b/main.cpp	Fri Jan 10 15:05:11 2020 +0000
@@ -33,14 +33,6 @@
     mySTM32(): MbedHardware(PD_5, PD_6, 57600) {};
 };
 
-/*
-void servo1_cb( const std_msgs::UInt16& cmd_msg) {
-    servo1.position(cmd_msg.data); //set servo angle, should be from 0-180
-}
-void servo2_cb( const std_msgs::UInt16& cmd_msg) {
-    servo2.position(cmd_msg.data); //set servo angle, should be from 0-180
-}    
-*/
 
 ros::NodeHandle_<mySTM32> nh;
 
@@ -51,22 +43,7 @@
 std_msgs::UInt8MultiArray range_msg;
 ros::Publisher TOFreadings("TOFreadings", &range_msg);
 
-//std_msgs::UInt16MultiArray range_msg;
-//ros::Publisher TOFstuff("TOFstuff", &range_msg);
 
-/*sensor_msgs::Range range_msg1, range_msg2,range_msg3,range_msg4;
-ros::Publisher tof1("tof1", &range_msg1);
-ros::Publisher tof2("tof2", &range_msg2);
-ros::Publisher tof3("tof3", &range_msg3);
-ros::Publisher tof4("tof4", &range_msg4);*/
-
-//std_msgs::Int32MultiArray range_msg;
-//array.data.clear();
-
-
-/* NOT USED ANYMORE */
-//ros::Subscriber<std_msgs::UInt16> sub2("servo1", servo1_cb);
-//ros::Subscriber<std_msgs::UInt16> sub3("servo2", servo2_cb);
 
 
 DigitalIn user_button(USER_BUTTON);
@@ -95,19 +72,13 @@
 
 
 std_msgs::UInt8MultiArray m;
-//sensor_msgs::Range range_msg[4];
+
 
  
 float TOFRange[4];
  
 DigitalOut led = LED1;
-/*
-Timer t;  // Timer 
 
- char frameid1[] = "/Rear Sensor";
- char frameid2[] = "/Front Left Sensor";
- char frameid3[] = "/Front Center Sensor"; 
- char frameid4[] = "/Front Right Sensor";*/
  
 /* Private Functions----------------------------------------------------------*/ 
 void power_check(void);
@@ -118,19 +89,13 @@
 -------------------------------------------------------------------------------*/
 int main()
 {
-    //t.start(); 
-    //IncSize=1;
+    
     
     nh.initNode();
     nh.subscribe(sub);
     nh.subscribe(sub1);
-    //nh.subscribe(sub2);
-    
-    /*nh.advertise(tof1);
-    nh.advertise(tof2);
-    nh.advertise(tof3);
-    nh.advertise(tof4);*/
-    
+
+  
     //Publishers
     nh.advertise(TOFreadings); //TOF readings
     
@@ -161,7 +126,7 @@
         TOFRange[1] = serviceTOF(VL6180X, ADDR4);
         TOFRange[2] = serviceTOF(VL6180X, ADDR5);
         TOFRange[3] = serviceTOF(VL6180X, ADDR6);
-
+        // 
         TOFrangePtr[0] = &TOFRange[0];
         TOFrangePtr[1] = &TOFRange[1];
         TOFrangePtr[2] = &TOFRange[2];
@@ -175,25 +140,10 @@
         
         //Publish data
         TOFreadings.publish(&range_msg);
-       /* //Check_for_obstacles(TOFRange);
-       range_msg1.header.frame_id =frameid1;
-       range_msg2.header.frame_id =frameid2;
-       range_msg3.header.frame_id =frameid3;
-       range_msg4.header.frame_id =frameid4;
-       
-       range_msg1.range = TOFRange[0];
-       range_msg2.range = TOFRange[1];
-       range_msg3.range = TOFRange[2];
-       range_msg4.range = TOFRange[3];
-        tof1.publish(&range_msg1);
-        tof2.publish(&range_msg2);
-        tof3.publish(&range_msg3);
-        tof4.publish(&range_msg4);*/
-        
         // Need to write code like the obstical ovaoidance, to stop the motors, by the nucleo 
         // sending a char to the PI
         
-        //pc.printf("Spinning...");
+       
         nh.spinOnce();
         wait_ms(5);
     }