Soft robot team / Mbed OS Robot_team1

Robot team project

Dependencies:   QEI Motordriver ros_lib_melodic

main.cpp@15:7c9bf41dd187, 2019-11-19 (annotated)

Committer:
dnulty
Date:
Tue Nov 19 15:28:02 2019 +0000
Revision:
15:7c9bf41dd187
Parent:
14:08047fde54f6
Child:
16:11282b7ee726
Working ROS, Motor and Interrupt

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 15:7c9bf41dd187 91 // A.speed(0);
dnulty 15:7c9bf41dd187 92 // B.speed(0);
dnulty 15:7c9bf41dd187 93 //
dnulty 15:7c9bf41dd187 94 // wheel_B.reset();
dnulty 15:7c9bf41dd187 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 15:7c9bf41dd187 123 }
dnulty 14:08047fde54f6 124 A.speed(0);
dnulty 14:08047fde54f6 125 B.speed(0);
dnulty 15:7c9bf41dd187 126
dnulty 15:7c9bf41dd187 127 wheel_B.reset();
dnulty 15:7c9bf41dd187 128 wheel_A.reset();
florine_van 10:be9de79cf6b0 129 }
florine_van 10:be9de79cf6b0 130
florine_van 5:8ef79eebbc97 131 int main()
florine_van 5:8ef79eebbc97 132 {
florine_van 7:2cf57f28255d 133 ros::NodeHandle nh;
florine_van 7:2cf57f28255d 134 nh.initNode();
florine_van 8:57aa8a35d983 135
dnulty 14:08047fde54f6 136 // ROS subscriber for motors control
dnulty 14:08047fde54f6 137 ros::Subscriber<std_msgs::Int32> motor_sub("motor_control", &controlMotor);
florine_van 11:35809512ec11 138
florine_van 10:be9de79cf6b0 139 nh.subscribe(motor_sub);
dnulty 14:08047fde54f6 140
florine_van 3:a3144a45f44c 141 // load settings onto VL6180X sensors
florine_van 6:858a5116688e 142 sensor_forward.init();
florine_van 3:a3144a45f44c 143 // change default address of sensor 2
florine_van 6:858a5116688e 144 sensor_forward.changeAddress(addr2);
florine_van 2:c537f1ebad7b 145
florine_van 6:858a5116688e 146 sensor_right.init();
florine_van 3:a3144a45f44c 147 // change default address of sensor 3
florine_van 6:858a5116688e 148 sensor_right.changeAddress(addr3);
florine_van 2:c537f1ebad7b 149
florine_van 6:858a5116688e 150 sensor_back.init();
florine_van 3:a3144a45f44c 151 // change default address of sensor 4
florine_van 6:858a5116688e 152 sensor_back.changeAddress(addr4);
florine_van 6:858a5116688e 153
florine_van 6:858a5116688e 154 sensor_left.init();
florine_van 2:c537f1ebad7b 155
dnulty 14:08047fde54f6 156 thread1.start(callback(&flip));
dnulty 14:08047fde54f6 157
florine_van 0:57855aafa907 158 while (1)
dnulty 14:08047fde54f6 159 {
florine_van 7:2cf57f28255d 160 nh.spinOnce();
dnulty 14:08047fde54f6 161 wait(1);
dnulty 14:08047fde54f6 162
dnulty 14:08047fde54f6 163 }
dnulty 14:08047fde54f6 164 }
florine_van 0:57855aafa907 165
florine_van 5:8ef79eebbc97 166
florine_van 5:8ef79eebbc97 167
dnulty 14:08047fde54f6 168