SOFT564Z Group 3 / Mbed 2 deprecated SOFT564Z_Group_3_final

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed Servo ros_lib_kinetic

main.cpp@11:12e73437dc9f, 2020-01-10 (annotated)

Committer:
hongyunAHN
Date:
Fri Jan 10 15:05:11 2020 +0000
Revision:
11:12e73437dc9f
Parent:
10:51dd168b5a96 
ii

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"
Alexshawcroft 9:326b8f261ef0 6 #include "main.h"
Alexshawcroft 9:326b8f261ef0 7 #include <Servo.h>
Stumi 0:6021ec1b1449 8 #include "TOF.h"
Stumi 4:36a04230554d 9 #include "Motor.h"
Stumi 4:36a04230554d 10 #include "power.h"
Stumi 6:2cc2aac35868 11 #include "Buzzer.h"
Stumi 5:bc5081f0c063 12 #include "LED.h"
Stumi 8:262c6c40bff9 13 #include <ros.h>
Stumi 8:262c6c40bff9 14 #include <std_msgs/UInt8.h>
Stumi 8:262c6c40bff9 15
Alexshawcroft 9:326b8f261ef0 16
Alexshawcroft 9:326b8f261ef0 17
Alexshawcroft 9:326b8f261ef0 18 #include <std_msgs/UInt16.h>
Alexshawcroft 9:326b8f261ef0 19 #include <std_msgs/UInt8MultiArray.h>
Alexshawcroft 9:326b8f261ef0 20 #include <std_msgs/UInt16MultiArray.h>
Alexshawcroft 9:326b8f261ef0 21 #include <std_msgs/UInt32MultiArray.h>
Alexshawcroft 9:326b8f261ef0 22
Alexshawcroft 9:326b8f261ef0 23 #include <std_msgs/String.h>
Alexshawcroft 9:326b8f261ef0 24 #include <ros/time.h>
Alexshawcroft 9:326b8f261ef0 25 #include <sensor_msgs/Range.h>
Alexshawcroft 9:326b8f261ef0 26
Alexshawcroft 9:326b8f261ef0 27
Alexshawcroft 9:326b8f261ef0 28
Alexshawcroft 9:326b8f261ef0 29
Stumi 8:262c6c40bff9 30 class mySTM32 : public MbedHardware
Stumi 8:262c6c40bff9 31 {
Stumi 8:262c6c40bff9 32 public:
Stumi 8:262c6c40bff9 33 mySTM32(): MbedHardware(PD_5, PD_6, 57600) {};
Stumi 8:262c6c40bff9 34 };
Stumi 8:262c6c40bff9 35
Alexshawcroft 9:326b8f261ef0 36
Stumi 8:262c6c40bff9 37 ros::NodeHandle_<mySTM32> nh;
Stumi 8:262c6c40bff9 38
Alexshawcroft 9:326b8f261ef0 39 ros::Subscriber<std_msgs::UInt8> sub("keyControl", &MotorKeySub); // Subscriber for Motor Control by Keyboard.
Alexshawcroft 9:326b8f261ef0 40 ros::Subscriber<std_msgs::UInt8> sub1("ServoControl", &servoKeySub); // Subscriber for Servo Control by keyboard.
Alexshawcroft 9:326b8f261ef0 41
Stumi 10:51dd168b5a96 42 //TOF publisher
Stumi 10:51dd168b5a96 43 std_msgs::UInt8MultiArray range_msg;
Stumi 10:51dd168b5a96 44 ros::Publisher TOFreadings("TOFreadings", &range_msg);
Stumi 10:51dd168b5a96 45
Alexshawcroft 9:326b8f261ef0 46
Alexshawcroft 9:326b8f261ef0 47
Stumi 0:6021ec1b1449 48
Stumi 7:8248af58df5a 49 DigitalIn user_button(USER_BUTTON);
Stumi 7:8248af58df5a 50 float power_levels[3]; //Array to voltage levels
Stumi 7:8248af58df5a 51
Stumi 1:9bc7f95c3c7d 52
Stumi 7:8248af58df5a 53 Ticker power_monitor;
Stumi 7:8248af58df5a 54
Stumi 1:9bc7f95c3c7d 55 //TOF chip shutdown signals
Stumi 7:8248af58df5a 56 DigitalOut TOF1(PC_8);
Stumi 7:8248af58df5a 57 DigitalOut TOF4(PC_11);
Stumi 10:51dd168b5a96 58 DigitalOut TOF5(PC_12);
Stumi 10:51dd168b5a96 59 DigitalOut TOF6(PD_2);
Stumi 0:6021ec1b1449 60
Alexshawcroft 9:326b8f261ef0 61
Stumi 7:8248af58df5a 62 //Class defines
Stumi 10:51dd168b5a96 63 cAdafruit_VL6180X VL6180X(TOF1,TOF4,TOF5,TOF6); //Define TOF sensor class and initialise devices
Stumi 4:36a04230554d 64 cPower cPower(VBATT, V5, V3);
Stumi 0:6021ec1b1449 65
Alexshawcroft 9:326b8f261ef0 66 /*------------------------------------------------------------------------------
Stumi 7:8248af58df5a 67 Function name: power_check
Stumi 7:8248af58df5a 68 Input Parameters: N/A
Stumi 7:8248af58df5a 69 Output Parameters: N/A
Stumi 7:8248af58df5a 70 Description: Routine interrupt to monitor battery levels
Alexshawcroft 9:326b8f261ef0 71 ------------------------------------------------------------------------------*/
Alexshawcroft 9:326b8f261ef0 72
Alexshawcroft 9:326b8f261ef0 73
Alexshawcroft 9:326b8f261ef0 74 std_msgs::UInt8MultiArray m;
hongyunAHN 11:12e73437dc9f 75
Stumi 7:8248af58df5a 76
Alexshawcroft 9:326b8f261ef0 77
Alexshawcroft 9:326b8f261ef0 78 float TOFRange[4];
Alexshawcroft 9:326b8f261ef0 79
Alexshawcroft 9:326b8f261ef0 80 DigitalOut led = LED1;
Alexshawcroft 9:326b8f261ef0 81
Stumi 10:51dd168b5a96 82
Alexshawcroft 9:326b8f261ef0 83 /* Private Functions----------------------------------------------------------*/
Alexshawcroft 9:326b8f261ef0 84 void power_check(void);
Stumi 7:8248af58df5a 85
Stumi 7:8248af58df5a 86
Stumi 7:8248af58df5a 87 /*--------------------------------------------------------------------------------
Stumi 7:8248af58df5a 88 MAIN CONTROL LOOP
Alexshawcroft 9:326b8f261ef0 89 -------------------------------------------------------------------------------*/
Stumi 0:6021ec1b1449 90 int main()
Stumi 0:6021ec1b1449 91 {
hongyunAHN 11:12e73437dc9f 92
Alexshawcroft 9:326b8f261ef0 93
Stumi 8:262c6c40bff9 94 nh.initNode();
Stumi 8:262c6c40bff9 95 nh.subscribe(sub);
Alexshawcroft 9:326b8f261ef0 96 nh.subscribe(sub1);
hongyunAHN 11:12e73437dc9f 97
hongyunAHN 11:12e73437dc9f 98
Stumi 10:51dd168b5a96 99 //Publishers
Stumi 10:51dd168b5a96 100 nh.advertise(TOFreadings); //TOF readings
Stumi 10:51dd168b5a96 101
Stumi 10:51dd168b5a96 102 //TOF publisher parameters
Stumi 10:51dd168b5a96 103 range_msg.layout.dim = (std_msgs::MultiArrayDimension *)
Stumi 10:51dd168b5a96 104 malloc(sizeof(std_msgs::MultiArrayDimension) * 2);
Stumi 10:51dd168b5a96 105 range_msg.layout.dim[0].label = "TOF readings";
Stumi 10:51dd168b5a96 106 range_msg.layout.dim[0].size = 4;
Stumi 10:51dd168b5a96 107 range_msg.layout.dim[0].stride = 1*4;
Stumi 10:51dd168b5a96 108 range_msg.layout.data_offset = 0;
Stumi 10:51dd168b5a96 109 range_msg.data = (uint8_t *)malloc(sizeof(int)*4);
Stumi 10:51dd168b5a96 110 range_msg.data_length = 4;
Alexshawcroft 9:326b8f261ef0 111
Alexshawcroft 9:326b8f261ef0 112 servo1.position(0);
Alexshawcroft 9:326b8f261ef0 113 servo1.position(0);
Stumi 10:51dd168b5a96 114 power_monitor.attach(power_check, 30.0); //Monitor battery every 30 seconds
Stumi 0:6021ec1b1449 115
Stumi 7:8248af58df5a 116 //Wait for user button to be pressed
Stumi 7:8248af58df5a 117 pc.printf("Press user button to start\n\r");
Stumi 8:262c6c40bff9 118
Stumi 7:8248af58df5a 119 while(user_button != 1) {}
Stumi 7:8248af58df5a 120
Stumi 7:8248af58df5a 121 while(1) {
Stumi 8:262c6c40bff9 122
Alexshawcroft 9:326b8f261ef0 123
Stumi 10:51dd168b5a96 124 //Perform TOF measurements
Stumi 4:36a04230554d 125 TOFRange[0] = serviceTOF(VL6180X, ADDR1);
Stumi 4:36a04230554d 126 TOFRange[1] = serviceTOF(VL6180X, ADDR4);
Stumi 10:51dd168b5a96 127 TOFRange[2] = serviceTOF(VL6180X, ADDR5);
Stumi 10:51dd168b5a96 128 TOFRange[3] = serviceTOF(VL6180X, ADDR6);
hongyunAHN 11:12e73437dc9f 129 //
Stumi 10:51dd168b5a96 130 TOFrangePtr[0] = &TOFRange[0];
Stumi 10:51dd168b5a96 131 TOFrangePtr[1] = &TOFRange[1];
Stumi 10:51dd168b5a96 132 TOFrangePtr[2] = &TOFRange[2];
Stumi 10:51dd168b5a96 133 TOFrangePtr[3] = &TOFRange[3];
Stumi 10:51dd168b5a96 134
Stumi 10:51dd168b5a96 135 //Assign to TOFreadings publisher
Stumi 10:51dd168b5a96 136 range_msg.data[0] = TOFRange[0];
Stumi 10:51dd168b5a96 137 range_msg.data[1] = TOFRange[1];
Stumi 10:51dd168b5a96 138 range_msg.data[2] = TOFRange[2];
Stumi 10:51dd168b5a96 139 range_msg.data[3] = TOFRange[3];
Stumi 10:51dd168b5a96 140
Stumi 10:51dd168b5a96 141 //Publish data
Stumi 10:51dd168b5a96 142 TOFreadings.publish(&range_msg);
Alexshawcroft 9:326b8f261ef0 143 // Need to write code like the obstical ovaoidance, to stop the motors, by the nucleo
Alexshawcroft 9:326b8f261ef0 144 // sending a char to the PI
Alexshawcroft 9:326b8f261ef0 145
hongyunAHN 11:12e73437dc9f 146
Stumi 8:262c6c40bff9 147 nh.spinOnce();
Alexshawcroft 9:326b8f261ef0 148 wait_ms(5);
Stumi 0:6021ec1b1449 149 }
Alexshawcroft 9:326b8f261ef0 150 }
Alexshawcroft 9:326b8f261ef0 151
Alexshawcroft 9:326b8f261ef0 152 void power_check()
Alexshawcroft 9:326b8f261ef0 153 {
Alexshawcroft 9:326b8f261ef0 154 power_levels[0] = cPower.monitor_battery(); //Monitors raw battery
Alexshawcroft 9:326b8f261ef0 155 power_levels[1] = cPower.monitor_5v_line(); //Monitors 5V line
Alexshawcroft 9:326b8f261ef0 156 power_levels[2] = cPower.monitor_3v3_line();//Monitors 3V3 Line
Alexshawcroft 9:326b8f261ef0 157
Alexshawcroft 9:326b8f261ef0 158 update_power_levels(power_levels[0]); //Sends the raw battery levels to the LED class
Stumi 0:6021ec1b1449 159 }