Soft robot team / Mbed OS Robot_team1

Robot team project

Dependencies:   QEI Motordriver ros_lib_melodic

main.cpp@1:d5adc483bce0, 2019-10-22 (annotated)

Committer:
florine_van
Date:
Tue Oct 22 09:41:35 2019 +0000
Revision:
1:d5adc483bce0
Parent:
0:57855aafa907
Child:
2:c537f1ebad7b
Not working code for two sensors

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 1:d5adc483bce0 23 int range2;
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 1:d5adc483bce0 28 Sensor sensor1(I2C_SDA, I2C_SCL, PC_9);
florine_van 1:d5adc483bce0 29 Sensor sensor2(I2C_SDA, I2C_SCL, PC_11);
florine_van 1:d5adc483bce0 30 //Sensor sensor3(I2C_SDA, I2C_SCL, PD_2);
florine_van 1:d5adc483bce0 31 //Sensor sensor4(I2C_SDA, I2C_SCL, PG_3);
florine_van 0:57855aafa907 32
florine_van 0:57855aafa907 33 /*
florine_van 0:57855aafa907 34 SHDN_1 = 0;
florine_van 0:57855aafa907 35 SHDN_3 = 0;
florine_van 0:57855aafa907 36 SHDN_2 = 0;
florine_van 0:57855aafa907 37 SHDN_4 = 0;
florine_van 0:57855aafa907 38 wait_ms(0.5);
florine_van 1:d5adc483bce0 39 */
florine_van 1:d5adc483bce0 40 sensor1.turnOff();
florine_van 1:d5adc483bce0 41 sensor2.turnOff();
florine_van 1:d5adc483bce0 42 // sensor3.turnOff();
florine_van 1:d5adc483bce0 43 // sensor4.turnOff();
florine_van 1:d5adc483bce0 44 wait_ms(0.5);
florine_van 0:57855aafa907 45
florine_van 1:d5adc483bce0 46 sensor1.turnOff();
florine_van 1:d5adc483bce0 47 sensor2.turnOn();
florine_van 1:d5adc483bce0 48 // sensor3.turnOff();
florine_van 1:d5adc483bce0 49 //sensor4.turnOff();
florine_van 1:d5adc483bce0 50
florine_van 1:d5adc483bce0 51 /*
florine_van 0:57855aafa907 52 SHDN_1 = 0;
florine_van 0:57855aafa907 53 SHDN_2 = 1;
florine_van 0:57855aafa907 54 SHDN_3 = 0;
florine_van 0:57855aafa907 55 SHDN_4 = 0;
florine_van 0:57855aafa907 56 */
florine_van 0:57855aafa907 57
florine_van 1:d5adc483bce0 58 sensor2.init();
florine_van 1:d5adc483bce0 59 sensor2.changeAddress(ADDR2);
florine_van 1:d5adc483bce0 60 /*
florine_van 1:d5adc483bce0 61 sensor3.turnOn();
florine_van 1:d5adc483bce0 62 sensor3.init();
florine_van 1:d5adc483bce0 63 sensor3.changeAddress(ADDR3);
florine_van 1:d5adc483bce0 64
florine_van 1:d5adc483bce0 65 sensor4.turnOn();
florine_van 1:d5adc483bce0 66 sensor4.init();
florine_van 1:d5adc483bce0 67 sensor4.changeAddress(ADDR4);
florine_van 1:d5adc483bce0 68 */
florine_van 1:d5adc483bce0 69 sensor1.turnOn();
florine_van 1:d5adc483bce0 70 sensor1.init();
florine_van 0:57855aafa907 71 /*
florine_van 0:57855aafa907 72 // load settings onto VL6180X sensors
florine_van 0:57855aafa907 73 VL6180_Init(ADDR1);
florine_van 0:57855aafa907 74 // change default address of sensor 2
florine_van 0:57855aafa907 75 WriteByte(0x212, addr2, ADDR1);
florine_van 0:57855aafa907 76
florine_van 0:57855aafa907 77 SHDN_3 = 1;
florine_van 0:57855aafa907 78 VL6180_Init(ADDR1);
florine_van 0:57855aafa907 79 // change default address of sensor 3
florine_van 0:57855aafa907 80 WriteByte(0x212, addr3, ADDR1);
florine_van 0:57855aafa907 81
florine_van 0:57855aafa907 82 SHDN_4 = 1;
florine_van 0:57855aafa907 83 VL6180_Init(ADDR1);
florine_van 0:57855aafa907 84 // change default address of sensor 4
florine_van 0:57855aafa907 85 WriteByte(0x212, addr4, ADDR1);
florine_van 0:57855aafa907 86
florine_van 0:57855aafa907 87 SHDN_1 = 1;
florine_van 0:57855aafa907 88 VL6180_Init(ADDR1);
florine_van 0:57855aafa907 89 */
florine_van 0:57855aafa907 90
florine_van 0:57855aafa907 91 while (1)
florine_van 0:57855aafa907 92 {
florine_van 0:57855aafa907 93 range1 = sensor1.read();
florine_van 0:57855aafa907 94 range2 = sensor2.read();
florine_van 1:d5adc483bce0 95 //range3 = sensor3.read();
florine_van 1:d5adc483bce0 96 //range4 = sensor4.read();
florine_van 0:57855aafa907 97
florine_van 0:57855aafa907 98 pc.printf("Range one = %d | range two = %d | range three = %d | range four = %d\r\n",range1, range2, range3, range4);
florine_van 0:57855aafa907 99 wait_ms(100);
florine_van 0:57855aafa907 100 }
florine_van 0:57855aafa907 101 }
florine_van 0:57855aafa907 102