Diff: main.cpp

Revision:

10:51dd168b5a96

Parent:

9:326b8f261ef0

Child:

11:12e73437dc9f

--- a/main.cpp	Sat Jan 04 21:35:25 2020 +0000
+++ b/main.cpp	Mon Jan 06 20:38:07 2020 +0000
@@ -47,14 +47,18 @@
 ros::Subscriber<std_msgs::UInt8> sub("keyControl", &MotorKeySub);  // Subscriber for Motor Control by Keyboard.
 ros::Subscriber<std_msgs::UInt8> sub1("ServoControl", &servoKeySub); // Subscriber for Servo Control by keyboard.
 
+//TOF publisher
+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;
+/*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);
+ros::Publisher tof4("tof4", &range_msg4);*/
 
 //std_msgs::Int32MultiArray range_msg;
 //array.data.clear();
@@ -74,12 +78,12 @@
 //TOF chip shutdown signals
 DigitalOut TOF1(PC_8);
 DigitalOut TOF4(PC_11);
-DigitalOut TOF6(PC_12);
-DigitalOut TOF7(PD_2);
+DigitalOut TOF5(PC_12);
+DigitalOut TOF6(PD_2);
 
 
 //Class defines
-cAdafruit_VL6180X VL6180X(TOF1,TOF4,TOF6,TOF7);  //Define TOF sensor class and initialise devices
+cAdafruit_VL6180X VL6180X(TOF1,TOF4,TOF5,TOF6);  //Define TOF sensor class and initialise devices
 cPower cPower(VBATT, V5, V3);
 
 /*------------------------------------------------------------------------------
@@ -97,12 +101,14 @@
 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";
+ char frameid4[] = "/Front Right Sensor";*/
+ 
 /* Private Functions----------------------------------------------------------*/ 
 void power_check(void);
 
@@ -113,22 +119,34 @@
 int main()
 {
     //t.start(); 
-    IncSize=1;
+    //IncSize=1;
     
     nh.initNode();
     nh.subscribe(sub);
     nh.subscribe(sub1);
     //nh.subscribe(sub2);
     
-    nh.advertise(tof1);
+    /*nh.advertise(tof1);
     nh.advertise(tof2);
     nh.advertise(tof3);
-    nh.advertise(tof4);
+    nh.advertise(tof4);*/
+    
+    //Publishers
+    nh.advertise(TOFreadings); //TOF readings
+    
+    //TOF publisher parameters
+    range_msg.layout.dim = (std_msgs::MultiArrayDimension *)
+    malloc(sizeof(std_msgs::MultiArrayDimension) * 2);
+    range_msg.layout.dim[0].label = "TOF readings";
+    range_msg.layout.dim[0].size = 4;
+    range_msg.layout.dim[0].stride = 1*4;
+    range_msg.layout.data_offset = 0;
+    range_msg.data = (uint8_t *)malloc(sizeof(int)*4);
+    range_msg.data_length = 4;
     
     servo1.position(0);
     servo1.position(0);
-    power_monitor.attach(power_check, 30.0); //Monitor battery every 30 seconds
-    
+    power_monitor.attach(power_check, 30.0); //Monitor battery every 30 seconds 
     
     //Wait for user button to be pressed
     pc.printf("Press user button to start\n\r");
@@ -138,13 +156,26 @@
     while(1) {
         
         
-        //Perform TOF measurements 
+        //Perform TOF measurements
         TOFRange[0] = serviceTOF(VL6180X, ADDR1);
         TOFRange[1] = serviceTOF(VL6180X, ADDR4);
-        TOFRange[2] = serviceTOF(VL6180X, ADDR6);
-        TOFRange[3] = serviceTOF(VL6180X, ADDR7);
+        TOFRange[2] = serviceTOF(VL6180X, ADDR5);
+        TOFRange[3] = serviceTOF(VL6180X, ADDR6);
 
-        //Check_for_obstacles(TOFRange); //Run obstacle avoidance
+        TOFrangePtr[0] = &TOFRange[0];
+        TOFrangePtr[1] = &TOFRange[1];
+        TOFrangePtr[2] = &TOFRange[2];
+        TOFrangePtr[3] = &TOFRange[3];
+        
+        //Assign to TOFreadings publisher
+        range_msg.data[0] = TOFRange[0];
+        range_msg.data[1] = TOFRange[1];
+        range_msg.data[2] = TOFRange[2];
+        range_msg.data[3] = TOFRange[3];
+        
+        //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;
@@ -157,7 +188,7 @@
         tof1.publish(&range_msg1);
         tof2.publish(&range_msg2);
         tof3.publish(&range_msg3);
-        tof4.publish(&range_msg4);
+        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