Soft robot team / Mbed OS Robot_team1

Robot team project

Dependencies:   QEI Motordriver ros_lib_melodic

main.cpp@21:2dcd6d0d004d, 2019-12-10 (annotated)

Committer:
florine_van
Date:
Tue Dec 10 10:30:21 2019 +0000
Revision:
21:2dcd6d0d004d
Parent:
20:074a3638c702 
Clean code

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 19:56bc8226b967 6 #include <mbed_custom_msgs/motor_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;
florine_van 19:56bc8226b967 46
florine_van 19:56bc8226b967 47 //Initialised motor speed
dnulty 16:11282b7ee726 48 double speed=0.6;
dnulty 17:8831c676778b 49 //used to change control to set distances of continous movement
florine_van 21:2dcd6d0d004d 50 bool control_type = 1;
dnulty 17:8831c676778b 51 //used for the switch cases and the distance travelled
dnulty 16:11282b7ee726 52 int32_t motor_option;
dnulty 17:8831c676778b 53 int32_t pulse = 6000;
dnulty 14:08047fde54f6 54
dnulty 14:08047fde54f6 55 // Set motorpins for driving
dnulty 14:08047fde54f6 56 Motor A(PB_13, PB_15, PC_6, 1); // pwm, fwd, rev, can brake
dnulty 14:08047fde54f6 57 Motor B(PB_4, PC_7, PA_15, 1); // pwm, fwd, rev, can brake
florine_van 11:35809512ec11 58 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 59 QEI wheel_B (PB_5, PB_3, NC, 3576, QEI::X4_ENCODING); //counts per rev = 12 * 298:1, use quadrature encoding
dnulty 14:08047fde54f6 60
dnulty 16:11282b7ee726 61 //Thread function to update sensor readings
dnulty 14:08047fde54f6 62 void flip() {
florine_van 19:56bc8226b967 63 while(1) {
florine_van 19:56bc8226b967 64 SerialMutex.lock();
florine_van 19:56bc8226b967 65
florine_van 19:56bc8226b967 66 // Sensor readings
florine_van 19:56bc8226b967 67 sensor_forward_reading = sensor_forward.read();
florine_van 19:56bc8226b967 68 sensor_back_reading = sensor_back.read();
florine_van 19:56bc8226b967 69 sensor_left_reading = sensor_left.read();
florine_van 19:56bc8226b967 70 sensor_right_reading = sensor_right.read();
florine_van 19:56bc8226b967 71
florine_van 19:56bc8226b967 72 SerialMutex.unlock();
florine_van 19:56bc8226b967 73 thread1.wait(2);
florine_van 19:56bc8226b967 74 }
dnulty 14:08047fde54f6 75 }
dnulty 14:08047fde54f6 76
florine_van 19:56bc8226b967 77 void motor_callback(const mbed_custom_msgs::motor_msg& msg)
dnulty 14:08047fde54f6 78 {
florine_van 19:56bc8226b967 79 motor_option = msg.dir;
florine_van 19:56bc8226b967 80 if( (msg.distance <= 0) or (msg.distance > 120) ) {
florine_van 19:56bc8226b967 81 pulse = 6000;
florine_van 19:56bc8226b967 82 } else {
dnulty 20:074a3638c702 83 pulse = msg.distance * 200; // Est 200 pulses per cm
florine_van 19:56bc8226b967 84 }
dnulty 16:11282b7ee726 85
dnulty 16:11282b7ee726 86 thread2.signal_set(1);
dnulty 16:11282b7ee726 87 }
dnulty 16:11282b7ee726 88
florine_van 19:56bc8226b967 89 // Function to move robot
florine_van 19:56bc8226b967 90 void move(float motorA, float motorB, int distance) {
florine_van 19:56bc8226b967 91 float current_pulse_reading = 0;
florine_van 19:56bc8226b967 92
florine_van 19:56bc8226b967 93 // Set motors speed
florine_van 19:56bc8226b967 94 A.speed(motorA);
florine_van 19:56bc8226b967 95 B.speed(motorB);
dnulty 17:8831c676778b 96
florine_van 19:56bc8226b967 97 while(abs(current_pulse_reading) <= distance) {
florine_van 19:56bc8226b967 98 current_pulse_reading = wheel_A.getPulses();
florine_van 19:56bc8226b967 99 //Stops Robot moving forward if front sensor detects
florine_van 19:56bc8226b967 100 if (sensor_back_reading <255 and motorA>0 and motorB>0){
florine_van 19:56bc8226b967 101 return;
dnulty 17:8831c676778b 102 }
florine_van 19:56bc8226b967 103
florine_van 19:56bc8226b967 104 //Stops robot reversing if back sensors detects
florine_van 19:56bc8226b967 105 if (sensor_forward_reading <255 and motorA<0 and motorB<0){
florine_van 19:56bc8226b967 106 return;
florine_van 19:56bc8226b967 107 }
dnulty 17:8831c676778b 108 }
dnulty 17:8831c676778b 109 }
dnulty 17:8831c676778b 110
dnulty 20:074a3638c702 111 void square()
dnulty 20:074a3638c702 112 {
dnulty 20:074a3638c702 113 float current_pulse_reading = 0;
dnulty 20:074a3638c702 114
florine_van 21:2dcd6d0d004d 115 for(int i = 0; i<4 ; i++) {
dnulty 20:074a3638c702 116 current_pulse_reading = 0;
florine_van 21:2dcd6d0d004d 117 while(abs(current_pulse_reading) <= 6000){
dnulty 20:074a3638c702 118 A.speed(-speed);
dnulty 20:074a3638c702 119 B.speed(-speed);
dnulty 20:074a3638c702 120
dnulty 20:074a3638c702 121 current_pulse_reading = wheel_A.getPulses();
dnulty 20:074a3638c702 122 //Stops Robot moving forward if front sensor detects
florine_van 21:2dcd6d0d004d 123 if (sensor_forward_reading <255){
dnulty 20:074a3638c702 124 return;
florine_van 21:2dcd6d0d004d 125 }
dnulty 20:074a3638c702 126 }
dnulty 20:074a3638c702 127 // need to set motor speed to 0
dnulty 20:074a3638c702 128 A.speed(0);
dnulty 20:074a3638c702 129 B.speed(0);
dnulty 20:074a3638c702 130 wait(0.05);
dnulty 20:074a3638c702 131
dnulty 20:074a3638c702 132 A.speed(-speed);
dnulty 20:074a3638c702 133 B.speed(speed);
florine_van 21:2dcd6d0d004d 134 while(abs(current_pulse_reading) <= 8688){
dnulty 20:074a3638c702 135 current_pulse_reading = wheel_A.getPulses();
dnulty 20:074a3638c702 136 }
florine_van 21:2dcd6d0d004d 137
dnulty 20:074a3638c702 138 wheel_B.reset();
dnulty 20:074a3638c702 139 wheel_A.reset();
florine_van 21:2dcd6d0d004d 140
florine_van 21:2dcd6d0d004d 141 }
florine_van 21:2dcd6d0d004d 142
florine_van 21:2dcd6d0d004d 143 // Stop motors
florine_van 21:2dcd6d0d004d 144 A.speed(0);
florine_van 21:2dcd6d0d004d 145 B.speed(0);
florine_van 21:2dcd6d0d004d 146 wait(0.05);
dnulty 20:074a3638c702 147 }
dnulty 20:074a3638c702 148
dnulty 20:074a3638c702 149 void dance()
dnulty 20:074a3638c702 150 {
dnulty 20:074a3638c702 151 wait(5);// required for audacity to play music
dnulty 20:074a3638c702 152
florine_van 21:2dcd6d0d004d 153 for (int i =0; i<5; i++) {
dnulty 20:074a3638c702 154 int distance = (rand()%16000+2000);
dnulty 20:074a3638c702 155 float A_speed = (rand()%2-1);
dnulty 20:074a3638c702 156 float B_speed = (rand()%2-1);
dnulty 20:074a3638c702 157 if((A_speed < 0.4) and (A_speed > -0.4)){
dnulty 20:074a3638c702 158 A_speed = 0.4;
florine_van 21:2dcd6d0d004d 159 }
dnulty 20:074a3638c702 160 if((B_speed < 0.4) and (B_speed > -0.4)){
dnulty 20:074a3638c702 161 B_speed = -0.4;
florine_van 21:2dcd6d0d004d 162 }
dnulty 20:074a3638c702 163 move(A_speed,B_speed,distance);
dnulty 20:074a3638c702 164 wheel_B.reset();
dnulty 20:074a3638c702 165 wheel_A.reset();
dnulty 20:074a3638c702 166 }
dnulty 20:074a3638c702 167 }
dnulty 20:074a3638c702 168
dnulty 20:074a3638c702 169 /* float current_pulse_reading = 0;
dnulty 20:074a3638c702 170 while(abs(current_pulse_reading) <= 5388)
dnulty 20:074a3638c702 171 {
dnulty 20:074a3638c702 172 current_pulse_reading = wheel_A.getPulses();
dnulty 20:074a3638c702 173 A.speed(-speed);
dnulty 20:074a3638c702 174 B.speed(0.0);
dnulty 20:074a3638c702 175 }
dnulty 20:074a3638c702 176 wait(0.05);
dnulty 20:074a3638c702 177 current_pulse_reading = 0;
dnulty 20:074a3638c702 178 wheel_B.reset();
dnulty 20:074a3638c702 179 wheel_A.reset();
dnulty 20:074a3638c702 180 while(abs(current_pulse_reading) <= 5388)
dnulty 20:074a3638c702 181 {
dnulty 20:074a3638c702 182 current_pulse_reading = wheel_A.getPulses();
dnulty 20:074a3638c702 183 A.speed(speed);
dnulty 20:074a3638c702 184 B.speed(0);
dnulty 20:074a3638c702 185 }
dnulty 20:074a3638c702 186 wait(0.05);
dnulty 20:074a3638c702 187 wheel_B.reset();
dnulty 20:074a3638c702 188 wheel_A.reset();
dnulty 20:074a3638c702 189 current_pulse_reading = 0;
dnulty 20:074a3638c702 190 while(abs(current_pulse_reading) <= 5388)
dnulty 20:074a3638c702 191 {
dnulty 20:074a3638c702 192 current_pulse_reading = wheel_B.getPulses();
dnulty 20:074a3638c702 193 A.speed(0);
dnulty 20:074a3638c702 194 B.speed(-speed);
dnulty 20:074a3638c702 195 }
dnulty 20:074a3638c702 196 wait(0.05);
dnulty 20:074a3638c702 197 wheel_B.reset();
dnulty 20:074a3638c702 198 wheel_A.reset();
dnulty 20:074a3638c702 199 current_pulse_reading = 0;
dnulty 20:074a3638c702 200 while(abs(current_pulse_reading) <=5388)
dnulty 20:074a3638c702 201 {
dnulty 20:074a3638c702 202 current_pulse_reading = wheel_B.getPulses();
dnulty 20:074a3638c702 203 A.speed(0);
dnulty 20:074a3638c702 204 B.speed(speed);
dnulty 20:074a3638c702 205 }
dnulty 20:074a3638c702 206 wait(0.05);
dnulty 20:074a3638c702 207 wheel_B.reset();
dnulty 20:074a3638c702 208 wheel_A.reset();
dnulty 20:074a3638c702 209 current_pulse_reading = 0;
dnulty 20:074a3638c702 210 while(abs(current_pulse_reading) <21504)
dnulty 20:074a3638c702 211 {
dnulty 20:074a3638c702 212 current_pulse_reading = wheel_A.getPulses();
dnulty 20:074a3638c702 213 A.speed(-speed);
dnulty 20:074a3638c702 214 B.speed(speed);
dnulty 20:074a3638c702 215 }
dnulty 20:074a3638c702 216 wait(0.05);
dnulty 20:074a3638c702 217 wheel_B.reset();
dnulty 20:074a3638c702 218 wheel_A.reset();
dnulty 20:074a3638c702 219 current_pulse_reading = 0;
dnulty 20:074a3638c702 220 while(abs(current_pulse_reading) <6000)
dnulty 20:074a3638c702 221 {
dnulty 20:074a3638c702 222 current_pulse_reading = wheel_A.getPulses();
dnulty 20:074a3638c702 223 A.speed(-speed);
dnulty 20:074a3638c702 224 B.speed(-speed);
dnulty 20:074a3638c702 225 }
dnulty 20:074a3638c702 226 wait(0.05);
dnulty 20:074a3638c702 227 wheel_B.reset();
dnulty 20:074a3638c702 228 wheel_A.reset();
dnulty 20:074a3638c702 229 current_pulse_reading = 0;
dnulty 20:074a3638c702 230 while(abs(current_pulse_reading) <6000)
dnulty 20:074a3638c702 231 {
dnulty 20:074a3638c702 232 current_pulse_reading = wheel_A.getPulses();
dnulty 20:074a3638c702 233 A.speed(speed);
dnulty 20:074a3638c702 234 B.speed(speed);
dnulty 20:074a3638c702 235 }
dnulty 20:074a3638c702 236 wait(0.05);
dnulty 20:074a3638c702 237 A.speed(0);
dnulty 20:074a3638c702 238 B.speed(0);
florine_van 21:2dcd6d0d004d 239 */
dnulty 20:074a3638c702 240
dnulty 20:074a3638c702 241
florine_van 19:56bc8226b967 242 // Function to set robot direction and distance according to data published on motor_control topic
dnulty 16:11282b7ee726 243 void control_motor(void) {
florine_van 19:56bc8226b967 244
florine_van 19:56bc8226b967 245 while (1){
florine_van 19:56bc8226b967 246 thread2.signal_wait(1);
florine_van 19:56bc8226b967 247
florine_van 19:56bc8226b967 248 switch(motor_option) {
florine_van 19:56bc8226b967 249
florine_van 19:56bc8226b967 250 // Do nothing - default value published on motor_contorl topic
florine_van 19:56bc8226b967 251 case 0:
florine_van 19:56bc8226b967 252 break;
florine_van 10:be9de79cf6b0 253
dnulty 20:074a3638c702 254 //Forward
florine_van 19:56bc8226b967 255 case 1://49
florine_van 19:56bc8226b967 256 if (control_type == 1){
florine_van 19:56bc8226b967 257 move(-speed, -speed, pulse);
florine_van 19:56bc8226b967 258 }
florine_van 19:56bc8226b967 259 else if (control_type ==0){
florine_van 19:56bc8226b967 260 move(-speed, -speed, 50);
florine_van 19:56bc8226b967 261 }
florine_van 19:56bc8226b967 262 break;
florine_van 19:56bc8226b967 263
dnulty 20:074a3638c702 264 //Left
florine_van 19:56bc8226b967 265 case 2://50
florine_van 19:56bc8226b967 266 if (control_type == 1 ){
dnulty 20:074a3638c702 267 move(speed, -speed, pulse/6.7); // Divide by 6.7 to convert to degrees
florine_van 19:56bc8226b967 268 }
florine_van 19:56bc8226b967 269 else if (control_type == 0){
florine_van 19:56bc8226b967 270 move(speed, -speed, 100);
florine_van 19:56bc8226b967 271 }
florine_van 19:56bc8226b967 272 break;
dnulty 16:11282b7ee726 273
dnulty 20:074a3638c702 274 //Right
florine_van 19:56bc8226b967 275 case 3://51
florine_van 19:56bc8226b967 276 if (control_type == 1 ){
florine_van 19:56bc8226b967 277 move(-speed, speed, pulse/6.7);
florine_van 19:56bc8226b967 278 }
florine_van 19:56bc8226b967 279 else if (control_type == 0 ){
florine_van 19:56bc8226b967 280 move(-speed, speed, 60);
florine_van 19:56bc8226b967 281 }
florine_van 19:56bc8226b967 282 break;
dnulty 16:11282b7ee726 283
dnulty 20:074a3638c702 284 // Reverse
florine_van 19:56bc8226b967 285 case 4://52
florine_van 19:56bc8226b967 286 if (control_type ==1 ){
florine_van 19:56bc8226b967 287 move(speed, speed, pulse);
florine_van 19:56bc8226b967 288 }
florine_van 19:56bc8226b967 289 else if (control_type ==0 ){
florine_van 19:56bc8226b967 290 move(speed, speed, 50);
florine_van 19:56bc8226b967 291 }
florine_van 19:56bc8226b967 292 break;
florine_van 19:56bc8226b967 293
florine_van 19:56bc8226b967 294 // Speed up
florine_van 19:56bc8226b967 295 case 5:
florine_van 19:56bc8226b967 296 if (speed < 1.0){
florine_van 19:56bc8226b967 297 speed += 0.2;
florine_van 19:56bc8226b967 298 }
florine_van 19:56bc8226b967 299 break;
florine_van 19:56bc8226b967 300
florine_van 19:56bc8226b967 301 // Speed down
florine_van 19:56bc8226b967 302 case 6:
florine_van 19:56bc8226b967 303 if (speed>0.2){
florine_van 19:56bc8226b967 304 speed -= 0.2;
florine_van 19:56bc8226b967 305 }
florine_van 19:56bc8226b967 306 break;
florine_van 19:56bc8226b967 307
florine_van 19:56bc8226b967 308 // Switch control type
florine_van 19:56bc8226b967 309 case 7:
florine_van 19:56bc8226b967 310 control_type =! control_type;
florine_van 19:56bc8226b967 311 break;
dnulty 16:11282b7ee726 312
florine_van 19:56bc8226b967 313 // Turn around
florine_van 19:56bc8226b967 314 case 8:
florine_van 19:56bc8226b967 315 if (control_type == 1 ){
florine_van 19:56bc8226b967 316 move(-speed, speed, 5376);
florine_van 19:56bc8226b967 317 }
florine_van 19:56bc8226b967 318 break;
florine_van 19:56bc8226b967 319
florine_van 19:56bc8226b967 320 // Draw a square
florine_van 19:56bc8226b967 321 case 9:
dnulty 20:074a3638c702 322 square();
dnulty 20:074a3638c702 323 break;
dnulty 20:074a3638c702 324
florine_van 21:2dcd6d0d004d 325 // Random dance
dnulty 20:074a3638c702 326 case 10:
dnulty 20:074a3638c702 327 dance();
florine_van 19:56bc8226b967 328 break;
florine_van 19:56bc8226b967 329 }
dnulty 20:074a3638c702 330
florine_van 19:56bc8226b967 331 //Reset speed and pulse count
florine_van 19:56bc8226b967 332 A.speed(0);
florine_van 19:56bc8226b967 333 B.speed(0);
dnulty 14:08047fde54f6 334
florine_van 19:56bc8226b967 335 wheel_B.reset();
florine_van 19:56bc8226b967 336 wheel_A.reset();
dnulty 16:11282b7ee726 337 }
florine_van 10:be9de79cf6b0 338 }
florine_van 10:be9de79cf6b0 339
florine_van 5:8ef79eebbc97 340 int main()
florine_van 5:8ef79eebbc97 341 {
florine_van 7:2cf57f28255d 342 ros::NodeHandle nh;
florine_van 7:2cf57f28255d 343 nh.initNode();
florine_van 8:57aa8a35d983 344
dnulty 14:08047fde54f6 345 // ROS subscriber for motors control
florine_van 19:56bc8226b967 346 ros::Subscriber<mbed_custom_msgs::motor_msg> motor_sub("motor_control", &motor_callback);
florine_van 10:be9de79cf6b0 347 nh.subscribe(motor_sub);
dnulty 14:08047fde54f6 348
florine_van 3:a3144a45f44c 349 // load settings onto VL6180X sensors
florine_van 6:858a5116688e 350 sensor_forward.init();
florine_van 3:a3144a45f44c 351 // change default address of sensor 2
florine_van 6:858a5116688e 352 sensor_forward.changeAddress(addr2);
florine_van 2:c537f1ebad7b 353
florine_van 6:858a5116688e 354 sensor_right.init();
florine_van 3:a3144a45f44c 355 // change default address of sensor 3
florine_van 6:858a5116688e 356 sensor_right.changeAddress(addr3);
florine_van 2:c537f1ebad7b 357
florine_van 6:858a5116688e 358 sensor_back.init();
florine_van 3:a3144a45f44c 359 // change default address of sensor 4
florine_van 6:858a5116688e 360 sensor_back.changeAddress(addr4);
florine_van 6:858a5116688e 361
florine_van 6:858a5116688e 362 sensor_left.init();
florine_van 2:c537f1ebad7b 363
dnulty 14:08047fde54f6 364 thread1.start(callback(&flip));
dnulty 16:11282b7ee726 365 thread2.start(callback(&control_motor));
dnulty 14:08047fde54f6 366
dnulty 16:11282b7ee726 367 while(1) {
florine_van 19:56bc8226b967 368 nh.spinOnce();
dnulty 16:11282b7ee726 369 }
dnulty 14:08047fde54f6 370 }
florine_van 0:57855aafa907 371
florine_van 5:8ef79eebbc97 372
florine_van 5:8ef79eebbc97 373
dnulty 14:08047fde54f6 374