SOFT564Z Group 3 / Mbed 2 deprecated SOFT564Z_Group_3v46666

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:276cc357015c, 2020-01-05 (annotated)

Committer:
hongyunAHN
Date:
Sun Jan 05 21:47:14 2020 +0000
Revision:
10:276cc357015c
Parent:
9:326b8f261ef0 
a

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
Alexshawcroft 9:326b8f261ef0 50 //std_msgs::UInt16MultiArray range_msg;
Alexshawcroft 9:326b8f261ef0 51 //ros::Publisher TOFstuff("TOFstuff", &range_msg);
Alexshawcroft 9:326b8f261ef0 52
Alexshawcroft 9:326b8f261ef0 53 sensor_msgs::Range range_msg1, range_msg2,range_msg3,range_msg4;
Alexshawcroft 9:326b8f261ef0 54 ros::Publisher tof1("tof1", &range_msg1);
Alexshawcroft 9:326b8f261ef0 55 ros::Publisher tof2("tof2", &range_msg2);
Alexshawcroft 9:326b8f261ef0 56 ros::Publisher tof3("tof3", &range_msg3);
Alexshawcroft 9:326b8f261ef0 57 ros::Publisher tof4("tof4", &range_msg4);
Alexshawcroft 9:326b8f261ef0 58
Alexshawcroft 9:326b8f261ef0 59 //std_msgs::Int32MultiArray range_msg;
Alexshawcroft 9:326b8f261ef0 60 //array.data.clear();
Alexshawcroft 9:326b8f261ef0 61
Alexshawcroft 9:326b8f261ef0 62
Alexshawcroft 9:326b8f261ef0 63 /* NOT USED ANYMORE */
Alexshawcroft 9:326b8f261ef0 64 //ros::Subscriber<std_msgs::UInt16> sub2("servo1", servo1_cb);
Alexshawcroft 9:326b8f261ef0 65 //ros::Subscriber<std_msgs::UInt16> sub3("servo2", servo2_cb);
Alexshawcroft 9:326b8f261ef0 66
Stumi 0:6021ec1b1449 67
Stumi 7:8248af58df5a 68 DigitalIn user_button(USER_BUTTON);
Stumi 7:8248af58df5a 69 float power_levels[3]; //Array to voltage levels
Stumi 7:8248af58df5a 70
Stumi 1:9bc7f95c3c7d 71
Stumi 7:8248af58df5a 72 Ticker power_monitor;
Stumi 7:8248af58df5a 73
Stumi 1:9bc7f95c3c7d 74 //TOF chip shutdown signals
Stumi 7:8248af58df5a 75 DigitalOut TOF1(PC_8);
Stumi 7:8248af58df5a 76 DigitalOut TOF4(PC_11);
Stumi 7:8248af58df5a 77 DigitalOut TOF6(PC_12);
Stumi 7:8248af58df5a 78 DigitalOut TOF7(PD_2);
Stumi 0:6021ec1b1449 79
Alexshawcroft 9:326b8f261ef0 80
Stumi 7:8248af58df5a 81 //Class defines
Stumi 4:36a04230554d 82 cAdafruit_VL6180X VL6180X(TOF1,TOF4,TOF6,TOF7); //Define TOF sensor class and initialise devices
Stumi 4:36a04230554d 83 cPower cPower(VBATT, V5, V3);
Stumi 0:6021ec1b1449 84
Alexshawcroft 9:326b8f261ef0 85 /*------------------------------------------------------------------------------
Stumi 7:8248af58df5a 86 Function name: power_check
Stumi 7:8248af58df5a 87 Input Parameters: N/A
Stumi 7:8248af58df5a 88 Output Parameters: N/A
Stumi 7:8248af58df5a 89 Description: Routine interrupt to monitor battery levels
Alexshawcroft 9:326b8f261ef0 90 ------------------------------------------------------------------------------*/
Alexshawcroft 9:326b8f261ef0 91
Alexshawcroft 9:326b8f261ef0 92
Alexshawcroft 9:326b8f261ef0 93 std_msgs::UInt8MultiArray m;
Alexshawcroft 9:326b8f261ef0 94 //sensor_msgs::Range range_msg[4];
Stumi 7:8248af58df5a 95
Alexshawcroft 9:326b8f261ef0 96
Alexshawcroft 9:326b8f261ef0 97 float TOFRange[4];
Alexshawcroft 9:326b8f261ef0 98
Alexshawcroft 9:326b8f261ef0 99 DigitalOut led = LED1;
Alexshawcroft 9:326b8f261ef0 100 Timer t; // Timer
Alexshawcroft 9:326b8f261ef0 101
Alexshawcroft 9:326b8f261ef0 102 char frameid1[] = "/Rear Sensor";
Alexshawcroft 9:326b8f261ef0 103 char frameid2[] = "/Front Left Sensor";
Alexshawcroft 9:326b8f261ef0 104 char frameid3[] = "/Front Center Sensor";
Alexshawcroft 9:326b8f261ef0 105 char frameid4[] = "/Front Right Sensor";
Alexshawcroft 9:326b8f261ef0 106 /* Private Functions----------------------------------------------------------*/
Alexshawcroft 9:326b8f261ef0 107 void power_check(void);
Stumi 7:8248af58df5a 108
Stumi 7:8248af58df5a 109
Stumi 7:8248af58df5a 110 /*--------------------------------------------------------------------------------
Stumi 7:8248af58df5a 111 MAIN CONTROL LOOP
Alexshawcroft 9:326b8f261ef0 112 -------------------------------------------------------------------------------*/
Stumi 0:6021ec1b1449 113 int main()
Stumi 0:6021ec1b1449 114 {
Alexshawcroft 9:326b8f261ef0 115 //t.start();
Alexshawcroft 9:326b8f261ef0 116 IncSize=1;
Alexshawcroft 9:326b8f261ef0 117
Stumi 8:262c6c40bff9 118 nh.initNode();
Stumi 8:262c6c40bff9 119 nh.subscribe(sub);
Alexshawcroft 9:326b8f261ef0 120 nh.subscribe(sub1);
Alexshawcroft 9:326b8f261ef0 121 //nh.subscribe(sub2);
Stumi 8:262c6c40bff9 122
Alexshawcroft 9:326b8f261ef0 123 nh.advertise(tof1);
Alexshawcroft 9:326b8f261ef0 124 nh.advertise(tof2);
Alexshawcroft 9:326b8f261ef0 125 nh.advertise(tof3);
Alexshawcroft 9:326b8f261ef0 126 nh.advertise(tof4);
Alexshawcroft 9:326b8f261ef0 127
Alexshawcroft 9:326b8f261ef0 128 servo1.position(0);
Alexshawcroft 9:326b8f261ef0 129 servo1.position(0);
Stumi 7:8248af58df5a 130 power_monitor.attach(power_check, 30.0); //Monitor battery every 30 seconds
Stumi 8:262c6c40bff9 131
Stumi 0:6021ec1b1449 132
Stumi 7:8248af58df5a 133 //Wait for user button to be pressed
Stumi 7:8248af58df5a 134 pc.printf("Press user button to start\n\r");
Stumi 8:262c6c40bff9 135
Stumi 7:8248af58df5a 136 while(user_button != 1) {}
Stumi 7:8248af58df5a 137
Stumi 7:8248af58df5a 138 while(1) {
Stumi 8:262c6c40bff9 139
Alexshawcroft 9:326b8f261ef0 140
Alexshawcroft 9:326b8f261ef0 141 //Perform TOF measurements
Stumi 4:36a04230554d 142 TOFRange[0] = serviceTOF(VL6180X, ADDR1);
Stumi 4:36a04230554d 143 TOFRange[1] = serviceTOF(VL6180X, ADDR4);
Stumi 4:36a04230554d 144 TOFRange[2] = serviceTOF(VL6180X, ADDR6);
Stumi 4:36a04230554d 145 TOFRange[3] = serviceTOF(VL6180X, ADDR7);
Stumi 4:36a04230554d 146
Alexshawcroft 9:326b8f261ef0 147 //Check_for_obstacles(TOFRange); //Run obstacle avoidance
Alexshawcroft 9:326b8f261ef0 148 range_msg1.header.frame_id =frameid1;
Alexshawcroft 9:326b8f261ef0 149 range_msg2.header.frame_id =frameid2;
Alexshawcroft 9:326b8f261ef0 150 range_msg3.header.frame_id =frameid3;
Alexshawcroft 9:326b8f261ef0 151 range_msg4.header.frame_id =frameid4;
Alexshawcroft 9:326b8f261ef0 152
Alexshawcroft 9:326b8f261ef0 153 range_msg1.range = TOFRange[0];
Alexshawcroft 9:326b8f261ef0 154 range_msg2.range = TOFRange[1];
Alexshawcroft 9:326b8f261ef0 155 range_msg3.range = TOFRange[2];
Alexshawcroft 9:326b8f261ef0 156 range_msg4.range = TOFRange[3];
Alexshawcroft 9:326b8f261ef0 157 tof1.publish(&range_msg1);
Alexshawcroft 9:326b8f261ef0 158 tof2.publish(&range_msg2);
Alexshawcroft 9:326b8f261ef0 159 tof3.publish(&range_msg3);
Alexshawcroft 9:326b8f261ef0 160 tof4.publish(&range_msg4);
Alexshawcroft 9:326b8f261ef0 161
Alexshawcroft 9:326b8f261ef0 162 // Need to write code like the obstical ovaoidance, to stop the motors, by the nucleo
Alexshawcroft 9:326b8f261ef0 163 // sending a char to the PI
Alexshawcroft 9:326b8f261ef0 164
Alexshawcroft 9:326b8f261ef0 165 //pc.printf("Spinning...");
Stumi 8:262c6c40bff9 166 nh.spinOnce();
Alexshawcroft 9:326b8f261ef0 167 wait_ms(5);
Stumi 0:6021ec1b1449 168 }
Alexshawcroft 9:326b8f261ef0 169 }
Alexshawcroft 9:326b8f261ef0 170
Alexshawcroft 9:326b8f261ef0 171 void power_check()
Alexshawcroft 9:326b8f261ef0 172 {
Alexshawcroft 9:326b8f261ef0 173 power_levels[0] = cPower.monitor_battery(); //Monitors raw battery
Alexshawcroft 9:326b8f261ef0 174 power_levels[1] = cPower.monitor_5v_line(); //Monitors 5V line
Alexshawcroft 9:326b8f261ef0 175 power_levels[2] = cPower.monitor_3v3_line();//Monitors 3V3 Line
Alexshawcroft 9:326b8f261ef0 176
Alexshawcroft 9:326b8f261ef0 177 update_power_levels(power_levels[0]); //Sends the raw battery levels to the LED class
Stumi 0:6021ec1b1449 178 }