Florine Vancappel / Mbed 2 deprecated Robot_team1_QEI_Douglas

Old working code

Dependencies:   mbed QEI ros_lib_melodic

main.cpp@11:35809512ec11, 2019-11-13 (annotated)

Committer:
florine_van
Date:
Wed Nov 13 15:59:06 2019 +0000
Revision:
11:35809512ec11
Parent:
10:be9de79cf6b0
Child:
12:0fa4c5a86c75
Implement QEI and distance moving

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 11:35809512ec11 36 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 37 QEI wheel_B (PB_5, PB_3, NC, 3576, QEI::X4_ENCODING); //counts per rev = 12 * 298:1, use quadrature encoding
florine_van 11:35809512ec11 38 Motor motor_left(PC_6, PB_15, PB_13, wheel_A);
florine_van 11:35809512ec11 39 Motor motor_right(PA_15, PC_7, PB_4, wheel_B);
florine_van 5:8ef79eebbc97 40
florine_van 10:be9de79cf6b0 41 void controlMotor(const std_msgs::Int32& motor_dir)
florine_van 10:be9de79cf6b0 42 {
florine_van 11:35809512ec11 43 switch(motor_dir.data) {
florine_van 11:35809512ec11 44 // Stop motor
florine_van 11:35809512ec11 45 case 0:
florine_van 11:35809512ec11 46 motor_left.moveForward(0);
florine_van 11:35809512ec11 47 motor_right.moveForward(0);
florine_van 11:35809512ec11 48 break;
florine_van 11:35809512ec11 49
florine_van 10:be9de79cf6b0 50 // Move forward
florine_van 10:be9de79cf6b0 51 case 1:
florine_van 11:35809512ec11 52 motor_left.moveForward(5691);
florine_van 11:35809512ec11 53 motor_right.moveForward(5691);
florine_van 10:be9de79cf6b0 54 break;
florine_van 10:be9de79cf6b0 55
florine_van 10:be9de79cf6b0 56 // Move left
florine_van 10:be9de79cf6b0 57 case 2:
florine_van 11:35809512ec11 58 motor_left.moveLeft(0.5);
florine_van 11:35809512ec11 59 motor_right.moveLeft(-0.5);
florine_van 10:be9de79cf6b0 60 break;
florine_van 10:be9de79cf6b0 61
florine_van 10:be9de79cf6b0 62 // Move right
florine_van 10:be9de79cf6b0 63 case 3:
florine_van 11:35809512ec11 64 motor_left.moveRight(-0.5);
florine_van 11:35809512ec11 65 motor_right.moveBackward(0.5);
florine_van 10:be9de79cf6b0 66 break;
florine_van 10:be9de79cf6b0 67 }
florine_van 10:be9de79cf6b0 68 }
florine_van 10:be9de79cf6b0 69
florine_van 10:be9de79cf6b0 70 // ROS subscriber for motors control
florine_van 10:be9de79cf6b0 71 ros::Subscriber<std_msgs::Int32> motor_sub("motor_control", &controlMotor);
florine_van 11:35809512ec11 72
florine_van 7:2cf57f28255d 73
florine_van 5:8ef79eebbc97 74 int main()
florine_van 5:8ef79eebbc97 75 {
florine_van 7:2cf57f28255d 76 ros::NodeHandle nh;
florine_van 7:2cf57f28255d 77 nh.initNode();
florine_van 8:57aa8a35d983 78
florine_van 10:be9de79cf6b0 79 // ROS publisher for sensor readings
florine_van 8:57aa8a35d983 80 mbed_custom_msgs::lidar_msg int_lidar_msg;
florine_van 8:57aa8a35d983 81 ros::Publisher lidar_pub("lidar_reading", &int_lidar_msg);
florine_van 11:35809512ec11 82
florine_van 8:57aa8a35d983 83 nh.advertise(lidar_pub);
florine_van 10:be9de79cf6b0 84 nh.subscribe(motor_sub);
florine_van 3:a3144a45f44c 85
florine_van 3:a3144a45f44c 86 // load settings onto VL6180X sensors
florine_van 6:858a5116688e 87 sensor_forward.init();
florine_van 3:a3144a45f44c 88 // change default address of sensor 2
florine_van 6:858a5116688e 89 sensor_forward.changeAddress(addr2);
florine_van 2:c537f1ebad7b 90
florine_van 6:858a5116688e 91 sensor_right.init();
florine_van 3:a3144a45f44c 92 // change default address of sensor 3
florine_van 6:858a5116688e 93 sensor_right.changeAddress(addr3);
florine_van 2:c537f1ebad7b 94
florine_van 6:858a5116688e 95 sensor_back.init();
florine_van 3:a3144a45f44c 96 // change default address of sensor 4
florine_van 6:858a5116688e 97 sensor_back.changeAddress(addr4);
florine_van 6:858a5116688e 98
florine_van 6:858a5116688e 99 sensor_left.init();
florine_van 2:c537f1ebad7b 100
florine_van 6:858a5116688e 101 //Set Speeds
florine_van 6:858a5116688e 102 motor_left.setSpeed(0.5f);
florine_van 6:858a5116688e 103 motor_right.setSpeed(0.5f);
florine_van 0:57855aafa907 104
florine_van 0:57855aafa907 105 while (1)
florine_van 10:be9de79cf6b0 106 {
florine_van 8:57aa8a35d983 107 int_lidar_msg.sensor_forward = sensor_forward.read();
florine_van 8:57aa8a35d983 108 int_lidar_msg.sensor_right = sensor_right.read();
florine_van 8:57aa8a35d983 109 int_lidar_msg.sensor_back = sensor_back.read();
florine_van 8:57aa8a35d983 110 int_lidar_msg.sensor_left = sensor_left.read();
florine_van 0:57855aafa907 111
florine_van 8:57aa8a35d983 112 lidar_pub.publish(&int_lidar_msg);
florine_van 10:be9de79cf6b0 113
florine_van 10:be9de79cf6b0 114 /*
florine_van 10:be9de79cf6b0 115 int range;
florine_van 10:be9de79cf6b0 116 range = sensor_forward.read();
florine_van 10:be9de79cf6b0 117 pc.printf("Range = %d\r\n", range);
florine_van 8:57aa8a35d983 118 */
florine_van 8:57aa8a35d983 119
florine_van 7:2cf57f28255d 120 nh.spinOnce();
florine_van 5:8ef79eebbc97 121
florine_van 4:cb50c6fa340b 122 wait_ms(10);
florine_van 0:57855aafa907 123 }
florine_van 0:57855aafa907 124 }
florine_van 0:57855aafa907 125
florine_van 5:8ef79eebbc97 126
florine_van 5:8ef79eebbc97 127