Soft robot team / Mbed OS Robot_team1

Robot team project

Dependencies:   QEI Motordriver ros_lib_melodic

main.cpp@10:be9de79cf6b0, 2019-11-12 (annotated)

Committer:
florine_van
Date:
Tue Nov 12 13:55:17 2019 +0000
Revision:
10:be9de79cf6b0
Parent:
8:57aa8a35d983
Child:
11:35809512ec11
Working code for lidar_reading 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 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>
florine_van 5:8ef79eebbc97 7
florine_van 0:57855aafa907 8 #include "Sensor.h"
florine_van 5:8ef79eebbc97 9 #include "Motor.h"
florine_van 0:57855aafa907 10
florine_van 0:57855aafa907 11 // Set up serial to pc
florine_van 7:2cf57f28255d 12 //Serial pc(SERIAL_TX, SERIAL_RX);
florine_van 7:2cf57f28255d 13
florine_van 0:57855aafa907 14 // Set up I²C on the STM32 NUCLEO-401RE
florine_van 0:57855aafa907 15 #define addr1 (0x29)
florine_van 0:57855aafa907 16 #define addr2 (0x2A)
florine_van 0:57855aafa907 17 #define addr3 (0x2B)
florine_van 0:57855aafa907 18 #define addr4 (0x2C)
florine_van 0:57855aafa907 19
florine_van 6:858a5116688e 20 #define S1 PC_8
florine_van 6:858a5116688e 21 #define S2 PC_9
florine_van 6:858a5116688e 22 #define S3 PC_10
florine_van 6:858a5116688e 23 #define S4 PC_11
florine_van 6:858a5116688e 24 #define S5 PC_12
florine_van 6:858a5116688e 25 #define S6 PD_2
florine_van 6:858a5116688e 26 #define S7 PG_2
florine_van 6:858a5116688e 27 #define S8 PG_3
florine_van 6:858a5116688e 28
florine_van 5:8ef79eebbc97 29 // VL6180x sensors
florine_van 7:2cf57f28255d 30 Sensor sensor_back(I2C_SDA, I2C_SCL, S1);
florine_van 7:2cf57f28255d 31 Sensor sensor_left(I2C_SDA, I2C_SCL, S3);
florine_van 7:2cf57f28255d 32 Sensor sensor_forward(I2C_SDA, I2C_SCL, S5);
florine_van 7:2cf57f28255d 33 Sensor sensor_right(I2C_SDA, I2C_SCL, S7);
florine_van 5:8ef79eebbc97 34
florine_van 5:8ef79eebbc97 35 // Motors
florine_van 6:858a5116688e 36 Motor motor_left(PC_6, PB_15, PB_13);
florine_van 6:858a5116688e 37 Motor motor_right(PA_15, PC_7, PB_4);
florine_van 7:2cf57f28255d 38
florine_van 10:be9de79cf6b0 39 /*
florine_van 7:2cf57f28255d 40 void avoidObstacle(const std_msgs::Empty& avoid_obstacle_msg)
florine_van 6:858a5116688e 41 {
florine_van 5:8ef79eebbc97 42 // When obstacle ahead
florine_van 6:858a5116688e 43 if (sensor_forward.getIsObstacle())
florine_van 5:8ef79eebbc97 44 {
florine_van 6:858a5116688e 45 if ( (sensor_right.getIsObstacle()) && (sensor_left.getIsObstacle()) )
florine_van 6:858a5116688e 46 {
florine_van 6:858a5116688e 47 //Turn backward
florine_van 6:858a5116688e 48 while(!sensor_back.getIsObstacle())
florine_van 6:858a5116688e 49 {
florine_van 6:858a5116688e 50 motor_left.moveForward();
florine_van 6:858a5116688e 51 motor_right.moveBackward();
florine_van 6:858a5116688e 52 }
florine_van 6:858a5116688e 53 }
florine_van 6:858a5116688e 54 if (sensor_left.getIsObstacle())
florine_van 6:858a5116688e 55 {
florine_van 6:858a5116688e 56 //Turn to the right
florine_van 6:858a5116688e 57 motor_left.moveForward();
florine_van 6:858a5116688e 58 motor_right.moveBackward();
florine_van 6:858a5116688e 59 }
florine_van 6:858a5116688e 60 else
florine_van 6:858a5116688e 61 {
florine_van 6:858a5116688e 62 // By default : turn to the left
florine_van 6:858a5116688e 63 motor_left.moveBackward();
florine_van 6:858a5116688e 64 motor_right.moveForward();
florine_van 6:858a5116688e 65 }
florine_van 5:8ef79eebbc97 66 }
florine_van 5:8ef79eebbc97 67 // No obstacle
florine_van 5:8ef79eebbc97 68 else
florine_van 5:8ef79eebbc97 69 {
florine_van 6:858a5116688e 70 motor_left.moveForward();
florine_van 6:858a5116688e 71 motor_right.moveForward();
florine_van 5:8ef79eebbc97 72 }
florine_van 5:8ef79eebbc97 73 }
florine_van 8:57aa8a35d983 74 */
florine_van 5:8ef79eebbc97 75
florine_van 10:be9de79cf6b0 76 /*
florine_van 10:be9de79cf6b0 77 TODO REMOVE COMMENT WHEN CUSTOM MSGS READY
florine_van 10:be9de79cf6b0 78 void controlMotor(const std_msgs::Int32& motor_dir)
florine_van 10:be9de79cf6b0 79 {
florine_van 10:be9de79cf6b0 80 switch (motor_dir)
florine_van 10:be9de79cf6b0 81 {
florine_van 10:be9de79cf6b0 82 // Move forward
florine_van 10:be9de79cf6b0 83 case 1:
florine_van 10:be9de79cf6b0 84 motor_left.moveForward();
florine_van 10:be9de79cf6b0 85 motor_right.moveForward();
florine_van 10:be9de79cf6b0 86 break;
florine_van 10:be9de79cf6b0 87
florine_van 10:be9de79cf6b0 88 // Move left
florine_van 10:be9de79cf6b0 89 case 2:
florine_van 10:be9de79cf6b0 90 motor_left.moveBackward();
florine_van 10:be9de79cf6b0 91 motor_right.moveForward();
florine_van 10:be9de79cf6b0 92 break;
florine_van 10:be9de79cf6b0 93
florine_van 10:be9de79cf6b0 94 // Move right
florine_van 10:be9de79cf6b0 95 case 3:
florine_van 10:be9de79cf6b0 96 motor_left.moveForward();
florine_van 10:be9de79cf6b0 97 motor_right.moveBackward();
florine_van 10:be9de79cf6b0 98 break;
florine_van 10:be9de79cf6b0 99 }
florine_van 10:be9de79cf6b0 100 }
florine_van 10:be9de79cf6b0 101 */
florine_van 10:be9de79cf6b0 102
florine_van 10:be9de79cf6b0 103 /*
florine_van 10:be9de79cf6b0 104 TODO REMOVE COMMENT WHEN CUSTOM MSGS READY
florine_van 10:be9de79cf6b0 105 // ROS subscriber for motors control
florine_van 10:be9de79cf6b0 106 ros::Subscriber<std_msgs::Int32> motor_sub("motor_control", &controlMotor);
florine_van 10:be9de79cf6b0 107 */
florine_van 7:2cf57f28255d 108
florine_van 5:8ef79eebbc97 109 int main()
florine_van 5:8ef79eebbc97 110 {
florine_van 7:2cf57f28255d 111 ros::NodeHandle nh;
florine_van 7:2cf57f28255d 112 nh.initNode();
florine_van 8:57aa8a35d983 113
florine_van 10:be9de79cf6b0 114 // ROS publisher for sensor readings
florine_van 8:57aa8a35d983 115 mbed_custom_msgs::lidar_msg int_lidar_msg;
florine_van 8:57aa8a35d983 116 ros::Publisher lidar_pub("lidar_reading", &int_lidar_msg);
florine_van 10:be9de79cf6b0 117 /*
florine_van 10:be9de79cf6b0 118 std_msgs::Int32 int_msg;
florine_van 10:be9de79cf6b0 119 ros::Publisher lidar_pub("lidar_reading", &int_msg);
florine_van 10:be9de79cf6b0 120 */
florine_van 8:57aa8a35d983 121 nh.advertise(lidar_pub);
florine_van 7:2cf57f28255d 122
florine_van 10:be9de79cf6b0 123 /*
florine_van 10:be9de79cf6b0 124 TODO REMOVE COMMENT WHEN CUSTOM MSGS READY
florine_van 10:be9de79cf6b0 125 nh.subscribe(motor_sub);
florine_van 10:be9de79cf6b0 126 */
florine_van 3:a3144a45f44c 127
florine_van 3:a3144a45f44c 128 // load settings onto VL6180X sensors
florine_van 6:858a5116688e 129 sensor_forward.init();
florine_van 3:a3144a45f44c 130 // change default address of sensor 2
florine_van 6:858a5116688e 131 sensor_forward.changeAddress(addr2);
florine_van 2:c537f1ebad7b 132
florine_van 6:858a5116688e 133 sensor_right.init();
florine_van 3:a3144a45f44c 134 // change default address of sensor 3
florine_van 6:858a5116688e 135 sensor_right.changeAddress(addr3);
florine_van 2:c537f1ebad7b 136
florine_van 6:858a5116688e 137 sensor_back.init();
florine_van 3:a3144a45f44c 138 // change default address of sensor 4
florine_van 6:858a5116688e 139 sensor_back.changeAddress(addr4);
florine_van 6:858a5116688e 140
florine_van 6:858a5116688e 141 sensor_left.init();
florine_van 2:c537f1ebad7b 142
florine_van 6:858a5116688e 143 //Set Speeds
florine_van 6:858a5116688e 144 motor_left.setSpeed(0.5f);
florine_van 6:858a5116688e 145 motor_right.setSpeed(0.5f);
florine_van 0:57855aafa907 146
florine_van 0:57855aafa907 147 while (1)
florine_van 10:be9de79cf6b0 148 {
florine_van 8:57aa8a35d983 149 int_lidar_msg.sensor_forward = sensor_forward.read();
florine_van 8:57aa8a35d983 150 int_lidar_msg.sensor_right = sensor_right.read();
florine_van 8:57aa8a35d983 151 int_lidar_msg.sensor_back = sensor_back.read();
florine_van 8:57aa8a35d983 152 int_lidar_msg.sensor_left = sensor_left.read();
florine_van 0:57855aafa907 153
florine_van 8:57aa8a35d983 154 lidar_pub.publish(&int_lidar_msg);
florine_van 10:be9de79cf6b0 155
florine_van 8:57aa8a35d983 156 /*
florine_van 10:be9de79cf6b0 157 int_msg.data = sensor_forward.read();
florine_van 10:be9de79cf6b0 158 lidar_pub.publish(&int_msg);
florine_van 10:be9de79cf6b0 159 */
florine_van 10:be9de79cf6b0 160
florine_van 10:be9de79cf6b0 161 /*
florine_van 10:be9de79cf6b0 162 int range;
florine_van 10:be9de79cf6b0 163 range = sensor_forward.read();
florine_van 10:be9de79cf6b0 164 pc.printf("Range = %d\r\n", range);
florine_van 8:57aa8a35d983 165 */
florine_van 8:57aa8a35d983 166
florine_van 7:2cf57f28255d 167 nh.spinOnce();
florine_van 5:8ef79eebbc97 168
florine_van 4:cb50c6fa340b 169 wait_ms(10);
florine_van 0:57855aafa907 170 }
florine_van 0:57855aafa907 171 }
florine_van 0:57855aafa907 172
florine_van 5:8ef79eebbc97 173
florine_van 5:8ef79eebbc97 174