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@10:51dd168b5a96, 2020-01-06 (annotated)

Committer:
Stumi
Date:
Mon Jan 06 20:38:07 2020 +0000
Revision:
10:51dd168b5a96
Parent:
9:326b8f261ef0
Child:
11:12e73437dc9f
added autonomous control to ros commands

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 /*
Alexshawcroft 9:326b8f261ef0 37 void servo1_cb( const std_msgs::UInt16& cmd_msg) {
Alexshawcroft 9:326b8f261ef0 38 servo1.position(cmd_msg.data); //set servo angle, should be from 0-180
Alexshawcroft 9:326b8f261ef0 39 }
Alexshawcroft 9:326b8f261ef0 40 void servo2_cb( const std_msgs::UInt16& cmd_msg) {
Alexshawcroft 9:326b8f261ef0 41 servo2.position(cmd_msg.data); //set servo angle, should be from 0-180
Alexshawcroft 9:326b8f261ef0 42 }
Alexshawcroft 9:326b8f261ef0 43 */
Alexshawcroft 9:326b8f261ef0 44
Stumi 8:262c6c40bff9 45 ros::NodeHandle_<mySTM32> nh;
Stumi 8:262c6c40bff9 46
Alexshawcroft 9:326b8f261ef0 47 ros::Subscriber<std_msgs::UInt8> sub("keyControl", &MotorKeySub); // Subscriber for Motor Control by Keyboard.
Alexshawcroft 9:326b8f261ef0 48 ros::Subscriber<std_msgs::UInt8> sub1("ServoControl", &servoKeySub); // Subscriber for Servo Control by keyboard.
Alexshawcroft 9:326b8f261ef0 49
Stumi 10:51dd168b5a96 50 //TOF publisher
Stumi 10:51dd168b5a96 51 std_msgs::UInt8MultiArray range_msg;
Stumi 10:51dd168b5a96 52 ros::Publisher TOFreadings("TOFreadings", &range_msg);
Stumi 10:51dd168b5a96 53
Alexshawcroft 9:326b8f261ef0 54 //std_msgs::UInt16MultiArray range_msg;
Alexshawcroft 9:326b8f261ef0 55 //ros::Publisher TOFstuff("TOFstuff", &range_msg);
Alexshawcroft 9:326b8f261ef0 56
Stumi 10:51dd168b5a96 57 /*sensor_msgs::Range range_msg1, range_msg2,range_msg3,range_msg4;
Alexshawcroft 9:326b8f261ef0 58 ros::Publisher tof1("tof1", &range_msg1);
Alexshawcroft 9:326b8f261ef0 59 ros::Publisher tof2("tof2", &range_msg2);
Alexshawcroft 9:326b8f261ef0 60 ros::Publisher tof3("tof3", &range_msg3);
Stumi 10:51dd168b5a96 61 ros::Publisher tof4("tof4", &range_msg4);*/
Alexshawcroft 9:326b8f261ef0 62
Alexshawcroft 9:326b8f261ef0 63 //std_msgs::Int32MultiArray range_msg;
Alexshawcroft 9:326b8f261ef0 64 //array.data.clear();
Alexshawcroft 9:326b8f261ef0 65
Alexshawcroft 9:326b8f261ef0 66
Alexshawcroft 9:326b8f261ef0 67 /* NOT USED ANYMORE */
Alexshawcroft 9:326b8f261ef0 68 //ros::Subscriber<std_msgs::UInt16> sub2("servo1", servo1_cb);
Alexshawcroft 9:326b8f261ef0 69 //ros::Subscriber<std_msgs::UInt16> sub3("servo2", servo2_cb);
Alexshawcroft 9:326b8f261ef0 70
Stumi 0:6021ec1b1449 71
Stumi 7:8248af58df5a 72 DigitalIn user_button(USER_BUTTON);
Stumi 7:8248af58df5a 73 float power_levels[3]; //Array to voltage levels
Stumi 7:8248af58df5a 74
Stumi 1:9bc7f95c3c7d 75
Stumi 7:8248af58df5a 76 Ticker power_monitor;
Stumi 7:8248af58df5a 77
Stumi 1:9bc7f95c3c7d 78 //TOF chip shutdown signals
Stumi 7:8248af58df5a 79 DigitalOut TOF1(PC_8);
Stumi 7:8248af58df5a 80 DigitalOut TOF4(PC_11);
Stumi 10:51dd168b5a96 81 DigitalOut TOF5(PC_12);
Stumi 10:51dd168b5a96 82 DigitalOut TOF6(PD_2);
Stumi 0:6021ec1b1449 83
Alexshawcroft 9:326b8f261ef0 84
Stumi 7:8248af58df5a 85 //Class defines
Stumi 10:51dd168b5a96 86 cAdafruit_VL6180X VL6180X(TOF1,TOF4,TOF5,TOF6); //Define TOF sensor class and initialise devices
Stumi 4:36a04230554d 87 cPower cPower(VBATT, V5, V3);
Stumi 0:6021ec1b1449 88
Alexshawcroft 9:326b8f261ef0 89 /*------------------------------------------------------------------------------
Stumi 7:8248af58df5a 90 Function name: power_check
Stumi 7:8248af58df5a 91 Input Parameters: N/A
Stumi 7:8248af58df5a 92 Output Parameters: N/A
Stumi 7:8248af58df5a 93 Description: Routine interrupt to monitor battery levels
Alexshawcroft 9:326b8f261ef0 94 ------------------------------------------------------------------------------*/
Alexshawcroft 9:326b8f261ef0 95
Alexshawcroft 9:326b8f261ef0 96
Alexshawcroft 9:326b8f261ef0 97 std_msgs::UInt8MultiArray m;
Alexshawcroft 9:326b8f261ef0 98 //sensor_msgs::Range range_msg[4];
Stumi 7:8248af58df5a 99
Alexshawcroft 9:326b8f261ef0 100
Alexshawcroft 9:326b8f261ef0 101 float TOFRange[4];
Alexshawcroft 9:326b8f261ef0 102
Alexshawcroft 9:326b8f261ef0 103 DigitalOut led = LED1;
Stumi 10:51dd168b5a96 104 /*
Alexshawcroft 9:326b8f261ef0 105 Timer t; // Timer
Alexshawcroft 9:326b8f261ef0 106
Alexshawcroft 9:326b8f261ef0 107 char frameid1[] = "/Rear Sensor";
Alexshawcroft 9:326b8f261ef0 108 char frameid2[] = "/Front Left Sensor";
Alexshawcroft 9:326b8f261ef0 109 char frameid3[] = "/Front Center Sensor";
Stumi 10:51dd168b5a96 110 char frameid4[] = "/Front Right Sensor";*/
Stumi 10:51dd168b5a96 111
Alexshawcroft 9:326b8f261ef0 112 /* Private Functions----------------------------------------------------------*/
Alexshawcroft 9:326b8f261ef0 113 void power_check(void);
Stumi 7:8248af58df5a 114
Stumi 7:8248af58df5a 115
Stumi 7:8248af58df5a 116 /*--------------------------------------------------------------------------------
Stumi 7:8248af58df5a 117 MAIN CONTROL LOOP
Alexshawcroft 9:326b8f261ef0 118 -------------------------------------------------------------------------------*/
Stumi 0:6021ec1b1449 119 int main()
Stumi 0:6021ec1b1449 120 {
Alexshawcroft 9:326b8f261ef0 121 //t.start();
Stumi 10:51dd168b5a96 122 //IncSize=1;
Alexshawcroft 9:326b8f261ef0 123
Stumi 8:262c6c40bff9 124 nh.initNode();
Stumi 8:262c6c40bff9 125 nh.subscribe(sub);
Alexshawcroft 9:326b8f261ef0 126 nh.subscribe(sub1);
Alexshawcroft 9:326b8f261ef0 127 //nh.subscribe(sub2);
Stumi 8:262c6c40bff9 128
Stumi 10:51dd168b5a96 129 /*nh.advertise(tof1);
Alexshawcroft 9:326b8f261ef0 130 nh.advertise(tof2);
Alexshawcroft 9:326b8f261ef0 131 nh.advertise(tof3);
Stumi 10:51dd168b5a96 132 nh.advertise(tof4);*/
Stumi 10:51dd168b5a96 133
Stumi 10:51dd168b5a96 134 //Publishers
Stumi 10:51dd168b5a96 135 nh.advertise(TOFreadings); //TOF readings
Stumi 10:51dd168b5a96 136
Stumi 10:51dd168b5a96 137 //TOF publisher parameters
Stumi 10:51dd168b5a96 138 range_msg.layout.dim = (std_msgs::MultiArrayDimension *)
Stumi 10:51dd168b5a96 139 malloc(sizeof(std_msgs::MultiArrayDimension) * 2);
Stumi 10:51dd168b5a96 140 range_msg.layout.dim[0].label = "TOF readings";
Stumi 10:51dd168b5a96 141 range_msg.layout.dim[0].size = 4;
Stumi 10:51dd168b5a96 142 range_msg.layout.dim[0].stride = 1*4;
Stumi 10:51dd168b5a96 143 range_msg.layout.data_offset = 0;
Stumi 10:51dd168b5a96 144 range_msg.data = (uint8_t *)malloc(sizeof(int)*4);
Stumi 10:51dd168b5a96 145 range_msg.data_length = 4;
Alexshawcroft 9:326b8f261ef0 146
Alexshawcroft 9:326b8f261ef0 147 servo1.position(0);
Alexshawcroft 9:326b8f261ef0 148 servo1.position(0);
Stumi 10:51dd168b5a96 149 power_monitor.attach(power_check, 30.0); //Monitor battery every 30 seconds
Stumi 0:6021ec1b1449 150
Stumi 7:8248af58df5a 151 //Wait for user button to be pressed
Stumi 7:8248af58df5a 152 pc.printf("Press user button to start\n\r");
Stumi 8:262c6c40bff9 153
Stumi 7:8248af58df5a 154 while(user_button != 1) {}
Stumi 7:8248af58df5a 155
Stumi 7:8248af58df5a 156 while(1) {
Stumi 8:262c6c40bff9 157
Alexshawcroft 9:326b8f261ef0 158
Stumi 10:51dd168b5a96 159 //Perform TOF measurements
Stumi 4:36a04230554d 160 TOFRange[0] = serviceTOF(VL6180X, ADDR1);
Stumi 4:36a04230554d 161 TOFRange[1] = serviceTOF(VL6180X, ADDR4);
Stumi 10:51dd168b5a96 162 TOFRange[2] = serviceTOF(VL6180X, ADDR5);
Stumi 10:51dd168b5a96 163 TOFRange[3] = serviceTOF(VL6180X, ADDR6);
Stumi 4:36a04230554d 164
Stumi 10:51dd168b5a96 165 TOFrangePtr[0] = &TOFRange[0];
Stumi 10:51dd168b5a96 166 TOFrangePtr[1] = &TOFRange[1];
Stumi 10:51dd168b5a96 167 TOFrangePtr[2] = &TOFRange[2];
Stumi 10:51dd168b5a96 168 TOFrangePtr[3] = &TOFRange[3];
Stumi 10:51dd168b5a96 169
Stumi 10:51dd168b5a96 170 //Assign to TOFreadings publisher
Stumi 10:51dd168b5a96 171 range_msg.data[0] = TOFRange[0];
Stumi 10:51dd168b5a96 172 range_msg.data[1] = TOFRange[1];
Stumi 10:51dd168b5a96 173 range_msg.data[2] = TOFRange[2];
Stumi 10:51dd168b5a96 174 range_msg.data[3] = TOFRange[3];
Stumi 10:51dd168b5a96 175
Stumi 10:51dd168b5a96 176 //Publish data
Stumi 10:51dd168b5a96 177 TOFreadings.publish(&range_msg);
Stumi 10:51dd168b5a96 178 /* //Check_for_obstacles(TOFRange);
Alexshawcroft 9:326b8f261ef0 179 range_msg1.header.frame_id =frameid1;
Alexshawcroft 9:326b8f261ef0 180 range_msg2.header.frame_id =frameid2;
Alexshawcroft 9:326b8f261ef0 181 range_msg3.header.frame_id =frameid3;
Alexshawcroft 9:326b8f261ef0 182 range_msg4.header.frame_id =frameid4;
Alexshawcroft 9:326b8f261ef0 183
Alexshawcroft 9:326b8f261ef0 184 range_msg1.range = TOFRange[0];
Alexshawcroft 9:326b8f261ef0 185 range_msg2.range = TOFRange[1];
Alexshawcroft 9:326b8f261ef0 186 range_msg3.range = TOFRange[2];
Alexshawcroft 9:326b8f261ef0 187 range_msg4.range = TOFRange[3];
Alexshawcroft 9:326b8f261ef0 188 tof1.publish(&range_msg1);
Alexshawcroft 9:326b8f261ef0 189 tof2.publish(&range_msg2);
Alexshawcroft 9:326b8f261ef0 190 tof3.publish(&range_msg3);
Stumi 10:51dd168b5a96 191 tof4.publish(&range_msg4);*/
Alexshawcroft 9:326b8f261ef0 192
Alexshawcroft 9:326b8f261ef0 193 // Need to write code like the obstical ovaoidance, to stop the motors, by the nucleo
Alexshawcroft 9:326b8f261ef0 194 // sending a char to the PI
Alexshawcroft 9:326b8f261ef0 195
Alexshawcroft 9:326b8f261ef0 196 //pc.printf("Spinning...");
Stumi 8:262c6c40bff9 197 nh.spinOnce();
Alexshawcroft 9:326b8f261ef0 198 wait_ms(5);
Stumi 0:6021ec1b1449 199 }
Alexshawcroft 9:326b8f261ef0 200 }
Alexshawcroft 9:326b8f261ef0 201
Alexshawcroft 9:326b8f261ef0 202 void power_check()
Alexshawcroft 9:326b8f261ef0 203 {
Alexshawcroft 9:326b8f261ef0 204 power_levels[0] = cPower.monitor_battery(); //Monitors raw battery
Alexshawcroft 9:326b8f261ef0 205 power_levels[1] = cPower.monitor_5v_line(); //Monitors 5V line
Alexshawcroft 9:326b8f261ef0 206 power_levels[2] = cPower.monitor_3v3_line();//Monitors 3V3 Line
Alexshawcroft 9:326b8f261ef0 207
Alexshawcroft 9:326b8f261ef0 208 update_power_levels(power_levels[0]); //Sends the raw battery levels to the LED class
Stumi 0:6021ec1b1449 209 }