Soft robot team / Mbed OS Robot_team1

Robot team project

Dependencies:   QEI Motordriver ros_lib_melodic

main.cpp@14:08047fde54f6, 2019-11-19 (annotated)

Committer:
dnulty
Date:
Tue Nov 19 12:42:43 2019 +0000
Revision:
14:08047fde54f6
Parent:
11:35809512ec11
Child:
15:7c9bf41dd187
Working interrupts and motor movement ROS

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 14:08047fde54f6 15 Thread thread1;
dnulty 14:08047fde54f6 16 Mutex SerialMutex;
dnulty 14:08047fde54f6 17
florine_van 0:57855aafa907 18 // Set up I²C on the STM32 NUCLEO-401RE
florine_van 0:57855aafa907 19 #define addr1 (0x29)
florine_van 0:57855aafa907 20 #define addr2 (0x2A)
florine_van 0:57855aafa907 21 #define addr3 (0x2B)
florine_van 0:57855aafa907 22 #define addr4 (0x2C)
florine_van 0:57855aafa907 23
florine_van 6:858a5116688e 24 #define S1 PC_8
florine_van 6:858a5116688e 25 #define S2 PC_9
florine_van 6:858a5116688e 26 #define S3 PC_10
florine_van 6:858a5116688e 27 #define S4 PC_11
florine_van 6:858a5116688e 28 #define S5 PC_12
florine_van 6:858a5116688e 29 #define S6 PD_2
florine_van 6:858a5116688e 30 #define S7 PG_2
florine_van 6:858a5116688e 31 #define S8 PG_3
florine_van 6:858a5116688e 32
dnulty 14:08047fde54f6 33 /*Ticker Sensor_readings()
dnulty 14:08047fde54f6 34 rangeForward = sensor_forward.read();
dnulty 14:08047fde54f6 35 */
dnulty 14:08047fde54f6 36
florine_van 5:8ef79eebbc97 37 // VL6180x sensors
florine_van 7:2cf57f28255d 38 Sensor sensor_back(I2C_SDA, I2C_SCL, S1);
florine_van 7:2cf57f28255d 39 Sensor sensor_left(I2C_SDA, I2C_SCL, S3);
florine_van 7:2cf57f28255d 40 Sensor sensor_forward(I2C_SDA, I2C_SCL, S5);
florine_van 7:2cf57f28255d 41 Sensor sensor_right(I2C_SDA, I2C_SCL, S7);
florine_van 5:8ef79eebbc97 42
dnulty 14:08047fde54f6 43 float sensor_forward_reading;
dnulty 14:08047fde54f6 44 float sensor_back_reading;
dnulty 14:08047fde54f6 45 float sensor_left_reading;
dnulty 14:08047fde54f6 46 float sensor_right_reading;
dnulty 14:08047fde54f6 47
dnulty 14:08047fde54f6 48 // Set motorpins for driving
dnulty 14:08047fde54f6 49 Motor A(PB_13, PB_15, PC_6, 1); // pwm, fwd, rev, can brake
dnulty 14:08047fde54f6 50 Motor B(PB_4, PC_7, PA_15, 1); // pwm, fwd, rev, can brake
florine_van 11:35809512ec11 51 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 52 QEI wheel_B (PB_5, PB_3, NC, 3576, QEI::X4_ENCODING); //counts per rev = 12 * 298:1, use quadrature encoding
dnulty 14:08047fde54f6 53
dnulty 14:08047fde54f6 54 void flip() {
dnulty 14:08047fde54f6 55 while(1) {
dnulty 14:08047fde54f6 56 SerialMutex.lock();
dnulty 14:08047fde54f6 57 sensor_forward_reading = sensor_forward.read();
dnulty 14:08047fde54f6 58 sensor_back_reading = sensor_back.read();
dnulty 14:08047fde54f6 59 sensor_left_reading = sensor_left.read();
dnulty 14:08047fde54f6 60 sensor_right_reading = sensor_right.read();
dnulty 14:08047fde54f6 61 SerialMutex.unlock();
dnulty 14:08047fde54f6 62 thread1.wait(2);
dnulty 14:08047fde54f6 63 }
dnulty 14:08047fde54f6 64
dnulty 14:08047fde54f6 65 }
dnulty 14:08047fde54f6 66
dnulty 14:08047fde54f6 67 void move(float motorA, float motorB, int distance)
dnulty 14:08047fde54f6 68 {
dnulty 14:08047fde54f6 69 // Variables to allow for any pulse reading
dnulty 14:08047fde54f6 70 float current_pulse_reading = 0;
dnulty 14:08047fde54f6 71
dnulty 14:08047fde54f6 72 // - is forward on our robot
dnulty 14:08047fde54f6 73 A.speed(motorA);
dnulty 14:08047fde54f6 74 B.speed(motorB);
dnulty 14:08047fde54f6 75 // loop for moving forward
dnulty 14:08047fde54f6 76 while(abs(current_pulse_reading) <= distance)
dnulty 14:08047fde54f6 77 {
dnulty 14:08047fde54f6 78 //pc.printf("current_pulse_reading 1= %f\r\n", current_pulse_reading);
dnulty 14:08047fde54f6 79 current_pulse_reading = wheel_A.getPulses();
dnulty 14:08047fde54f6 80 //SerialMutex.lock();
dnulty 14:08047fde54f6 81 //pc.printf("forward = %f, back = %f\n\r", sensor_forward_reading, sensor_back_reading);
dnulty 14:08047fde54f6 82 //SerialMutex.unlock();
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 14:08047fde54f6 86 if (sensor_forward_reading <255 and motorA<0 and motorB<0){
dnulty 14:08047fde54f6 87 return;
dnulty 14:08047fde54f6 88 }
dnulty 14:08047fde54f6 89 }
dnulty 14:08047fde54f6 90
dnulty 14:08047fde54f6 91 A.speed(0);
dnulty 14:08047fde54f6 92 B.speed(0);
dnulty 14:08047fde54f6 93
dnulty 14:08047fde54f6 94 wheel_B.reset();
dnulty 14:08047fde54f6 95 wheel_A.reset();
dnulty 14:08047fde54f6 96
dnulty 14:08047fde54f6 97 }
dnulty 14:08047fde54f6 98
florine_van 5:8ef79eebbc97 99
florine_van 10:be9de79cf6b0 100 void controlMotor(const std_msgs::Int32& motor_dir)
florine_van 10:be9de79cf6b0 101 {
florine_van 11:35809512ec11 102 switch(motor_dir.data) {
dnulty 14:08047fde54f6 103
dnulty 14:08047fde54f6 104 case 0://49
dnulty 14:08047fde54f6 105 move(-0.5, -0.5, 10000);
florine_van 10:be9de79cf6b0 106 break;
florine_van 10:be9de79cf6b0 107
florine_van 10:be9de79cf6b0 108 // Move left
dnulty 14:08047fde54f6 109 case 1://50
dnulty 14:08047fde54f6 110 move(0.5, -0.5, 5000);
florine_van 10:be9de79cf6b0 111 break;
florine_van 10:be9de79cf6b0 112
florine_van 10:be9de79cf6b0 113 // Move right
dnulty 14:08047fde54f6 114 case 2://51
dnulty 14:08047fde54f6 115 move(-0.5, 0.5, 5000);
dnulty 14:08047fde54f6 116 break;
dnulty 14:08047fde54f6 117
dnulty 14:08047fde54f6 118 // Reverse
dnulty 14:08047fde54f6 119 case 3://52
dnulty 14:08047fde54f6 120 move(0.5, 0.5, 10000);
florine_van 10:be9de79cf6b0 121 break;
dnulty 14:08047fde54f6 122
dnulty 14:08047fde54f6 123 case default:
dnulty 14:08047fde54f6 124 A.speed(0);
dnulty 14:08047fde54f6 125 B.speed(0);
florine_van 10:be9de79cf6b0 126 }
dnulty 14:08047fde54f6 127 A.speed(0);
dnulty 14:08047fde54f6 128 B.speed(0);
dnulty 14:08047fde54f6 129 // switch(motor_dir.data) {
dnulty 14:08047fde54f6 130 // case 0:
dnulty 14:08047fde54f6 131 // motor_left.stop();
dnulty 14:08047fde54f6 132 // motor_right.stop();
dnulty 14:08047fde54f6 133 // break;
dnulty 14:08047fde54f6 134 //
dnulty 14:08047fde54f6 135 // // Move forward
dnulty 14:08047fde54f6 136 // case 1:
dnulty 14:08047fde54f6 137 // move("forward", -0.5, -0.5);
dnulty 14:08047fde54f6 138 // break;
dnulty 14:08047fde54f6 139 //
dnulty 14:08047fde54f6 140 // // Move left
dnulty 14:08047fde54f6 141 // case 2:
dnulty 14:08047fde54f6 142 // move("left", 0.5, -0.5, 3000, -1);
dnulty 14:08047fde54f6 143 // break;
dnulty 14:08047fde54f6 144 //
dnulty 14:08047fde54f6 145 // // Move right
dnulty 14:08047fde54f6 146 // case 3:
dnulty 14:08047fde54f6 147 // move("right", -0.5, 0.5, 3000, 1);
dnulty 14:08047fde54f6 148 // break;
dnulty 14:08047fde54f6 149 //
dnulty 14:08047fde54f6 150 // // Reverse
dnulty 14:08047fde54f6 151 // case 4:
dnulty 14:08047fde54f6 152 // move("reverse", 0.5, 0.5, 5691, -1);
dnulty 14:08047fde54f6 153 // break;
dnulty 14:08047fde54f6 154 // }
florine_van 10:be9de79cf6b0 155 }
florine_van 10:be9de79cf6b0 156
dnulty 14:08047fde54f6 157
dnulty 14:08047fde54f6 158
florine_van 11:35809512ec11 159
florine_van 7:2cf57f28255d 160
florine_van 5:8ef79eebbc97 161 int main()
florine_van 5:8ef79eebbc97 162 {
florine_van 7:2cf57f28255d 163 ros::NodeHandle nh;
florine_van 7:2cf57f28255d 164 nh.initNode();
florine_van 8:57aa8a35d983 165
dnulty 14:08047fde54f6 166 // ROS subscriber for motors control
dnulty 14:08047fde54f6 167 ros::Subscriber<std_msgs::Int32> motor_sub("motor_control", &controlMotor);
florine_van 11:35809512ec11 168
florine_van 10:be9de79cf6b0 169 nh.subscribe(motor_sub);
dnulty 14:08047fde54f6 170
florine_van 3:a3144a45f44c 171 // load settings onto VL6180X sensors
florine_van 6:858a5116688e 172 sensor_forward.init();
florine_van 3:a3144a45f44c 173 // change default address of sensor 2
florine_van 6:858a5116688e 174 sensor_forward.changeAddress(addr2);
florine_van 2:c537f1ebad7b 175
florine_van 6:858a5116688e 176 sensor_right.init();
florine_van 3:a3144a45f44c 177 // change default address of sensor 3
florine_van 6:858a5116688e 178 sensor_right.changeAddress(addr3);
florine_van 2:c537f1ebad7b 179
florine_van 6:858a5116688e 180 sensor_back.init();
florine_van 3:a3144a45f44c 181 // change default address of sensor 4
florine_van 6:858a5116688e 182 sensor_back.changeAddress(addr4);
florine_van 6:858a5116688e 183
florine_van 6:858a5116688e 184 sensor_left.init();
florine_van 2:c537f1ebad7b 185
dnulty 14:08047fde54f6 186 thread1.start(callback(&flip));
dnulty 14:08047fde54f6 187
florine_van 0:57855aafa907 188 while (1)
dnulty 14:08047fde54f6 189 {
florine_van 7:2cf57f28255d 190 nh.spinOnce();
dnulty 14:08047fde54f6 191 wait(1);
dnulty 14:08047fde54f6 192
dnulty 14:08047fde54f6 193 }
dnulty 14:08047fde54f6 194 }
florine_van 0:57855aafa907 195
florine_van 5:8ef79eebbc97 196
florine_van 5:8ef79eebbc97 197
dnulty 14:08047fde54f6 198