Soft robot team / Mbed OS Robot_team1

Robot team project

Dependencies:   QEI Motordriver ros_lib_melodic

main.cpp@23:d1dc248b4e54, 2020-01-06 (annotated)

Committer:
dnulty
Date:
Mon Jan 06 14:55:03 2020 +0000
Revision:
23:d1dc248b4e54
Parent:
20:074a3638c702
Child:
24:4d6ff25fb9cc
06/01/2020

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 23:d1dc248b4e54 141 wheel_B.reset();
dnulty 23:d1dc248b4e54 142 wheel_A.reset();
dnulty 23:d1dc248b4e54 143 while(abs(current_pulse_reading) <= 2782)
dnulty 20:074a3638c702 144 {
dnulty 20:074a3638c702 145 current_pulse_reading = wheel_A.getPulses();
dnulty 20:074a3638c702 146 }
dnulty 20:074a3638c702 147 wheel_B.reset();
dnulty 20:074a3638c702 148 wheel_A.reset();
dnulty 20:074a3638c702 149 }
dnulty 20:074a3638c702 150 A.speed(0);
dnulty 20:074a3638c702 151 B.speed(0);
dnulty 20:074a3638c702 152 wait(0.05);
dnulty 20:074a3638c702 153 }
dnulty 20:074a3638c702 154
dnulty 20:074a3638c702 155 void dance()
dnulty 20:074a3638c702 156 {
dnulty 20:074a3638c702 157 wait(5);// required for audacity to play music
dnulty 20:074a3638c702 158
dnulty 23:d1dc248b4e54 159 for (int i =0; i<10; i++)
dnulty 20:074a3638c702 160 {
dnulty 23:d1dc248b4e54 161 int distance = (rand()%7000+2000);
dnulty 20:074a3638c702 162 float A_speed = (rand()%2-1);
dnulty 20:074a3638c702 163 float B_speed = (rand()%2-1);
dnulty 23:d1dc248b4e54 164 /* if((A_speed < 0.4) && (A_speed > -0.4)){
dnulty 20:074a3638c702 165 A_speed = 0.4;
dnulty 20:074a3638c702 166 }
dnulty 23:d1dc248b4e54 167 if((B_speed < 0.4) && (B_speed > -0.4)){
dnulty 23:d1dc248b4e54 168 B_speed = 0.4;
dnulty 23:d1dc248b4e54 169 } */
dnulty 20:074a3638c702 170 move(A_speed,B_speed,distance);
dnulty 20:074a3638c702 171 wheel_B.reset();
dnulty 20:074a3638c702 172 wheel_A.reset();
dnulty 20:074a3638c702 173 }
dnulty 23:d1dc248b4e54 174 }
dnulty 20:074a3638c702 175
florine_van 19:56bc8226b967 176 // Function to set robot direction and distance according to data published on motor_control topic
dnulty 16:11282b7ee726 177 void control_motor(void) {
florine_van 19:56bc8226b967 178
florine_van 19:56bc8226b967 179 while (1){
florine_van 19:56bc8226b967 180 thread2.signal_wait(1);
florine_van 19:56bc8226b967 181
florine_van 19:56bc8226b967 182 switch(motor_option) {
florine_van 19:56bc8226b967 183
florine_van 19:56bc8226b967 184 // Do nothing - default value published on motor_contorl topic
florine_van 19:56bc8226b967 185 case 0:
florine_van 19:56bc8226b967 186 break;
florine_van 10:be9de79cf6b0 187
dnulty 20:074a3638c702 188 //Forward
florine_van 19:56bc8226b967 189 case 1://49
florine_van 19:56bc8226b967 190 if (control_type == 1){
florine_van 19:56bc8226b967 191 move(-speed, -speed, pulse);
florine_van 19:56bc8226b967 192 }
florine_van 19:56bc8226b967 193 else if (control_type ==0){
florine_van 19:56bc8226b967 194 move(-speed, -speed, 50);
florine_van 19:56bc8226b967 195 }
florine_van 19:56bc8226b967 196 break;
florine_van 19:56bc8226b967 197
dnulty 20:074a3638c702 198 //Left
florine_van 19:56bc8226b967 199 case 2://50
florine_van 19:56bc8226b967 200 if (control_type == 1 ){
dnulty 20:074a3638c702 201 move(speed, -speed, pulse/6.7); // Divide by 6.7 to convert to degrees
florine_van 19:56bc8226b967 202 }
florine_van 19:56bc8226b967 203 else if (control_type == 0){
florine_van 19:56bc8226b967 204 move(speed, -speed, 100);
florine_van 19:56bc8226b967 205 }
florine_van 19:56bc8226b967 206 break;
dnulty 16:11282b7ee726 207
dnulty 20:074a3638c702 208 //Right
florine_van 19:56bc8226b967 209 case 3://51
florine_van 19:56bc8226b967 210 if (control_type == 1 ){
florine_van 19:56bc8226b967 211 move(-speed, speed, pulse/6.7);
florine_van 19:56bc8226b967 212 }
florine_van 19:56bc8226b967 213 else if (control_type == 0 ){
florine_van 19:56bc8226b967 214 move(-speed, speed, 60);
florine_van 19:56bc8226b967 215 }
florine_van 19:56bc8226b967 216 break;
dnulty 16:11282b7ee726 217
dnulty 20:074a3638c702 218 // Reverse
florine_van 19:56bc8226b967 219 case 4://52
florine_van 19:56bc8226b967 220 if (control_type ==1 ){
florine_van 19:56bc8226b967 221 move(speed, speed, pulse);
florine_van 19:56bc8226b967 222 }
florine_van 19:56bc8226b967 223 else if (control_type ==0 ){
florine_van 19:56bc8226b967 224 move(speed, speed, 50);
florine_van 19:56bc8226b967 225 }
florine_van 19:56bc8226b967 226 break;
florine_van 19:56bc8226b967 227
florine_van 19:56bc8226b967 228 // Speed up
florine_van 19:56bc8226b967 229 case 5:
florine_van 19:56bc8226b967 230 if (speed < 1.0){
florine_van 19:56bc8226b967 231 speed += 0.2;
florine_van 19:56bc8226b967 232 }
florine_van 19:56bc8226b967 233 break;
florine_van 19:56bc8226b967 234
florine_van 19:56bc8226b967 235 // Speed down
florine_van 19:56bc8226b967 236 case 6:
florine_van 19:56bc8226b967 237 if (speed>0.2){
florine_van 19:56bc8226b967 238 speed -= 0.2;
florine_van 19:56bc8226b967 239 }
florine_van 19:56bc8226b967 240 break;
florine_van 19:56bc8226b967 241
florine_van 19:56bc8226b967 242 // Switch control type
florine_van 19:56bc8226b967 243 case 7:
florine_van 19:56bc8226b967 244 control_type =! control_type;
florine_van 19:56bc8226b967 245 break;
dnulty 16:11282b7ee726 246
florine_van 19:56bc8226b967 247 // Turn around
florine_van 19:56bc8226b967 248 case 8:
florine_van 19:56bc8226b967 249 if (control_type == 1 ){
florine_van 19:56bc8226b967 250 move(-speed, speed, 5376);
florine_van 19:56bc8226b967 251 }
florine_van 19:56bc8226b967 252 break;
florine_van 19:56bc8226b967 253
florine_van 19:56bc8226b967 254 // Draw a square
florine_van 19:56bc8226b967 255 case 9:
dnulty 20:074a3638c702 256 square();
dnulty 20:074a3638c702 257 break;
dnulty 20:074a3638c702 258
dnulty 20:074a3638c702 259 case 10:
dnulty 20:074a3638c702 260 dance();
florine_van 19:56bc8226b967 261 break;
florine_van 19:56bc8226b967 262 }
dnulty 20:074a3638c702 263
florine_van 19:56bc8226b967 264 //Reset speed and pulse count
florine_van 19:56bc8226b967 265 A.speed(0);
florine_van 19:56bc8226b967 266 B.speed(0);
dnulty 14:08047fde54f6 267
florine_van 19:56bc8226b967 268 wheel_B.reset();
florine_van 19:56bc8226b967 269 wheel_A.reset();
dnulty 16:11282b7ee726 270 }
florine_van 10:be9de79cf6b0 271 }
florine_van 10:be9de79cf6b0 272
florine_van 5:8ef79eebbc97 273 int main()
florine_van 5:8ef79eebbc97 274 {
florine_van 7:2cf57f28255d 275 ros::NodeHandle nh;
florine_van 7:2cf57f28255d 276 nh.initNode();
florine_van 8:57aa8a35d983 277
dnulty 14:08047fde54f6 278 // ROS subscriber for motors control
florine_van 19:56bc8226b967 279 ros::Subscriber<mbed_custom_msgs::motor_msg> motor_sub("motor_control", &motor_callback);
florine_van 10:be9de79cf6b0 280 nh.subscribe(motor_sub);
dnulty 14:08047fde54f6 281
florine_van 3:a3144a45f44c 282 // load settings onto VL6180X sensors
florine_van 6:858a5116688e 283 sensor_forward.init();
florine_van 3:a3144a45f44c 284 // change default address of sensor 2
florine_van 6:858a5116688e 285 sensor_forward.changeAddress(addr2);
florine_van 2:c537f1ebad7b 286
florine_van 6:858a5116688e 287 sensor_right.init();
florine_van 3:a3144a45f44c 288 // change default address of sensor 3
florine_van 6:858a5116688e 289 sensor_right.changeAddress(addr3);
florine_van 2:c537f1ebad7b 290
florine_van 6:858a5116688e 291 sensor_back.init();
florine_van 3:a3144a45f44c 292 // change default address of sensor 4
florine_van 6:858a5116688e 293 sensor_back.changeAddress(addr4);
florine_van 6:858a5116688e 294
florine_van 6:858a5116688e 295 sensor_left.init();
florine_van 2:c537f1ebad7b 296
dnulty 14:08047fde54f6 297 thread1.start(callback(&flip));
dnulty 16:11282b7ee726 298 thread2.start(callback(&control_motor));
dnulty 14:08047fde54f6 299
dnulty 16:11282b7ee726 300 while(1) {
florine_van 19:56bc8226b967 301 nh.spinOnce();
dnulty 16:11282b7ee726 302 }
dnulty 14:08047fde54f6 303 }
florine_van 0:57855aafa907 304
florine_van 5:8ef79eebbc97 305
florine_van 5:8ef79eebbc97 306
dnulty 14:08047fde54f6 307