Soft robot team / Mbed OS Robot_team1

Robot team project

Dependencies:   QEI Motordriver ros_lib_melodic

main.cpp@16:11282b7ee726, 2019-11-23 (annotated)

Committer:
dnulty
Date:
Sat Nov 23 15:33:23 2019 +0000
Revision:
16:11282b7ee726
Parent:
15:7c9bf41dd187
Child:
17:8831c676778b
23/11/19

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 16:11282b7ee726 46 double speed=0.6;
dnulty 16:11282b7ee726 47 bool control_type = 1;;
dnulty 16:11282b7ee726 48 int32_t motor_option;
dnulty 14:08047fde54f6 49
dnulty 14:08047fde54f6 50 // Set motorpins for driving
dnulty 14:08047fde54f6 51 Motor A(PB_13, PB_15, PC_6, 1); // pwm, fwd, rev, can brake
dnulty 14:08047fde54f6 52 Motor B(PB_4, PC_7, PA_15, 1); // pwm, fwd, rev, can brake
florine_van 11:35809512ec11 53 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 54 QEI wheel_B (PB_5, PB_3, NC, 3576, QEI::X4_ENCODING); //counts per rev = 12 * 298:1, use quadrature encoding
dnulty 14:08047fde54f6 55
dnulty 16:11282b7ee726 56 //Thread function to update sensor readings
dnulty 14:08047fde54f6 57 void flip() {
dnulty 14:08047fde54f6 58 while(1) {
dnulty 14:08047fde54f6 59 SerialMutex.lock();
dnulty 14:08047fde54f6 60 sensor_forward_reading = sensor_forward.read();
dnulty 14:08047fde54f6 61 sensor_back_reading = sensor_back.read();
dnulty 14:08047fde54f6 62 sensor_left_reading = sensor_left.read();
dnulty 14:08047fde54f6 63 sensor_right_reading = sensor_right.read();
dnulty 14:08047fde54f6 64 SerialMutex.unlock();
dnulty 14:08047fde54f6 65 thread1.wait(2);
dnulty 14:08047fde54f6 66 }
dnulty 14:08047fde54f6 67
dnulty 14:08047fde54f6 68 }
dnulty 14:08047fde54f6 69
dnulty 16:11282b7ee726 70 // Function to move robot (motor speed, motor speed, pulses to travel)
dnulty 14:08047fde54f6 71 void move(float motorA, float motorB, int distance)
dnulty 14:08047fde54f6 72 {
dnulty 14:08047fde54f6 73 float current_pulse_reading = 0;
dnulty 14:08047fde54f6 74
dnulty 14:08047fde54f6 75 // - is forward on our robot
dnulty 14:08047fde54f6 76 A.speed(motorA);
dnulty 14:08047fde54f6 77 B.speed(motorB);
dnulty 16:11282b7ee726 78
dnulty 14:08047fde54f6 79 while(abs(current_pulse_reading) <= distance)
dnulty 14:08047fde54f6 80 {
dnulty 14:08047fde54f6 81 current_pulse_reading = wheel_A.getPulses();
dnulty 16:11282b7ee726 82 //Stops Robot moving forward if front sensor detects
dnulty 14:08047fde54f6 83 if (sensor_back_reading <255 and motorA>0 and motorB>0){
dnulty 14:08047fde54f6 84 return;
dnulty 14:08047fde54f6 85 }
dnulty 16:11282b7ee726 86 //Stops robot reversing if back sensors detects
dnulty 14:08047fde54f6 87 if (sensor_forward_reading <255 and motorA<0 and motorB<0){
dnulty 14:08047fde54f6 88 return;
dnulty 14:08047fde54f6 89 }
dnulty 14:08047fde54f6 90 }
dnulty 14:08047fde54f6 91 }
dnulty 14:08047fde54f6 92
florine_van 5:8ef79eebbc97 93
dnulty 16:11282b7ee726 94 void motor_callback(const std_msgs::Int32& motor_dir)
florine_van 10:be9de79cf6b0 95 {
dnulty 16:11282b7ee726 96 motor_option = motor_dir.data;
dnulty 16:11282b7ee726 97 thread2.signal_set(1);
dnulty 16:11282b7ee726 98 }
dnulty 16:11282b7ee726 99
dnulty 16:11282b7ee726 100 void control_motor(void) {
dnulty 16:11282b7ee726 101 while (1)
dnulty 16:11282b7ee726 102 {
dnulty 16:11282b7ee726 103 thread2.signal_wait(1);
dnulty 16:11282b7ee726 104 switch(motor_option) {
dnulty 16:11282b7ee726 105 //Forward 30cm
dnulty 16:11282b7ee726 106 case 0://49
dnulty 16:11282b7ee726 107 if (control_type == 1){
dnulty 16:11282b7ee726 108 move(-speed, -speed, 5960);
dnulty 16:11282b7ee726 109 }
dnulty 16:11282b7ee726 110 else if (control_type ==0){
dnulty 16:11282b7ee726 111 move(-speed, -speed, 50);
dnulty 16:11282b7ee726 112 }
dnulty 16:11282b7ee726 113 break;
florine_van 10:be9de79cf6b0 114
dnulty 16:11282b7ee726 115 //Left 30 degrees
dnulty 16:11282b7ee726 116 case 1://50
dnulty 16:11282b7ee726 117 if (control_type == 1 ){
dnulty 16:11282b7ee726 118 move(speed, -speed, 894);
dnulty 16:11282b7ee726 119 }
dnulty 16:11282b7ee726 120 else if (control_type == 0){
dnulty 16:11282b7ee726 121 move(speed, -speed, 50);
dnulty 16:11282b7ee726 122 }
dnulty 16:11282b7ee726 123 break;
dnulty 16:11282b7ee726 124
dnulty 16:11282b7ee726 125 //Right 30 degrees
dnulty 16:11282b7ee726 126 case 2://51
dnulty 16:11282b7ee726 127 if (control_type == 1 ){
dnulty 16:11282b7ee726 128 move(-speed, speed, 894);
dnulty 16:11282b7ee726 129 }
dnulty 16:11282b7ee726 130 else if (control_type == 0 ){
dnulty 16:11282b7ee726 131 move(-speed, speed, 30);
dnulty 16:11282b7ee726 132 }
dnulty 16:11282b7ee726 133 break;
dnulty 16:11282b7ee726 134
dnulty 16:11282b7ee726 135 // Reverse 30cm
dnulty 16:11282b7ee726 136 case 3://52
dnulty 16:11282b7ee726 137 if (control_type ==1 ){
dnulty 16:11282b7ee726 138 move(speed, speed, 5960);
dnulty 16:11282b7ee726 139 }
dnulty 16:11282b7ee726 140 else if (control_type ==0 ){
dnulty 16:11282b7ee726 141 move(speed, speed, 50);
dnulty 16:11282b7ee726 142 }
dnulty 16:11282b7ee726 143 break;
dnulty 16:11282b7ee726 144
dnulty 16:11282b7ee726 145 case 4://speed up
dnulty 16:11282b7ee726 146 if (speed<1.0){
dnulty 16:11282b7ee726 147 speed += 0.2;
dnulty 16:11282b7ee726 148 }
dnulty 16:11282b7ee726 149 break;
dnulty 16:11282b7ee726 150
dnulty 16:11282b7ee726 151 case 5://speed down
dnulty 16:11282b7ee726 152 if (speed>0.2){
dnulty 16:11282b7ee726 153 speed -= 0.2;
dnulty 16:11282b7ee726 154 }
dnulty 16:11282b7ee726 155 break;
dnulty 16:11282b7ee726 156
dnulty 16:11282b7ee726 157 case 6: // switch control type
dnulty 16:11282b7ee726 158 control_type =! control_type;
dnulty 16:11282b7ee726 159 break;
dnulty 16:11282b7ee726 160 }
dnulty 16:11282b7ee726 161
dnulty 16:11282b7ee726 162 /*
dnulty 16:11282b7ee726 163 case 8: //control type 3
dnulty 16:11282b7ee726 164 control_type = 3;
dnulty 16:11282b7ee726 165 */
dnulty 16:11282b7ee726 166 //Reset speed and pulse count
dnulty 16:11282b7ee726 167 A.speed(0);
dnulty 16:11282b7ee726 168 B.speed(0);
dnulty 14:08047fde54f6 169
dnulty 16:11282b7ee726 170 wheel_B.reset();
dnulty 16:11282b7ee726 171 wheel_A.reset();
dnulty 16:11282b7ee726 172 //nh.spinOnce();
dnulty 16:11282b7ee726 173 }
florine_van 10:be9de79cf6b0 174 }
florine_van 10:be9de79cf6b0 175
florine_van 5:8ef79eebbc97 176 int main()
florine_van 5:8ef79eebbc97 177 {
florine_van 7:2cf57f28255d 178 ros::NodeHandle nh;
florine_van 7:2cf57f28255d 179 nh.initNode();
florine_van 8:57aa8a35d983 180
dnulty 14:08047fde54f6 181 // ROS subscriber for motors control
dnulty 16:11282b7ee726 182 ros::Subscriber<std_msgs::Int32> motor_sub("motor_control", &motor_callback);
florine_van 11:35809512ec11 183
florine_van 10:be9de79cf6b0 184 nh.subscribe(motor_sub);
dnulty 14:08047fde54f6 185
florine_van 3:a3144a45f44c 186 // load settings onto VL6180X sensors
florine_van 6:858a5116688e 187 sensor_forward.init();
florine_van 3:a3144a45f44c 188 // change default address of sensor 2
florine_van 6:858a5116688e 189 sensor_forward.changeAddress(addr2);
florine_van 2:c537f1ebad7b 190
florine_van 6:858a5116688e 191 sensor_right.init();
florine_van 3:a3144a45f44c 192 // change default address of sensor 3
florine_van 6:858a5116688e 193 sensor_right.changeAddress(addr3);
florine_van 2:c537f1ebad7b 194
florine_van 6:858a5116688e 195 sensor_back.init();
florine_van 3:a3144a45f44c 196 // change default address of sensor 4
florine_van 6:858a5116688e 197 sensor_back.changeAddress(addr4);
florine_van 6:858a5116688e 198
florine_van 6:858a5116688e 199 sensor_left.init();
florine_van 2:c537f1ebad7b 200
dnulty 14:08047fde54f6 201 thread1.start(callback(&flip));
dnulty 16:11282b7ee726 202 thread2.start(callback(&control_motor));
dnulty 14:08047fde54f6 203
dnulty 16:11282b7ee726 204 while(1) {
florine_van 7:2cf57f28255d 205 nh.spinOnce();
dnulty 16:11282b7ee726 206
dnulty 16:11282b7ee726 207 }
dnulty 14:08047fde54f6 208
dnulty 16:11282b7ee726 209
dnulty 14:08047fde54f6 210 }
florine_van 0:57855aafa907 211
florine_van 5:8ef79eebbc97 212
florine_van 5:8ef79eebbc97 213
dnulty 14:08047fde54f6 214