Revision 9:326b8f261ef0, committed 2020-01-04

Comitter:

Alexshawcroft

Date:

Sat Jan 04 21:35:25 2020 +0000

Parent:

8:262c6c40bff9

Commit message:

oh

Changed in this revision

--- a/Motors/Motor.cpp	Tue Dec 10 11:52:53 2019 +0000
+++ b/Motors/Motor.cpp	Sat Jan 04 21:35:25 2020 +0000
@@ -2,209 +2,350 @@
 Filename: main.cpp
 Description: Main program loop
 --------------------------------------------------------------------------------*/
+#include "main.h"
 #include "Buzzer.h"
 #include "Motor.h"
 #include "LED.h"
 #include <ros.h>
+#include <Servo.h>
 #include <std_msgs/UInt8.h>
+//#include <std_msgs/UInt16.h>
+
 
 cMotor MotorL(M1_PWM, M1_IN1, M1_IN2);  //Left motor class and pin initialisation
 cMotor MotorR(M2_PWM, M2_IN1, M2_IN2);  //Right motor class and pin initialisation
 
+//Servo servo1(srvoTilt);
+//Servo servo2(srvoPan);
+
 //Class definitions
 cBuzzer cBuzzer(Buzz);
 cRGB_LED cRGB_LED(DIAG_RED, DIAG_BLU, DIAG_GRN);
 
+int Servo1Pos;
+int Servo2Pos;
 
-void keySub(const std_msgs::UInt8 &keyPress)
+/*--------------------------------------------------------------------------------
+Function name: KeySub
+Input Parameters: std_msgs::UInt8 &KeyPress
+Output Parameters:
+Description:
+----------------------------------------------------------------------------------*/
+void MotorKeySub(const std_msgs::UInt8 &keyPress)
 {
     printf("%d", keyPress.data);
-    
-    if (keyPress.data == 8) 
-    {
+
+    // Lowercase chars //
+    if (keyPress.data == 119) { // w
+        if (keyPress.data  == 88 || 120) { // X
+            MotorR.Stop();
+            MotorL.Stop();
+        }
         MotorR.Forwards(1.0f);
         MotorL.Forwards(1.0f);
-        wait(0.5);
+        wait(0.05);
     }
-    
-    else if (keyPress.data == 4) 
-    {
+
+    else if (keyPress.data == 97) { // a
+        if (keyPress.data  == 88 || 120) { // X
+            MotorR.Stop();
+            MotorL.Stop();
+        }
         MotorR.Forwards(1.0f);
         MotorL.Backwards(1.0f);
-        wait(0.5);
-    } 
-    
-    else if (keyPress.data  == 6) 
-    {
+        wait(0.05);
+    }
+
+    else if (keyPress.data  == 100) { // d
+        if (keyPress.data  == 88 || 120) { // X
+            MotorR.Stop();
+            MotorL.Stop();
+        }
         MotorR.Backwards(1.0f);
         MotorL.Forwards(1.0f);
-        wait(0.5);
-    } 
-    
-    else if (keyPress.data  == 2) 
-    {
+        wait(0.05);
+    }
+
+    else if (keyPress.data  == 115) { // s
+        if (keyPress.data  == 88 || 120) { // X
+            MotorR.Stop();
+            MotorL.Stop();
+        }
         MotorR.Backwards(1.0f);
         MotorL.Backwards(1.0f);
-        wait(0.5);
-    } 
-    
-    else if (keyPress.data  == 5) 
-    {
+        wait(0.05);
+    }
+
+
+    // Upper Case Chars
+    else if (keyPress.data == 87) { // W
+        if (keyPress.data  == 88 || 120) { // X
+            MotorR.Stop();
+            MotorL.Stop();
+        }
+        MotorR.Forwards(1.0f);
+        MotorL.Forwards(1.0f);
+        wait(0.387);
+    }
+
+    else if (keyPress.data == 65) { // A
+        if (keyPress.data  == 88 || 120) { // X
+            MotorR.Stop();
+            MotorL.Stop();
+        }
+        MotorR.Forwards(1.0f);
+        MotorL.Backwards(1.0f);
+        wait(0.387);
+    }
+
+    else if (keyPress.data  == 68) { // D
+        if (keyPress.data  == 88 || 120) { // X
+            MotorR.Stop();
+            MotorL.Stop();
+        }
+        MotorR.Backwards(1.0f);
+        MotorL.Forwards(1.0f);
+        wait(0.387);
+    }
+
+    else if (keyPress.data  == 83) { // S
+        if (keyPress.data  == 88 || 120) { // X
+            MotorR.Stop();
+            MotorL.Stop();
+        }
+        MotorR.Backwards(1.0f);
+        MotorL.Backwards(1.0f);
+        wait(0.387);
+    }
+
+
+    else {
         MotorR.Stop();
         MotorL.Stop();
-        wait(0.5);
-    } 
-    
-    else 
-    {
-        MotorR.Stop();
-        MotorL.Stop(); 
     }
 }
 
-    /*--------------------------------------------------------------------------------
-    Function name: update_power_levels
-    Input Parameters: vBatt
-    Output Parameters: N/A
-    Description: Function used to send the battery level over to the LED class locally from global source files
-    ----------------------------------------------------------------------------------*/
-    void update_power_levels(float vBatt) {
-        cRGB_LED.record_power(vBatt);
-    }
-
-    /*--------------------------------------------------------------------------------
-    Function name: Check_for_obstacles
-    Input Parameters: cMotor - MotorL (left motor) cMotor MotorR (right motor) TOFrange[4] - sensor measurements
-    Output Parameters: N/A
-    Description: Simple obstacle avoidance functionality
-    ----------------------------------------------------------------------------------*/
-    void Check_for_obstacles(uint8_t TOFRange[4]) {
-        //If top right sensor(6) detects something
-        if (TOFRange[2]<200) {
+/*--------------------------------------------------------------------------------
+Function name: update_power_levels
+Input Parameters: vBatt
+Output Parameters: N/A
+Description: Function used to send the battery level over to the LED class locally from global source files
+----------------------------------------------------------------------------------*/
+void update_power_levels(float vBatt)
+{
+    cRGB_LED.record_power(vBatt);
+}
 
-            if(TOFRange[2]>150) {   //Provided its 15cm away...
-                cBuzzer.Beep();
-                cRGB_LED.blue_led();
-                wait(0.02);
-                cRGB_LED.display_power();
-                if(TOFRange[3]<200) {   //...and the back sensor detects something
-                    //Smooth turn right
-                    MotorR.Forwards(0.8f);
-                    MotorL.Forwards(0.2f);
-
-                } else if(TOFRange[1]<200) { //...and the top left sensor detects something
-                    //Smooth turn left
-                    MotorR.Forwards(0.2f);
-                    MotorL.Forwards(0.8f);
+/*--------------------------------------------------------------------------------
+Function name: Check_for_obstacles
+Input Parameters: cMotor - MotorL (left motor) cMotor MotorR (right motor) TOFrange[4] - sensor measurements
+Output Parameters: N/A
+Description: Simple obstacle avoidance functionality
+----------------------------------------------------------------------------------*/
+void Check_for_obstacles(uint8_t TOFRange[4])
+{
+    //If top right sensor(6) detects something
+    if (TOFRange[2]<200) {
 
-                } else {
-                    MotorR.Forwards(0.8f);
-                    MotorL.Forwards(0.2f);
-                }
-
-            } else {
-
-                if(TOFRange[3]<200) {
-                    MotorR.Backwards(0.1f);
-                    MotorL.Backwards(0.9f);
-
-                } else if(TOFRange[1]<200) {
-                    MotorR.Backwards(0.9f);
-                    MotorL.Backwards(0.1f);
-
-                } else {
-                    MotorR.Backwards(0.1f);
-                    MotorL.Backwards(0.9f);
-                }
-            }
-
-        } else if(TOFRange[3]<200) {
+        if(TOFRange[2]>150) {   //Provided its 15cm away...
             cBuzzer.Beep();
             cRGB_LED.blue_led();
             wait(0.02);
             cRGB_LED.display_power();
+            if(TOFRange[3]<200) {   //...and the back sensor detects something
+                //Smooth turn right
+                MotorR.Forwards(0.8f);
+                MotorL.Forwards(0.2f);
 
-            if(TOFRange[1]<200) {
-                MotorR.Forwards(1.0f);
-                MotorL.Forwards(1.0f);
+            } else if(TOFRange[1]<200) { //...and the top left sensor detects something
+                //Smooth turn left
+                MotorR.Forwards(0.2f);
+                MotorL.Forwards(0.8f);
+
             } else {
-
-                MotorR.Forwards(0.9f);
-                MotorL.Forwards(0.1f);
+                MotorR.Forwards(0.8f);
+                MotorL.Forwards(0.2f);
             }
 
-        } else if(TOFRange[1]<200) {
-            cBuzzer.Beep();
-            cRGB_LED.blue_led();
-            wait(0.02);
-            cRGB_LED.display_power();
-            MotorR.Forwards(0.1f);
-            MotorL.Forwards(0.9f);
+        } else {
+
+            if(TOFRange[3]<200) {
+                MotorR.Backwards(0.1f);
+                MotorL.Backwards(0.9f);
+
+            } else if(TOFRange[1]<200) {
+                MotorR.Backwards(0.9f);
+                MotorL.Backwards(0.1f);
 
-        } else if(TOFRange[0]<200) {
-            cBuzzer.Beep();
-            cRGB_LED.blue_led();
-            wait(0.02);
-            cRGB_LED.display_power();
+            } else {
+                MotorR.Backwards(0.1f);
+                MotorL.Backwards(0.9f);
+            }
+        }
+
+    } else if(TOFRange[3]<200) {
+        cBuzzer.Beep();
+        cRGB_LED.blue_led();
+        wait(0.02);
+        cRGB_LED.display_power();
+
+        if(TOFRange[1]<200) {
             MotorR.Forwards(1.0f);
             MotorL.Forwards(1.0f);
+        } else {
+
+            MotorR.Forwards(0.9f);
+            MotorL.Forwards(0.1f);
         }
 
-        else {
-            MotorR.Forwards(1.0f);
-            MotorL.Forwards(1.0f);
+    } else if(TOFRange[1]<200) {
+        cBuzzer.Beep();
+        cRGB_LED.blue_led();
+        wait(0.02);
+        cRGB_LED.display_power();
+        MotorR.Forwards(0.1f);
+        MotorL.Forwards(0.9f);
+
+    } else if(TOFRange[0]<200) {
+        cBuzzer.Beep();
+        cRGB_LED.blue_led();
+        wait(0.02);
+        cRGB_LED.display_power();
+        MotorR.Forwards(1.0f);
+        MotorL.Forwards(1.0f);
+    }
+
+    else {
+        MotorR.Forwards(1.0f);
+        MotorL.Forwards(1.0f);
+    }
+}
+
+/*--------------------------------------------------------------------------------
+Function name: cMotor
+Input Parameters: PwmOut pwm - Motor PWM, DigitalOut fwd - Motor input1, DigitalOut rev - Motor Input2
+Output Parameters: N/A
+Description: Class constructor (Initialisation upon creating class)
+----------------------------------------------------------------------------------*/
+cMotor::cMotor(PwmOut pwm, DigitalOut fwd, DigitalOut rev):_pwm(pwm), _fwd(fwd), _rev(rev)
+{
+
+    // Set initial condition of PWM
+    _pwm.period(0.001); //1KHz
+    _pwm = 0;
+
+    // Initial condition of output enables
+    _fwd = 0;
+    _rev = 0;
+}
+
+/*--------------------------------------------------------------------------------
+Function name: Forwards
+Input Parameters: float speed - PWM duty between 0-1
+Output Parameters: N/A
+Description: Drives the motor forwards at a designated speed
+----------------------------------------------------------------------------------*/
+void cMotor::Forwards(float speed)
+{
+    _fwd = 1;
+    _rev = 0;
+    _pwm = speed;
+}
+
+/*--------------------------------------------------------------------------------
+Function name: Backwards
+Input Parameters: float speed - PWM duty between 0-1
+Output Parameters: N/A
+Description: Drives the motor backwards at a designated speed
+----------------------------------------------------------------------------------*/
+void cMotor::Backwards(float speed)
+{
+    _fwd = 0;
+    _rev = 1;
+    _pwm = speed;
+}
+
+/*--------------------------------------------------------------------------------
+Function name: Stop
+Input Parameters: N/A
+Output Parameters: N/A
+Description: Drives the motor backwards at a designated speed
+----------------------------------------------------------------------------------*/
+void cMotor::Stop()
+{
+    _fwd = 0;
+    _rev = 0;
+    _pwm = 0;
+}
+
+
+
+/*--------------------------------------------------------------------------------
+Function name: ServoKeySub
+Input Parameters:
+Output Parameters:
+Description:
+----------------------------------------------------------------------------------*/
+/* Ascii values for arrow Keys
+37(left arrow)
+38(up arrow)
+39(right arrow)
+40(down arrow)
+*/
+
+void servoKeySub(const std_msgs::UInt8 &ServoKeyPress)
+{
+    printf("%d", ServoKeyPress.data);
+
+    if (ServoKeyPress.data == 106) {   // j for Pan Left
+        if(Servo1Pos>-45) {
+            Servo1Pos = Servo1Pos-IncSize;
+            servo1.position(Servo1Pos);
+            wait(0.01);
         }
     }
 
-    /*--------------------------------------------------------------------------------
-    Function name: cMotor
-    Input Parameters: PwmOut pwm - Motor PWM, DigitalOut fwd - Motor input1, DigitalOut rev - Motor Input2
-    Output Parameters: N/A
-    Description: Class constructor (Initialisation upon creating class)
-    ----------------------------------------------------------------------------------*/
-    cMotor::cMotor(PwmOut pwm, DigitalOut fwd, DigitalOut rev):_pwm(pwm), _fwd(fwd), _rev(rev) {
+    else if (ServoKeyPress.data == 108) {   // l for Pan Right
+        if(Servo1Pos<45) {
+            Servo1Pos = Servo1Pos+IncSize;
+            servo1.position(Servo1Pos);
+            wait(0.01);
+        }
+    }
 
-        // Set initial condition of PWM
-        _pwm.period(0.001); //1KHz
-        _pwm = 0;
 
-        // Initial condition of output enables
-        _fwd = 0;
-        _rev = 0;
+    else if (ServoKeyPress.data == 105) {   // i for Tilt Up
+        if(Servo2Pos>-45){
+        Servo2Pos = Servo2Pos-IncSize;
+        servo2.position(Servo2Pos);
+        wait(0.01);
+        }
     }
 
-    /*--------------------------------------------------------------------------------
-    Function name: Forwards
-    Input Parameters: float speed - PWM duty between 0-1
-    Output Parameters: N/A
-    Description: Drives the motor forwards at a designated speed
-    ----------------------------------------------------------------------------------*/
-    void cMotor::Forwards(float speed) {
-        _fwd = 1;
-        _rev = 0;
-        _pwm = speed;
+    else if (ServoKeyPress.data == 107) {   // K for Tilt Down
+        if(Servo2Pos<45){
+        Servo2Pos = Servo2Pos+IncSize;
+        servo2.position(Servo2Pos);
+        wait(0.01);
+         }
     }
 
-    /*--------------------------------------------------------------------------------
-    Function name: Backwards
-    Input Parameters: float speed - PWM duty between 0-1
-    Output Parameters: N/A
-    Description: Drives the motor backwards at a designated speed
-    ----------------------------------------------------------------------------------*/
-    void cMotor::Backwards(float speed) {
-        _fwd = 0;
-        _rev = 1;
-        _pwm = speed;
+    else if (ServoKeyPress.data == 44) {   //  for Up
+        if(IncSize<20) {
+            IncSize=(IncSize+1);
+        }
+    } else if (ServoKeyPress.data == 46) { //  for Up
+        if(IncSize>1) {
+            IncSize=(IncSize-1);
+        }
     }
 
-    /*--------------------------------------------------------------------------------
-    Function name: Stop
-    Input Parameters: N/A
-    Output Parameters: N/A
-    Description: Drives the motor backwards at a designated speed
-    ----------------------------------------------------------------------------------*/
-    void cMotor::Stop() {
-        _fwd = 0;
-        _rev = 0;
-        _pwm = 0;
-    }
\ No newline at end of file
+
+
+    pc.printf("Servo 1 = %d  Servo 2 = %d \n\r",Servo1Pos,Servo2Pos);
+    servo1.position(Servo1Pos);
+    servo2.position(Servo2Pos);
+
+}
+
+

--- a/Motors/Motor.h	Tue Dec 10 11:52:53 2019 +0000
+++ b/Motors/Motor.h	Sat Jan 04 21:35:25 2020 +0000
@@ -8,6 +8,7 @@
 #include "mbed.h"
 #include "Buzzer.h"
 #include <ros.h>
+#include <Servo.h>
 #include <std_msgs/UInt8.h>
 
 /*--------------------------------------------------------------------------------
@@ -27,6 +28,12 @@
 #define M2_PWM  PB_4
 #define M2_A    PB_5
 #define M2_B    PB_3
+#define srvoPan PE_5
+#define srvoTilt PF_9
+
+
+static Servo servo1(srvoTilt);
+static Servo servo2(srvoPan);
 
 /*--------------------------------------------------------------------------------
 ---------------------------------CLASSES------------------------------------------
@@ -53,11 +60,17 @@
 --------------------------------------------------------------------------------*/
 void Check_for_obstacles(uint8_t TOFRange[4]); //Obstacle avoidance functionality
 void update_power_levels(float vBatt);  //Sends power level to the LED class for further processing
-void keySub(const std_msgs::UInt8 &keyPress);
+void MotorKeySub(const std_msgs::UInt8 &keyPress);
+void servoKeySub(const std_msgs::UInt8 &ServoKeyPress);
+
 /*--------------------------------------------------------------------------------
 --------------------------------External Variables--------------------------------
 --------------------------------------------------------------------------------*/
 //extern cMotor MotorL;  //Left motor class and pin initialisation
 //extern cMotor MotorR;  //Right motor class and pin initialisation
 
+
+static int IncSize; //Increment size for servo movment in increments of 1 - 20 degrees
+ 
+
 #endif
\ No newline at end of file

--- a/RGB_LED/LED.h	Tue Dec 10 11:52:53 2019 +0000
+++ b/RGB_LED/LED.h	Sat Jan 04 21:35:25 2020 +0000
@@ -14,6 +14,7 @@
 
 class cRGB_LED
 {
+    
 public:
     cRGB_LED(DigitalOut DIAG_RED, PwmOut DIAG_BLU, PwmOut DIAG_GRN);  //Constructor, initialises the 3 outputs
     void red_led();     //Turns on Red LED

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo.lib	Sat Jan 04 21:35:25 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/simon/code/Servo/#36b69a7ced07

--- 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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.h	Sat Jan 04 21:35:25 2020 +0000
@@ -0,0 +1,17 @@
+/* main.h */
+#ifndef _MAIN_H_
+#define _MAIN_H_
+
+/* Includes ------------------------------------------------------------------*/
+#include "mbed.h"
+
+/* Defines -------------------------------------------------------------------*/
+#define PcBaud 9600 
+
+static Serial pc(SERIAL_TX, SERIAL_RX, PcBaud);    //set-up serial to pc
+
+
+
+#endif /* _MAIN_H_ */
+
+ 
\ No newline at end of file