Soft robot team / Mbed OS Robot_team1

Robot team project

Dependencies:   QEI Motordriver ros_lib_melodic

main.cpp@0:57855aafa907, 2019-10-22 (annotated)

Committer:
florine_van
Date:
Tue Oct 22 09:12:03 2019 +0000
Revision:
0:57855aafa907
Child:
1:d5adc483bce0
Implementation of one sensor;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
florine_van 0:57855aafa907 1 #include "mbed.h"
florine_van 0:57855aafa907 2 #include "Sensor.h"
florine_van 0:57855aafa907 3
florine_van 0:57855aafa907 4 // Set up serial to pc
florine_van 0:57855aafa907 5 Serial pc(SERIAL_TX, SERIAL_RX);
florine_van 0:57855aafa907 6
florine_van 0:57855aafa907 7 // Set up I²C on the STM32 NUCLEO-401RE
florine_van 0:57855aafa907 8 #define addr1 (0x29)
florine_van 0:57855aafa907 9 #define addr2 (0x2A)
florine_van 0:57855aafa907 10 #define addr3 (0x2B)
florine_van 0:57855aafa907 11 #define addr4 (0x2C)
florine_van 0:57855aafa907 12
florine_van 0:57855aafa907 13 //Macro for addresses
florine_van 0:57855aafa907 14 // I²C address of VL6180 shifted by 1 bit
florine_van 0:57855aafa907 15 //(0x29 << 1) so the R/W command can be added
florine_van 0:57855aafa907 16 #define ADDR1 (addr1<<1)
florine_van 0:57855aafa907 17 #define ADDR2 (addr2<<1)
florine_van 0:57855aafa907 18 #define ADDR3 (addr3<<1)
florine_van 0:57855aafa907 19 #define ADDR4 (addr4<<1)
florine_van 0:57855aafa907 20
florine_van 0:57855aafa907 21 int main() {
florine_van 0:57855aafa907 22 int range1;
florine_van 0:57855aafa907 23 int range2=0;
florine_van 0:57855aafa907 24 int range3=0;
florine_van 0:57855aafa907 25 int range4=0;
florine_van 0:57855aafa907 26
florine_van 0:57855aafa907 27 // Create and init sensor 1
florine_van 0:57855aafa907 28 Sensor sensor1(ADDR1, I2C_SDA, I2C_SCL, PC_9);
florine_van 0:57855aafa907 29
florine_van 0:57855aafa907 30 /*
florine_van 0:57855aafa907 31 Sensor sensor2(ADDR2, I2C_SDA, I2C_SCL, PC_11);
florine_van 0:57855aafa907 32 Sensor sensor3(ADDR3, I2C_SDA, I2C_SCL, PD_2);
florine_van 0:57855aafa907 33 Sensor sensor4(ADDR4, I2C_SDA, I2C_SCL, PG_3);
florine_van 0:57855aafa907 34 */
florine_van 0:57855aafa907 35
florine_van 0:57855aafa907 36 /*
florine_van 0:57855aafa907 37 SHDN_1 = 0;
florine_van 0:57855aafa907 38 SHDN_3 = 0;
florine_van 0:57855aafa907 39 SHDN_2 = 0;
florine_van 0:57855aafa907 40 SHDN_4 = 0;
florine_van 0:57855aafa907 41 wait_ms(0.5);
florine_van 0:57855aafa907 42
florine_van 0:57855aafa907 43 SHDN_1 = 0;
florine_van 0:57855aafa907 44 SHDN_2 = 1;
florine_van 0:57855aafa907 45 SHDN_3 = 0;
florine_van 0:57855aafa907 46 SHDN_4 = 0;
florine_van 0:57855aafa907 47 */
florine_van 0:57855aafa907 48
florine_van 0:57855aafa907 49
florine_van 0:57855aafa907 50 /*
florine_van 0:57855aafa907 51 // load settings onto VL6180X sensors
florine_van 0:57855aafa907 52 VL6180_Init(ADDR1);
florine_van 0:57855aafa907 53 // change default address of sensor 2
florine_van 0:57855aafa907 54 WriteByte(0x212, addr2, ADDR1);
florine_van 0:57855aafa907 55
florine_van 0:57855aafa907 56 SHDN_3 = 1;
florine_van 0:57855aafa907 57 VL6180_Init(ADDR1);
florine_van 0:57855aafa907 58 // change default address of sensor 3
florine_van 0:57855aafa907 59 WriteByte(0x212, addr3, ADDR1);
florine_van 0:57855aafa907 60
florine_van 0:57855aafa907 61 SHDN_4 = 1;
florine_van 0:57855aafa907 62 VL6180_Init(ADDR1);
florine_van 0:57855aafa907 63 // change default address of sensor 4
florine_van 0:57855aafa907 64 WriteByte(0x212, addr4, ADDR1);
florine_van 0:57855aafa907 65
florine_van 0:57855aafa907 66 SHDN_1 = 1;
florine_van 0:57855aafa907 67 VL6180_Init(ADDR1);
florine_van 0:57855aafa907 68 */
florine_van 0:57855aafa907 69
florine_van 0:57855aafa907 70 while (1)
florine_van 0:57855aafa907 71 {
florine_van 0:57855aafa907 72 range1 = sensor1.read();
florine_van 0:57855aafa907 73
florine_van 0:57855aafa907 74 /*
florine_van 0:57855aafa907 75 range2 = sensor2.read();
florine_van 0:57855aafa907 76 range3 = sensor3.read();
florine_van 0:57855aafa907 77 range4 = sensor4.read();
florine_van 0:57855aafa907 78 */
florine_van 0:57855aafa907 79
florine_van 0:57855aafa907 80 pc.printf("Range one = %d | range two = %d | range three = %d | range four = %d\r\n",range1, range2, range3, range4);
florine_van 0:57855aafa907 81 wait_ms(100);
florine_van 0:57855aafa907 82 }
florine_van 0:57855aafa907 83 }
florine_van 0:57855aafa907 84