Soft robot team / Mbed OS Robot_team1

Robot team project

Dependencies:   QEI Motordriver ros_lib_melodic

main.cpp@17:8831c676778b, 2019-11-30 (annotated)

Committer:
dnulty
Date:
Sat Nov 30 15:13:50 2019 +0000
Revision:
17:8831c676778b
Parent:
16:11282b7ee726
Child:
19:56bc8226b967
Add distance setting subscriber;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
florine_van 0:57855aafa907 1 #include "mbed.h"
florine_van 7:2cf57f28255d 2 #include <ros.h>
florine_van 7:2cf57f28255d 3 #include <std_msgs/Empty.h>
florine_van 7:2cf57f28255d 4 #include <std_msgs/Int32.h>
florine_van 10:be9de79cf6b0 5 #include <std_msgs/String.h>
florine_van 8:57aa8a35d983 6 #include <mbed_custom_msgs/lidar_msg.h>
dnulty 14:08047fde54f6 7 #include <motordriver.h>
dnulty 14:08047fde54f6 8 #include "QEI.h"
dnulty 14:08047fde54f6 9 #include <cstdlib>
florine_van 0:57855aafa907 10 #include "Sensor.h"
florine_van 0:57855aafa907 11
florine_van 0:57855aafa907 12 // Set up serial to pc
florine_van 7:2cf57f28255d 13 //Serial pc(SERIAL_TX, SERIAL_RX);
florine_van 7:2cf57f28255d 14
dnulty 16:11282b7ee726 15 //Threading for sensor readings
dnulty 14:08047fde54f6 16 Thread thread1;
dnulty 16:11282b7ee726 17 Thread thread2;
dnulty 14:08047fde54f6 18 Mutex SerialMutex;
dnulty 14:08047fde54f6 19
florine_van 0:57855aafa907 20 // Set up I²C on the STM32 NUCLEO-401RE
florine_van 0:57855aafa907 21 #define addr1 (0x29)
florine_van 0:57855aafa907 22 #define addr2 (0x2A)
florine_van 0:57855aafa907 23 #define addr3 (0x2B)
florine_van 0:57855aafa907 24 #define addr4 (0x2C)
florine_van 0:57855aafa907 25
florine_van 6:858a5116688e 26 #define S1 PC_8
florine_van 6:858a5116688e 27 #define S2 PC_9
florine_van 6:858a5116688e 28 #define S3 PC_10
florine_van 6:858a5116688e 29 #define S4 PC_11
florine_van 6:858a5116688e 30 #define S5 PC_12
florine_van 6:858a5116688e 31 #define S6 PD_2
florine_van 6:858a5116688e 32 #define S7 PG_2
florine_van 6:858a5116688e 33 #define S8 PG_3
florine_van 6:858a5116688e 34
florine_van 5:8ef79eebbc97 35 // VL6180x sensors
florine_van 7:2cf57f28255d 36 Sensor sensor_back(I2C_SDA, I2C_SCL, S1);
florine_van 7:2cf57f28255d 37 Sensor sensor_left(I2C_SDA, I2C_SCL, S3);
florine_van 7:2cf57f28255d 38 Sensor sensor_forward(I2C_SDA, I2C_SCL, S5);
florine_van 7:2cf57f28255d 39 Sensor sensor_right(I2C_SDA, I2C_SCL, S7);
florine_van 5:8ef79eebbc97 40
dnulty 16:11282b7ee726 41 // Floats for sensor readings
dnulty 14:08047fde54f6 42 float sensor_forward_reading;
dnulty 14:08047fde54f6 43 float sensor_back_reading;
dnulty 14:08047fde54f6 44 float sensor_left_reading;
dnulty 14:08047fde54f6 45 float sensor_right_reading;
dnulty 17:8831c676778b 46 //initialised motor speed
dnulty 16:11282b7ee726 47 double speed=0.6;
dnulty 17:8831c676778b 48 //used to change control to set distances of continous movement
dnulty 16:11282b7ee726 49 bool control_type = 1;;
dnulty 17:8831c676778b 50 //used for the switch cases and the distance travelled
dnulty 16:11282b7ee726 51 int32_t motor_option;
dnulty 17:8831c676778b 52 int32_t pulse = 6000;
dnulty 14:08047fde54f6 53
dnulty 14:08047fde54f6 54 // Set motorpins for driving
dnulty 14:08047fde54f6 55 Motor A(PB_13, PB_15, PC_6, 1); // pwm, fwd, rev, can brake
dnulty 14:08047fde54f6 56 Motor B(PB_4, PC_7, PA_15, 1); // pwm, fwd, rev, can brake
florine_van 11:35809512ec11 57 QEI wheel_A (PB_12, PA_4, NC, 3576, QEI::X4_ENCODING); //counts per rev = 12 * 298:1, use quadrature encoding
florine_van 11:35809512ec11 58 QEI wheel_B (PB_5, PB_3, NC, 3576, QEI::X4_ENCODING); //counts per rev = 12 * 298:1, use quadrature encoding
dnulty 14:08047fde54f6 59
dnulty 16:11282b7ee726 60 //Thread function to update sensor readings
dnulty 14:08047fde54f6 61 void flip() {
dnulty 14:08047fde54f6 62 while(1) {
dnulty 14:08047fde54f6 63 SerialMutex.lock();
dnulty 14:08047fde54f6 64 sensor_forward_reading = sensor_forward.read();
dnulty 14:08047fde54f6 65 sensor_back_reading = sensor_back.read();
dnulty 14:08047fde54f6 66 sensor_left_reading = sensor_left.read();
dnulty 14:08047fde54f6 67 sensor_right_reading = sensor_right.read();
dnulty 14:08047fde54f6 68 SerialMutex.unlock();
dnulty 14:08047fde54f6 69 thread1.wait(2);
dnulty 14:08047fde54f6 70 }
dnulty 14:08047fde54f6 71
dnulty 14:08047fde54f6 72 }
dnulty 14:08047fde54f6 73
dnulty 16:11282b7ee726 74 // Function to move robot (motor speed, motor speed, pulses to travel)
dnulty 14:08047fde54f6 75 void move(float motorA, float motorB, int distance)
dnulty 14:08047fde54f6 76 {
dnulty 14:08047fde54f6 77 float current_pulse_reading = 0;
dnulty 14:08047fde54f6 78
dnulty 14:08047fde54f6 79 // - is forward on our robot
dnulty 14:08047fde54f6 80 A.speed(motorA);
dnulty 14:08047fde54f6 81 B.speed(motorB);
dnulty 16:11282b7ee726 82
dnulty 14:08047fde54f6 83 while(abs(current_pulse_reading) <= distance)
dnulty 14:08047fde54f6 84 {
dnulty 14:08047fde54f6 85 current_pulse_reading = wheel_A.getPulses();
dnulty 16:11282b7ee726 86 //Stops Robot moving forward if front sensor detects
dnulty 14:08047fde54f6 87 if (sensor_back_reading <255 and motorA>0 and motorB>0){
dnulty 14:08047fde54f6 88 return;
dnulty 14:08047fde54f6 89 }
dnulty 16:11282b7ee726 90 //Stops robot reversing if back sensors detects
dnulty 14:08047fde54f6 91 if (sensor_forward_reading <255 and motorA<0 and motorB<0){
dnulty 14:08047fde54f6 92 return;
dnulty 14:08047fde54f6 93 }
dnulty 14:08047fde54f6 94 }
dnulty 14:08047fde54f6 95 }
dnulty 14:08047fde54f6 96
florine_van 5:8ef79eebbc97 97
dnulty 16:11282b7ee726 98 void motor_callback(const std_msgs::Int32& motor_dir)
florine_van 10:be9de79cf6b0 99 {
dnulty 16:11282b7ee726 100 motor_option = motor_dir.data;
dnulty 16:11282b7ee726 101 thread2.signal_set(1);
dnulty 16:11282b7ee726 102 }
dnulty 16:11282b7ee726 103
dnulty 17:8831c676778b 104 void pulse_callback(const std_msgs::Int32& distance){
dnulty 17:8831c676778b 105
dnulty 17:8831c676778b 106 //default value of approx 30cm if no distance is recieved or to great
dnulty 17:8831c676778b 107 if((distance.data <= 0) or (distance.data>120)){
dnulty 17:8831c676778b 108 pulse = 6000;
dnulty 17:8831c676778b 109 }
dnulty 17:8831c676778b 110 else {
dnulty 17:8831c676778b 111 pulse = distance.data*200;
dnulty 17:8831c676778b 112 }
dnulty 17:8831c676778b 113
dnulty 17:8831c676778b 114 }
dnulty 17:8831c676778b 115
dnulty 16:11282b7ee726 116 void control_motor(void) {
dnulty 16:11282b7ee726 117 while (1)
dnulty 16:11282b7ee726 118 {
dnulty 16:11282b7ee726 119 thread2.signal_wait(1);
dnulty 16:11282b7ee726 120 switch(motor_option) {
dnulty 16:11282b7ee726 121 //Forward 30cm
dnulty 16:11282b7ee726 122 case 0://49
dnulty 16:11282b7ee726 123 if (control_type == 1){
dnulty 17:8831c676778b 124 move(-speed, -speed, pulse);
dnulty 16:11282b7ee726 125 }
dnulty 16:11282b7ee726 126 else if (control_type ==0){
dnulty 16:11282b7ee726 127 move(-speed, -speed, 50);
dnulty 16:11282b7ee726 128 }
dnulty 16:11282b7ee726 129 break;
florine_van 10:be9de79cf6b0 130
dnulty 16:11282b7ee726 131 //Left 30 degrees
dnulty 16:11282b7ee726 132 case 1://50
dnulty 16:11282b7ee726 133 if (control_type == 1 ){
dnulty 17:8831c676778b 134 move(speed, -speed, 896);
dnulty 16:11282b7ee726 135 }
dnulty 16:11282b7ee726 136 else if (control_type == 0){
dnulty 17:8831c676778b 137 move(speed, -speed, 100);
dnulty 16:11282b7ee726 138 }
dnulty 16:11282b7ee726 139 break;
dnulty 16:11282b7ee726 140
dnulty 16:11282b7ee726 141 //Right 30 degrees
dnulty 16:11282b7ee726 142 case 2://51
dnulty 16:11282b7ee726 143 if (control_type == 1 ){
dnulty 17:8831c676778b 144 move(-speed, speed, 896);
dnulty 16:11282b7ee726 145 }
dnulty 16:11282b7ee726 146 else if (control_type == 0 ){
dnulty 17:8831c676778b 147 move(-speed, speed, 60);
dnulty 16:11282b7ee726 148 }
dnulty 16:11282b7ee726 149 break;
dnulty 16:11282b7ee726 150
dnulty 16:11282b7ee726 151 // Reverse 30cm
dnulty 16:11282b7ee726 152 case 3://52
dnulty 16:11282b7ee726 153 if (control_type ==1 ){
dnulty 17:8831c676778b 154 move(speed, speed, pulse);
dnulty 16:11282b7ee726 155 }
dnulty 16:11282b7ee726 156 else if (control_type ==0 ){
dnulty 16:11282b7ee726 157 move(speed, speed, 50);
dnulty 16:11282b7ee726 158 }
dnulty 16:11282b7ee726 159 break;
dnulty 16:11282b7ee726 160
dnulty 16:11282b7ee726 161 case 4://speed up
dnulty 16:11282b7ee726 162 if (speed<1.0){
dnulty 16:11282b7ee726 163 speed += 0.2;
dnulty 16:11282b7ee726 164 }
dnulty 16:11282b7ee726 165 break;
dnulty 16:11282b7ee726 166
dnulty 16:11282b7ee726 167 case 5://speed down
dnulty 16:11282b7ee726 168 if (speed>0.2){
dnulty 16:11282b7ee726 169 speed -= 0.2;
dnulty 16:11282b7ee726 170 }
dnulty 16:11282b7ee726 171 break;
dnulty 16:11282b7ee726 172
dnulty 16:11282b7ee726 173 case 6: // switch control type
dnulty 16:11282b7ee726 174 control_type =! control_type;
dnulty 16:11282b7ee726 175 break;
dnulty 17:8831c676778b 176
dnulty 17:8831c676778b 177 case 7: //turn around
dnulty 17:8831c676778b 178 if (control_type == 1 ){
dnulty 17:8831c676778b 179 move(-speed, speed, 5376);
dnulty 17:8831c676778b 180 break;
dnulty 16:11282b7ee726 181
dnulty 17:8831c676778b 182 case 8: //To show case 8 does nothing
dnulty 17:8831c676778b 183 break;
dnulty 17:8831c676778b 184 }
dnulty 17:8831c676778b 185
dnulty 16:11282b7ee726 186 //Reset speed and pulse count
dnulty 16:11282b7ee726 187 A.speed(0);
dnulty 16:11282b7ee726 188 B.speed(0);
dnulty 14:08047fde54f6 189
dnulty 16:11282b7ee726 190 wheel_B.reset();
dnulty 16:11282b7ee726 191 wheel_A.reset();
dnulty 16:11282b7ee726 192 //nh.spinOnce();
dnulty 16:11282b7ee726 193 }
florine_van 10:be9de79cf6b0 194 }
florine_van 10:be9de79cf6b0 195
florine_van 5:8ef79eebbc97 196 int main()
florine_van 5:8ef79eebbc97 197 {
florine_van 7:2cf57f28255d 198 ros::NodeHandle nh;
florine_van 7:2cf57f28255d 199 nh.initNode();
florine_van 8:57aa8a35d983 200
dnulty 14:08047fde54f6 201 // ROS subscriber for motors control
dnulty 17:8831c676778b 202 ros::Subscriber<std_msgs::Int32> pulse_sub("distance_setting", &pulse_callback);
dnulty 16:11282b7ee726 203 ros::Subscriber<std_msgs::Int32> motor_sub("motor_control", &motor_callback);
florine_van 11:35809512ec11 204
dnulty 17:8831c676778b 205
florine_van 10:be9de79cf6b0 206 nh.subscribe(motor_sub);
dnulty 17:8831c676778b 207 nh.subscribe(pulse_sub);
dnulty 14:08047fde54f6 208
florine_van 3:a3144a45f44c 209 // load settings onto VL6180X sensors
florine_van 6:858a5116688e 210 sensor_forward.init();
florine_van 3:a3144a45f44c 211 // change default address of sensor 2
florine_van 6:858a5116688e 212 sensor_forward.changeAddress(addr2);
florine_van 2:c537f1ebad7b 213
florine_van 6:858a5116688e 214 sensor_right.init();
florine_van 3:a3144a45f44c 215 // change default address of sensor 3
florine_van 6:858a5116688e 216 sensor_right.changeAddress(addr3);
florine_van 2:c537f1ebad7b 217
florine_van 6:858a5116688e 218 sensor_back.init();
florine_van 3:a3144a45f44c 219 // change default address of sensor 4
florine_van 6:858a5116688e 220 sensor_back.changeAddress(addr4);
florine_van 6:858a5116688e 221
florine_van 6:858a5116688e 222 sensor_left.init();
florine_van 2:c537f1ebad7b 223
dnulty 14:08047fde54f6 224 thread1.start(callback(&flip));
dnulty 16:11282b7ee726 225 thread2.start(callback(&control_motor));
dnulty 14:08047fde54f6 226
dnulty 16:11282b7ee726 227 while(1) {
florine_van 7:2cf57f28255d 228 nh.spinOnce();
dnulty 16:11282b7ee726 229
dnulty 16:11282b7ee726 230 }
dnulty 14:08047fde54f6 231
dnulty 16:11282b7ee726 232
dnulty 14:08047fde54f6 233 }
florine_van 0:57855aafa907 234
florine_van 5:8ef79eebbc97 235
florine_van 5:8ef79eebbc97 236
dnulty 14:08047fde54f6 237