with the tof code
Dependencies: mbed Servo ros_lib_kinetic
Diff: main.cpp
- Revision:
- 9:326b8f261ef0
- Parent:
- 8:262c6c40bff9
--- 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