Soft robot team / Mbed OS Robot_team1

Robot team project

Dependencies:   QEI Motordriver ros_lib_melodic

main.cpp@19:56bc8226b967, 2019-12-03 (annotated)

Committer:
florine_van
Date:
Tue Dec 03 09:39:58 2019 +0000
Revision:
19:56bc8226b967
Parent:
17:8831c676778b
Child:
20:074a3638c702
Clean code and remove unused lines;

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 {
florine_van 19:56bc8226b967 83 pulse = msg.distance * 200;
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
florine_van 19:56bc8226b967 112 // Function to set robot direction and distance according to data published on motor_control topic
dnulty 16:11282b7ee726 113 void control_motor(void) {
florine_van 19:56bc8226b967 114
florine_van 19:56bc8226b967 115 while (1){
florine_van 19:56bc8226b967 116 thread2.signal_wait(1);
florine_van 19:56bc8226b967 117
florine_van 19:56bc8226b967 118 switch(motor_option) {
florine_van 19:56bc8226b967 119
florine_van 19:56bc8226b967 120 // Do nothing - default value published on motor_contorl topic
florine_van 19:56bc8226b967 121 case 0:
florine_van 19:56bc8226b967 122 break;
florine_van 10:be9de79cf6b0 123
florine_van 19:56bc8226b967 124 //Forward for pulse/200cm
florine_van 19:56bc8226b967 125 case 1://49
florine_van 19:56bc8226b967 126 if (control_type == 1){
florine_van 19:56bc8226b967 127 move(-speed, -speed, pulse);
florine_van 19:56bc8226b967 128 }
florine_van 19:56bc8226b967 129 else if (control_type ==0){
florine_van 19:56bc8226b967 130 move(-speed, -speed, 50);
florine_van 19:56bc8226b967 131 }
florine_van 19:56bc8226b967 132 break;
florine_van 19:56bc8226b967 133
florine_van 19:56bc8226b967 134 //Left pulse pulse/200/6.7 degrees
florine_van 19:56bc8226b967 135 case 2://50
florine_van 19:56bc8226b967 136 if (control_type == 1 ){
florine_van 19:56bc8226b967 137 move(speed, -speed, pulse/6.7);
florine_van 19:56bc8226b967 138 }
florine_van 19:56bc8226b967 139 else if (control_type == 0){
florine_van 19:56bc8226b967 140 move(speed, -speed, 100);
florine_van 19:56bc8226b967 141 }
florine_van 19:56bc8226b967 142 break;
dnulty 16:11282b7ee726 143
florine_van 19:56bc8226b967 144 //Right pulse/200/6.7 degrees
florine_van 19:56bc8226b967 145 case 3://51
florine_van 19:56bc8226b967 146 if (control_type == 1 ){
florine_van 19:56bc8226b967 147 move(-speed, speed, pulse/6.7);
florine_van 19:56bc8226b967 148 }
florine_van 19:56bc8226b967 149 else if (control_type == 0 ){
florine_van 19:56bc8226b967 150 move(-speed, speed, 60);
florine_van 19:56bc8226b967 151 }
florine_van 19:56bc8226b967 152 break;
dnulty 16:11282b7ee726 153
florine_van 19:56bc8226b967 154 // Reverse for pulse/200cm
florine_van 19:56bc8226b967 155 case 4://52
florine_van 19:56bc8226b967 156 if (control_type ==1 ){
florine_van 19:56bc8226b967 157 move(speed, speed, pulse);
florine_van 19:56bc8226b967 158 }
florine_van 19:56bc8226b967 159 else if (control_type ==0 ){
florine_van 19:56bc8226b967 160 move(speed, speed, 50);
florine_van 19:56bc8226b967 161 }
florine_van 19:56bc8226b967 162 break;
florine_van 19:56bc8226b967 163
florine_van 19:56bc8226b967 164 // Speed up
florine_van 19:56bc8226b967 165 case 5:
florine_van 19:56bc8226b967 166 if (speed < 1.0){
florine_van 19:56bc8226b967 167 speed += 0.2;
florine_van 19:56bc8226b967 168 }
florine_van 19:56bc8226b967 169 break;
florine_van 19:56bc8226b967 170
florine_van 19:56bc8226b967 171 // Speed down
florine_van 19:56bc8226b967 172 case 6:
florine_van 19:56bc8226b967 173 if (speed>0.2){
florine_van 19:56bc8226b967 174 speed -= 0.2;
florine_van 19:56bc8226b967 175 }
florine_van 19:56bc8226b967 176 break;
florine_van 19:56bc8226b967 177
florine_van 19:56bc8226b967 178 // Switch control type
florine_van 19:56bc8226b967 179 case 7:
florine_van 19:56bc8226b967 180 control_type =! control_type;
florine_van 19:56bc8226b967 181 break;
dnulty 16:11282b7ee726 182
florine_van 19:56bc8226b967 183 // Turn around
florine_van 19:56bc8226b967 184 case 8:
florine_van 19:56bc8226b967 185 if (control_type == 1 ){
florine_van 19:56bc8226b967 186 move(-speed, speed, 5376);
florine_van 19:56bc8226b967 187 }
florine_van 19:56bc8226b967 188 break;
florine_van 19:56bc8226b967 189
florine_van 19:56bc8226b967 190 // Draw a square
florine_van 19:56bc8226b967 191 case 9:
florine_van 19:56bc8226b967 192 for(int i = 0;i<4;i++){
florine_van 19:56bc8226b967 193 move(-speed, -speed, 6000);
florine_van 19:56bc8226b967 194 move(-speed,speed,896);
florine_van 19:56bc8226b967 195 }
florine_van 19:56bc8226b967 196 break;
florine_van 19:56bc8226b967 197 }
dnulty 16:11282b7ee726 198
florine_van 19:56bc8226b967 199 //Reset speed and pulse count
florine_van 19:56bc8226b967 200 A.speed(0);
florine_van 19:56bc8226b967 201 B.speed(0);
dnulty 14:08047fde54f6 202
florine_van 19:56bc8226b967 203 wheel_B.reset();
florine_van 19:56bc8226b967 204 wheel_A.reset();
dnulty 16:11282b7ee726 205 }
florine_van 10:be9de79cf6b0 206 }
florine_van 10:be9de79cf6b0 207
florine_van 5:8ef79eebbc97 208 int main()
florine_van 5:8ef79eebbc97 209 {
florine_van 7:2cf57f28255d 210 ros::NodeHandle nh;
florine_van 7:2cf57f28255d 211 nh.initNode();
florine_van 8:57aa8a35d983 212
dnulty 14:08047fde54f6 213 // ROS subscriber for motors control
florine_van 19:56bc8226b967 214 ros::Subscriber<mbed_custom_msgs::motor_msg> motor_sub("motor_control", &motor_callback);
florine_van 10:be9de79cf6b0 215 nh.subscribe(motor_sub);
dnulty 14:08047fde54f6 216
florine_van 3:a3144a45f44c 217 // load settings onto VL6180X sensors
florine_van 6:858a5116688e 218 sensor_forward.init();
florine_van 3:a3144a45f44c 219 // change default address of sensor 2
florine_van 6:858a5116688e 220 sensor_forward.changeAddress(addr2);
florine_van 2:c537f1ebad7b 221
florine_van 6:858a5116688e 222 sensor_right.init();
florine_van 3:a3144a45f44c 223 // change default address of sensor 3
florine_van 6:858a5116688e 224 sensor_right.changeAddress(addr3);
florine_van 2:c537f1ebad7b 225
florine_van 6:858a5116688e 226 sensor_back.init();
florine_van 3:a3144a45f44c 227 // change default address of sensor 4
florine_van 6:858a5116688e 228 sensor_back.changeAddress(addr4);
florine_van 6:858a5116688e 229
florine_van 6:858a5116688e 230 sensor_left.init();
florine_van 2:c537f1ebad7b 231
dnulty 14:08047fde54f6 232 thread1.start(callback(&flip));
dnulty 16:11282b7ee726 233 thread2.start(callback(&control_motor));
dnulty 14:08047fde54f6 234
dnulty 16:11282b7ee726 235 while(1) {
florine_van 19:56bc8226b967 236 nh.spinOnce();
dnulty 16:11282b7ee726 237 }
dnulty 14:08047fde54f6 238 }
florine_van 0:57855aafa907 239
florine_van 5:8ef79eebbc97 240
florine_van 5:8ef79eebbc97 241
dnulty 14:08047fde54f6 242