Diff: main.cpp

Revision:

9:326b8f261ef0

Parent:

8:262c6c40bff9

Child:

10:51dd168b5a96

--- a/main.cpp	Tue Dec 10 11:52:53 2019 +0000
+++ b/main.cpp	Sat Jan 04 21:35:25 2020 +0000
@@ -3,6 +3,8 @@
 Description: Main program loop
 --------------------------------------------------------------------------------*/
 #include "mbed.h"
+#include "main.h"
+#include <Servo.h>
 #include "TOF.h"
 #include "Motor.h"
 #include "power.h"
@@ -11,20 +13,61 @@
 #include <ros.h>
 #include <std_msgs/UInt8.h>
 
+
+
+#include <std_msgs/UInt16.h>
+#include <std_msgs/UInt8MultiArray.h>
+#include <std_msgs/UInt16MultiArray.h>
+#include <std_msgs/UInt32MultiArray.h>
+
+#include <std_msgs/String.h>
+#include <ros/time.h>
+#include <sensor_msgs/Range.h>
+
+
+
+
 class mySTM32 : public MbedHardware
 {
 public:
     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;
 
-ros::Subscriber<std_msgs::UInt8> sub("keyControl", &keySub);
+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.
+
+//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);
 float power_levels[3]; //Array to voltage levels
 
-Serial pc(SERIAL_TX, SERIAL_RX, 9600);    //set-up serial to pc
 
 Ticker power_monitor;
 
@@ -34,37 +77,58 @@
 DigitalOut TOF6(PC_12);
 DigitalOut TOF7(PD_2);
 
+
 //Class defines
 cAdafruit_VL6180X VL6180X(TOF1,TOF4,TOF6,TOF7);  //Define TOF sensor class and initialise devices
 cPower cPower(VBATT, V5, V3);
 
-/*--------------------------------------------------------------------------------
+/*------------------------------------------------------------------------------
 Function name: power_check
 Input Parameters: N/A
 Output Parameters: N/A
 Description: Routine interrupt to monitor battery levels
-----------------------------------------------------------------------------------*/
-void power_check()
-{
-    power_levels[0] = cPower.monitor_battery(); //Monitors raw battery
-    power_levels[1] = cPower.monitor_5v_line(); //Monitors 5V line
-    power_levels[2] = cPower.monitor_3v3_line();//Monitors 3V3 Line
+------------------------------------------------------------------------------*/
+
+
+std_msgs::UInt8MultiArray m;
+//sensor_msgs::Range range_msg[4];
 
-    update_power_levels(power_levels[0]); //Sends the raw battery levels to the LED class 
-}
+ 
+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);
 
 
 /*--------------------------------------------------------------------------------
                                     MAIN CONTROL LOOP
-----------------------------------------------------------------------------------*/
+-------------------------------------------------------------------------------*/
 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);
+    
+    servo1.position(0);
+    servo1.position(0);
     power_monitor.attach(power_check, 30.0); //Monitor battery every 30 seconds
     
-    uint8_t TOFRange[4]; //Array to store TOF measurements 0-sensor1, 1-sensor4, 2-sensor6, 3-sensor7
     
     //Wait for user button to be pressed
     pc.printf("Press user button to start\n\r");
@@ -73,17 +137,42 @@
     
     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);
 
-        Check_for_obstacles(TOFRange); //Run obstacle avoidance
-        */
-        pc.printf("Spinning...");
+        //Check_for_obstacles(TOFRange); //Run obstacle avoidance
+       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(1);
+        wait_ms(5);
     }
+}
+
+void power_check()
+{
+    power_levels[0] = cPower.monitor_battery(); //Monitors raw battery
+    power_levels[1] = cPower.monitor_5v_line(); //Monitors 5V line
+    power_levels[2] = cPower.monitor_3v3_line();//Monitors 3V3 Line
+
+    update_power_levels(power_levels[0]); //Sends the raw battery levels to the LED class 
 }
\ No newline at end of file