SOFT564Z Group 3 / Mbed 2 deprecated SOFT564Z_Group_3v3

with the tof code

Dependencies:   mbed Servo ros_lib_kinetic

main.cpp@8:262c6c40bff9, 2019-12-10 (annotated)

Committer:
Stumi
Date:
Tue Dec 10 11:52:53 2019 +0000
Revision:
8:262c6c40bff9
Parent:
7:8248af58df5a
Child:
9:326b8f261ef0
Communicates keyboard inputs to the robot using rosnodes;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Stumi 0:6021ec1b1449 1 /*--------------------------------------------------------------------------------
Stumi 0:6021ec1b1449 2 Filename: main.cpp
Stumi 0:6021ec1b1449 3 Description: Main program loop
Stumi 0:6021ec1b1449 4 --------------------------------------------------------------------------------*/
Stumi 0:6021ec1b1449 5 #include "mbed.h"
Stumi 0:6021ec1b1449 6 #include "TOF.h"
Stumi 4:36a04230554d 7 #include "Motor.h"
Stumi 4:36a04230554d 8 #include "power.h"
Stumi 6:2cc2aac35868 9 #include "Buzzer.h"
Stumi 5:bc5081f0c063 10 #include "LED.h"
Stumi 8:262c6c40bff9 11 #include <ros.h>
Stumi 8:262c6c40bff9 12 #include <std_msgs/UInt8.h>
Stumi 8:262c6c40bff9 13
Stumi 8:262c6c40bff9 14 class mySTM32 : public MbedHardware
Stumi 8:262c6c40bff9 15 {
Stumi 8:262c6c40bff9 16 public:
Stumi 8:262c6c40bff9 17 mySTM32(): MbedHardware(PD_5, PD_6, 57600) {};
Stumi 8:262c6c40bff9 18 };
Stumi 8:262c6c40bff9 19
Stumi 8:262c6c40bff9 20 ros::NodeHandle_<mySTM32> nh;
Stumi 8:262c6c40bff9 21
Stumi 8:262c6c40bff9 22 ros::Subscriber<std_msgs::UInt8> sub("keyControl", &keySub);
Stumi 0:6021ec1b1449 23
Stumi 7:8248af58df5a 24 DigitalIn user_button(USER_BUTTON);
Stumi 7:8248af58df5a 25 float power_levels[3]; //Array to voltage levels
Stumi 7:8248af58df5a 26
Stumi 8:262c6c40bff9 27 Serial pc(SERIAL_TX, SERIAL_RX, 9600); //set-up serial to pc
Stumi 1:9bc7f95c3c7d 28
Stumi 7:8248af58df5a 29 Ticker power_monitor;
Stumi 7:8248af58df5a 30
Stumi 1:9bc7f95c3c7d 31 //TOF chip shutdown signals
Stumi 7:8248af58df5a 32 DigitalOut TOF1(PC_8);
Stumi 7:8248af58df5a 33 DigitalOut TOF4(PC_11);
Stumi 7:8248af58df5a 34 DigitalOut TOF6(PC_12);
Stumi 7:8248af58df5a 35 DigitalOut TOF7(PD_2);
Stumi 0:6021ec1b1449 36
Stumi 7:8248af58df5a 37 //Class defines
Stumi 4:36a04230554d 38 cAdafruit_VL6180X VL6180X(TOF1,TOF4,TOF6,TOF7); //Define TOF sensor class and initialise devices
Stumi 4:36a04230554d 39 cPower cPower(VBATT, V5, V3);
Stumi 0:6021ec1b1449 40
Stumi 7:8248af58df5a 41 /*--------------------------------------------------------------------------------
Stumi 7:8248af58df5a 42 Function name: power_check
Stumi 7:8248af58df5a 43 Input Parameters: N/A
Stumi 7:8248af58df5a 44 Output Parameters: N/A
Stumi 7:8248af58df5a 45 Description: Routine interrupt to monitor battery levels
Stumi 7:8248af58df5a 46 ----------------------------------------------------------------------------------*/
Stumi 7:8248af58df5a 47 void power_check()
Stumi 7:8248af58df5a 48 {
Stumi 7:8248af58df5a 49 power_levels[0] = cPower.monitor_battery(); //Monitors raw battery
Stumi 7:8248af58df5a 50 power_levels[1] = cPower.monitor_5v_line(); //Monitors 5V line
Stumi 7:8248af58df5a 51 power_levels[2] = cPower.monitor_3v3_line();//Monitors 3V3 Line
Stumi 7:8248af58df5a 52
Stumi 7:8248af58df5a 53 update_power_levels(power_levels[0]); //Sends the raw battery levels to the LED class
Stumi 7:8248af58df5a 54 }
Stumi 7:8248af58df5a 55
Stumi 7:8248af58df5a 56
Stumi 7:8248af58df5a 57 /*--------------------------------------------------------------------------------
Stumi 7:8248af58df5a 58 MAIN CONTROL LOOP
Stumi 7:8248af58df5a 59 ----------------------------------------------------------------------------------*/
Stumi 0:6021ec1b1449 60 int main()
Stumi 0:6021ec1b1449 61 {
Stumi 8:262c6c40bff9 62 nh.initNode();
Stumi 8:262c6c40bff9 63 nh.subscribe(sub);
Stumi 8:262c6c40bff9 64
Stumi 7:8248af58df5a 65 power_monitor.attach(power_check, 30.0); //Monitor battery every 30 seconds
Stumi 8:262c6c40bff9 66
Stumi 4:36a04230554d 67 uint8_t TOFRange[4]; //Array to store TOF measurements 0-sensor1, 1-sensor4, 2-sensor6, 3-sensor7
Stumi 0:6021ec1b1449 68
Stumi 7:8248af58df5a 69 //Wait for user button to be pressed
Stumi 7:8248af58df5a 70 pc.printf("Press user button to start\n\r");
Stumi 8:262c6c40bff9 71
Stumi 7:8248af58df5a 72 while(user_button != 1) {}
Stumi 7:8248af58df5a 73
Stumi 7:8248af58df5a 74 while(1) {
Stumi 8:262c6c40bff9 75
Stumi 8:262c6c40bff9 76 /*
Stumi 1:9bc7f95c3c7d 77 //Perform TOF measurements
Stumi 4:36a04230554d 78 TOFRange[0] = serviceTOF(VL6180X, ADDR1);
Stumi 4:36a04230554d 79 TOFRange[1] = serviceTOF(VL6180X, ADDR4);
Stumi 4:36a04230554d 80 TOFRange[2] = serviceTOF(VL6180X, ADDR6);
Stumi 4:36a04230554d 81 TOFRange[3] = serviceTOF(VL6180X, ADDR7);
Stumi 4:36a04230554d 82
Stumi 7:8248af58df5a 83 Check_for_obstacles(TOFRange); //Run obstacle avoidance
Stumi 8:262c6c40bff9 84 */
Stumi 8:262c6c40bff9 85 pc.printf("Spinning...");
Stumi 8:262c6c40bff9 86 nh.spinOnce();
Stumi 8:262c6c40bff9 87 wait_ms(1);
Stumi 0:6021ec1b1449 88 }
Stumi 0:6021ec1b1449 89 }