Soft robot team / Mbed OS Robot_team1

Robot team project

Dependencies:   QEI Motordriver ros_lib_melodic

main.cpp@13:d41144f89195, 2019-11-19 (annotated)

Committer:
florine_van
Date:
Tue Nov 19 09:28:39 2019 +0000
Revision:
13:d41144f89195
Parent:
12:817da876ae2f 
Add distance setting subscriber

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 12:817da876ae2f 3 #include <cstdlib>
florine_van 7:2cf57f28255d 4 #include <std_msgs/Empty.h>
florine_van 7:2cf57f28255d 5 #include <std_msgs/Int32.h>
florine_van 10:be9de79cf6b0 6 #include <std_msgs/String.h>
florine_van 8:57aa8a35d983 7 #include <mbed_custom_msgs/lidar_msg.h>
florine_van 5:8ef79eebbc97 8
florine_van 0:57855aafa907 9 #include "Sensor.h"
florine_van 5:8ef79eebbc97 10 #include "Motor.h"
florine_van 12:817da876ae2f 11 #include "QEI.h"
florine_van 7:2cf57f28255d 12
florine_van 0:57855aafa907 13 // Set up I²C on the STM32 NUCLEO-401RE
florine_van 0:57855aafa907 14 #define addr1 (0x29)
florine_van 0:57855aafa907 15 #define addr2 (0x2A)
florine_van 0:57855aafa907 16 #define addr3 (0x2B)
florine_van 0:57855aafa907 17 #define addr4 (0x2C)
florine_van 0:57855aafa907 18
florine_van 6:858a5116688e 19 #define S1 PC_8
florine_van 6:858a5116688e 20 #define S2 PC_9
florine_van 6:858a5116688e 21 #define S3 PC_10
florine_van 6:858a5116688e 22 #define S4 PC_11
florine_van 6:858a5116688e 23 #define S5 PC_12
florine_van 6:858a5116688e 24 #define S6 PD_2
florine_van 6:858a5116688e 25 #define S7 PG_2
florine_van 6:858a5116688e 26 #define S8 PG_3
florine_van 6:858a5116688e 27
florine_van 5:8ef79eebbc97 28 // VL6180x sensors
florine_van 7:2cf57f28255d 29 Sensor sensor_back(I2C_SDA, I2C_SCL, S1);
florine_van 7:2cf57f28255d 30 Sensor sensor_left(I2C_SDA, I2C_SCL, S3);
florine_van 7:2cf57f28255d 31 Sensor sensor_forward(I2C_SDA, I2C_SCL, S5);
florine_van 7:2cf57f28255d 32 Sensor sensor_right(I2C_SDA, I2C_SCL, S7);
florine_van 5:8ef79eebbc97 33
florine_van 5:8ef79eebbc97 34 // Motors
florine_van 11:35809512ec11 35 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 36 QEI wheel_B (PB_5, PB_3, NC, 3576, QEI::X4_ENCODING); //counts per rev = 12 * 298:1, use quadrature encoding
florine_van 12:817da876ae2f 37 Motor motor_left(PC_6, PB_15, PB_13);
florine_van 12:817da876ae2f 38 Motor motor_right(PA_15, PC_7, PB_4);
florine_van 12:817da876ae2f 39
florine_van 13:d41144f89195 40 // Distance settings
florine_van 13:d41144f89195 41 int nbPulse = 5691;
florine_van 12:817da876ae2f 42
florine_van 13:d41144f89195 43
florine_van 13:d41144f89195 44 void pulse(int pulse)
florine_van 12:817da876ae2f 45 {
florine_van 12:817da876ae2f 46 // Variable to allow for any pulse reading
florine_van 12:817da876ae2f 47 float current_pulse_reading = wheel_B.getPulses();
florine_van 12:817da876ae2f 48
florine_van 13:d41144f89195 49 while(abs(current_pulse_reading) <= pulse)
florine_van 12:817da876ae2f 50 {
florine_van 12:817da876ae2f 51 current_pulse_reading = wheel_A.getPulses();
florine_van 12:817da876ae2f 52 }
florine_van 12:817da876ae2f 53
florine_van 12:817da876ae2f 54 motor_left.stop();
florine_van 12:817da876ae2f 55 motor_right.stop();
florine_van 12:817da876ae2f 56
florine_van 12:817da876ae2f 57 wheel_A.reset();
florine_van 12:817da876ae2f 58 wheel_B.reset();
florine_van 12:817da876ae2f 59 }
florine_van 5:8ef79eebbc97 60
florine_van 10:be9de79cf6b0 61 void controlMotor(const std_msgs::Int32& motor_dir)
florine_van 10:be9de79cf6b0 62 {
florine_van 11:35809512ec11 63 switch(motor_dir.data) {
florine_van 12:817da876ae2f 64 // Stop motors
florine_van 11:35809512ec11 65 case 0:
florine_van 12:817da876ae2f 66 motor_left.stop();
florine_van 12:817da876ae2f 67 motor_right.stop();
florine_van 11:35809512ec11 68 break;
florine_van 11:35809512ec11 69
florine_van 10:be9de79cf6b0 70 // Move forward
florine_van 10:be9de79cf6b0 71 case 1:
florine_van 12:817da876ae2f 72 motor_left.moveForward();
florine_van 12:817da876ae2f 73 motor_right.moveForward();
florine_van 13:d41144f89195 74 pulse(nbPulse);
florine_van 10:be9de79cf6b0 75 break;
florine_van 10:be9de79cf6b0 76
florine_van 10:be9de79cf6b0 77 // Move left
florine_van 10:be9de79cf6b0 78 case 2:
florine_van 12:817da876ae2f 79 motor_left.moveBackward();
florine_van 12:817da876ae2f 80 motor_right.moveForward();
florine_van 12:817da876ae2f 81 pulse(3000);
florine_van 10:be9de79cf6b0 82 break;
florine_van 10:be9de79cf6b0 83
florine_van 10:be9de79cf6b0 84 // Move right
florine_van 10:be9de79cf6b0 85 case 3:
florine_van 12:817da876ae2f 86 motor_left.moveForward();
florine_van 12:817da876ae2f 87 motor_right.moveBackward();
florine_van 12:817da876ae2f 88 pulse(3000);
florine_van 12:817da876ae2f 89 break;
florine_van 12:817da876ae2f 90
florine_van 12:817da876ae2f 91 // Reverse
florine_van 12:817da876ae2f 92 case 4:
florine_van 12:817da876ae2f 93 motor_left.moveBackward();
florine_van 12:817da876ae2f 94 motor_right.moveBackward();
florine_van 13:d41144f89195 95 pulse(nbPulse);
florine_van 10:be9de79cf6b0 96 break;
florine_van 10:be9de79cf6b0 97 }
florine_van 10:be9de79cf6b0 98 }
florine_van 10:be9de79cf6b0 99
florine_van 13:d41144f89195 100 void setDistance(const std_msgs::Int32& distance)
florine_van 13:d41144f89195 101 {
florine_van 13:d41144f89195 102 nbPulse = (int) 189.7 * distance.data;
florine_van 13:d41144f89195 103 }
florine_van 13:d41144f89195 104
florine_van 5:8ef79eebbc97 105 int main()
florine_van 5:8ef79eebbc97 106 {
florine_van 7:2cf57f28255d 107 ros::NodeHandle nh;
florine_van 7:2cf57f28255d 108 nh.initNode();
florine_van 8:57aa8a35d983 109
florine_van 10:be9de79cf6b0 110 // ROS publisher for sensor readings
florine_van 12:817da876ae2f 111 mbed_custom_msgs::lidar_msg lidar_msg;
florine_van 12:817da876ae2f 112 ros::Publisher lidar_pub("lidar_reading", &lidar_msg);
florine_van 12:817da876ae2f 113
florine_van 12:817da876ae2f 114 // ROS subscriber for motors control
florine_van 12:817da876ae2f 115 ros::Subscriber<std_msgs::Int32> motor_sub("motor_control", &controlMotor);
florine_van 13:d41144f89195 116
florine_van 13:d41144f89195 117 // ROS subscriber for distance setting
florine_van 13:d41144f89195 118 ros::Subscriber<std_msgs::Int32> distance_sub("distance_setting", &setDistance);
florine_van 11:35809512ec11 119
florine_van 8:57aa8a35d983 120 nh.advertise(lidar_pub);
florine_van 10:be9de79cf6b0 121 nh.subscribe(motor_sub);
florine_van 13:d41144f89195 122 nh.subscribe(distance_sub);
florine_van 3:a3144a45f44c 123
florine_van 3:a3144a45f44c 124 // load settings onto VL6180X sensors
florine_van 6:858a5116688e 125 sensor_forward.init();
florine_van 3:a3144a45f44c 126 // change default address of sensor 2
florine_van 6:858a5116688e 127 sensor_forward.changeAddress(addr2);
florine_van 12:817da876ae2f 128
florine_van 6:858a5116688e 129 sensor_right.init();
florine_van 3:a3144a45f44c 130 // change default address of sensor 3
florine_van 6:858a5116688e 131 sensor_right.changeAddress(addr3);
florine_van 2:c537f1ebad7b 132
florine_van 6:858a5116688e 133 sensor_back.init();
florine_van 3:a3144a45f44c 134 // change default address of sensor 4
florine_van 6:858a5116688e 135 sensor_back.changeAddress(addr4);
florine_van 6:858a5116688e 136
florine_van 6:858a5116688e 137 sensor_left.init();
florine_van 2:c537f1ebad7b 138
florine_van 6:858a5116688e 139 //Set Speeds
florine_van 6:858a5116688e 140 motor_left.setSpeed(0.5f);
florine_van 6:858a5116688e 141 motor_right.setSpeed(0.5f);
florine_van 0:57855aafa907 142
florine_van 0:57855aafa907 143 while (1)
florine_van 12:817da876ae2f 144 {
florine_van 12:817da876ae2f 145 lidar_msg.sensor_forward = sensor_forward.read();
florine_van 12:817da876ae2f 146 lidar_msg.sensor_back = sensor_back.read();
florine_van 12:817da876ae2f 147 lidar_msg.sensor_left = sensor_left.read();
florine_van 12:817da876ae2f 148 lidar_msg.sensor_right = sensor_right.read();
florine_van 12:817da876ae2f 149 lidar_pub.publish(&lidar_msg);
florine_van 12:817da876ae2f 150
florine_van 7:2cf57f28255d 151 nh.spinOnce();
florine_van 5:8ef79eebbc97 152
florine_van 4:cb50c6fa340b 153 wait_ms(10);
florine_van 0:57855aafa907 154 }
florine_van 0:57855aafa907 155 }
florine_van 0:57855aafa907 156
florine_van 5:8ef79eebbc97 157
florine_van 5:8ef79eebbc97 158