Soft robot team / Mbed OS Robot_team1

Robot team project

Dependencies:   QEI Motordriver ros_lib_melodic

main.cpp@8:57aa8a35d983, 2019-10-30 (annotated)

Committer:
florine_van
Date:
Wed Oct 30 13:12:25 2019 +0000
Revision:
8:57aa8a35d983
Parent:
7:2cf57f28255d
Child:
10:be9de79cf6b0
Add custom message

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 8:57aa8a35d983 5 #include <mbed_custom_msgs/lidar_msg.h>
florine_van 5:8ef79eebbc97 6
florine_van 0:57855aafa907 7 #include "Sensor.h"
florine_van 5:8ef79eebbc97 8 #include "Motor.h"
florine_van 0:57855aafa907 9
florine_van 0:57855aafa907 10 // Set up serial to pc
florine_van 7:2cf57f28255d 11 //Serial pc(SERIAL_TX, SERIAL_RX);
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 6:858a5116688e 35 Motor motor_left(PC_6, PB_15, PB_13);
florine_van 6:858a5116688e 36 Motor motor_right(PA_15, PC_7, PB_4);
florine_van 7:2cf57f28255d 37
florine_van 8:57aa8a35d983 38 /*
florine_van 7:2cf57f28255d 39 void avoidObstacle(const std_msgs::Empty& avoid_obstacle_msg)
florine_van 6:858a5116688e 40 {
florine_van 5:8ef79eebbc97 41 // When obstacle ahead
florine_van 6:858a5116688e 42 if (sensor_forward.getIsObstacle())
florine_van 5:8ef79eebbc97 43 {
florine_van 6:858a5116688e 44 if ( (sensor_right.getIsObstacle()) && (sensor_left.getIsObstacle()) )
florine_van 6:858a5116688e 45 {
florine_van 6:858a5116688e 46 //Turn backward
florine_van 6:858a5116688e 47 while(!sensor_back.getIsObstacle())
florine_van 6:858a5116688e 48 {
florine_van 6:858a5116688e 49 motor_left.moveForward();
florine_van 6:858a5116688e 50 motor_right.moveBackward();
florine_van 6:858a5116688e 51 }
florine_van 6:858a5116688e 52 }
florine_van 6:858a5116688e 53 if (sensor_left.getIsObstacle())
florine_van 6:858a5116688e 54 {
florine_van 6:858a5116688e 55 //Turn to the right
florine_van 6:858a5116688e 56 motor_left.moveForward();
florine_van 6:858a5116688e 57 motor_right.moveBackward();
florine_van 6:858a5116688e 58 }
florine_van 6:858a5116688e 59 else
florine_van 6:858a5116688e 60 {
florine_van 6:858a5116688e 61 // By default : turn to the left
florine_van 6:858a5116688e 62 motor_left.moveBackward();
florine_van 6:858a5116688e 63 motor_right.moveForward();
florine_van 6:858a5116688e 64 }
florine_van 5:8ef79eebbc97 65 }
florine_van 5:8ef79eebbc97 66 // No obstacle
florine_van 5:8ef79eebbc97 67 else
florine_van 5:8ef79eebbc97 68 {
florine_van 6:858a5116688e 69 motor_left.moveForward();
florine_van 6:858a5116688e 70 motor_right.moveForward();
florine_van 5:8ef79eebbc97 71 }
florine_van 5:8ef79eebbc97 72 }
florine_van 8:57aa8a35d983 73 */
florine_van 5:8ef79eebbc97 74
florine_van 8:57aa8a35d983 75 //ros::Subscriber<std_msgs::Empty> avoid_obstacle_sub("avoid_obstacle", &avoidObstacle);
florine_van 7:2cf57f28255d 76
florine_van 5:8ef79eebbc97 77 int main()
florine_van 5:8ef79eebbc97 78 {
florine_van 7:2cf57f28255d 79 ros::NodeHandle nh;
florine_van 7:2cf57f28255d 80 nh.initNode();
florine_van 8:57aa8a35d983 81
florine_van 8:57aa8a35d983 82 mbed_custom_msgs::lidar_msg int_lidar_msg;
florine_van 8:57aa8a35d983 83 ros::Publisher lidar_pub("lidar_reading", &int_lidar_msg);
florine_van 8:57aa8a35d983 84 nh.advertise(lidar_pub);
florine_van 7:2cf57f28255d 85
florine_van 8:57aa8a35d983 86 /*
florine_van 7:2cf57f28255d 87 std_msgs::Int32 int1_sensor_msg;
florine_van 7:2cf57f28255d 88 ros::Publisher range_forward_pub("sensor_forward", &int1_sensor_msg);
florine_van 7:2cf57f28255d 89 nh.advertise(range_forward_pub);
florine_van 7:2cf57f28255d 90
florine_van 7:2cf57f28255d 91 std_msgs::Int32 int2_sensor_msg;
florine_van 7:2cf57f28255d 92 ros::Publisher range_back_pub("sensor_back", &int2_sensor_msg);
florine_van 7:2cf57f28255d 93 nh.advertise(range_forward_pub);
florine_van 7:2cf57f28255d 94
florine_van 7:2cf57f28255d 95 std_msgs::Int32 int3_sensor_msg;
florine_van 7:2cf57f28255d 96 ros::Publisher range_left_pub("sensor_left", &int3_sensor_msg);
florine_van 7:2cf57f28255d 97 nh.advertise(range_forward_pub);
florine_van 7:2cf57f28255d 98
florine_van 7:2cf57f28255d 99 std_msgs::Int32 int4_sensor_msg;
florine_van 7:2cf57f28255d 100 ros::Publisher range_right_pub("sensor_right", &int4_sensor_msg);
florine_van 7:2cf57f28255d 101 nh.advertise(range_forward_pub);
florine_van 7:2cf57f28255d 102
florine_van 7:2cf57f28255d 103 nh.subscribe(avoid_obstacle_sub);
florine_van 7:2cf57f28255d 104
florine_van 0:57855aafa907 105 int range1;
florine_van 1:d5adc483bce0 106 int range2;
florine_van 2:c537f1ebad7b 107 int range3;
florine_van 2:c537f1ebad7b 108 int range4;
florine_van 8:57aa8a35d983 109 */
florine_van 3:a3144a45f44c 110
florine_van 3:a3144a45f44c 111 // load settings onto VL6180X sensors
florine_van 6:858a5116688e 112 sensor_forward.init();
florine_van 3:a3144a45f44c 113 // change default address of sensor 2
florine_van 6:858a5116688e 114 sensor_forward.changeAddress(addr2);
florine_van 2:c537f1ebad7b 115
florine_van 6:858a5116688e 116 sensor_right.init();
florine_van 3:a3144a45f44c 117 // change default address of sensor 3
florine_van 6:858a5116688e 118 sensor_right.changeAddress(addr3);
florine_van 2:c537f1ebad7b 119
florine_van 6:858a5116688e 120 sensor_back.init();
florine_van 3:a3144a45f44c 121 // change default address of sensor 4
florine_van 6:858a5116688e 122 sensor_back.changeAddress(addr4);
florine_van 6:858a5116688e 123
florine_van 6:858a5116688e 124 sensor_left.init();
florine_van 2:c537f1ebad7b 125
florine_van 6:858a5116688e 126 //Set Speeds
florine_van 6:858a5116688e 127 motor_left.setSpeed(0.5f);
florine_van 6:858a5116688e 128 motor_right.setSpeed(0.5f);
florine_van 0:57855aafa907 129
florine_van 0:57855aafa907 130 while (1)
florine_van 0:57855aafa907 131 {
florine_van 8:57aa8a35d983 132 int_lidar_msg.sensor_forward = sensor_forward.read();
florine_van 8:57aa8a35d983 133 int_lidar_msg.sensor_right = sensor_right.read();
florine_van 8:57aa8a35d983 134 int_lidar_msg.sensor_back = sensor_back.read();
florine_van 8:57aa8a35d983 135 int_lidar_msg.sensor_left = sensor_left.read();
florine_van 0:57855aafa907 136
florine_van 8:57aa8a35d983 137 lidar_pub.publish(&int_lidar_msg);
florine_van 7:2cf57f28255d 138 //pc.printf("Range forward = %d | range back = %d | range right = %d | range left = %d\r\n",range1, range3, range2, range4);
florine_van 8:57aa8a35d983 139
florine_van 8:57aa8a35d983 140 /*
florine_van 7:2cf57f28255d 141 int1_sensor_msg.data = range1;
florine_van 7:2cf57f28255d 142 int2_sensor_msg.data = range3;
florine_van 7:2cf57f28255d 143 int3_sensor_msg.data = range4;
florine_van 7:2cf57f28255d 144 int4_sensor_msg.data = range2;
florine_van 7:2cf57f28255d 145 range_forward_pub.publish(&int1_sensor_msg);
florine_van 7:2cf57f28255d 146 range_back_pub.publish(&int2_sensor_msg);
florine_van 7:2cf57f28255d 147 range_left_pub.publish(&int3_sensor_msg);
florine_van 7:2cf57f28255d 148 range_right_pub.publish(&int4_sensor_msg);
florine_van 8:57aa8a35d983 149 */
florine_van 8:57aa8a35d983 150
florine_van 7:2cf57f28255d 151 nh.spinOnce();
florine_van 5:8ef79eebbc97 152
florine_van 7:2cf57f28255d 153 //avoidObstacle();
florine_van 5:8ef79eebbc97 154
florine_van 4:cb50c6fa340b 155 wait_ms(10);
florine_van 0:57855aafa907 156 }
florine_van 0:57855aafa907 157 }
florine_van 0:57855aafa907 158
florine_van 5:8ef79eebbc97 159
florine_van 5:8ef79eebbc97 160