Soft robot team / Mbed OS Robot_team1

Robot team project

Dependencies:   QEI Motordriver ros_lib_melodic

main.cpp@20:074a3638c702, 2019-12-10 (annotated)

Committer:
dnulty
Date:
Tue Dec 10 10:23:54 2019 +0000
Revision:
20:074a3638c702
Parent:
19:56bc8226b967
Child:
21:2dcd6d0d004d
Child:
23:d1dc248b4e54
10/12/2019

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