Soft robot team / Mbed OS Robot_team1

Robot team project

Dependencies:   QEI Motordriver ros_lib_melodic

main.cpp@2:c537f1ebad7b, 2019-10-22 (annotated)

Committer:
florine_van
Date:
Tue Oct 22 10:05:12 2019 +0000
Revision:
2:c537f1ebad7b
Parent:
1:d5adc483bce0
Child:
3:a3144a45f44c
Working code for 4 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 int main() {
florine_van 0:57855aafa907 14 int range1;
florine_van 1:d5adc483bce0 15 int range2;
florine_van 2:c537f1ebad7b 16 int range3;
florine_van 2:c537f1ebad7b 17 int range4;
florine_van 0:57855aafa907 18
florine_van 2:c537f1ebad7b 19 // Create sensors
florine_van 1:d5adc483bce0 20 Sensor sensor1(I2C_SDA, I2C_SCL, PC_9);
florine_van 1:d5adc483bce0 21 Sensor sensor2(I2C_SDA, I2C_SCL, PC_11);
florine_van 2:c537f1ebad7b 22 Sensor sensor3(I2C_SDA, I2C_SCL, PD_2);
florine_van 2:c537f1ebad7b 23 Sensor sensor4(I2C_SDA, I2C_SCL, PG_3);
florine_van 0:57855aafa907 24
florine_van 1:d5adc483bce0 25 sensor1.turnOff();
florine_van 1:d5adc483bce0 26 sensor2.turnOff();
florine_van 2:c537f1ebad7b 27 sensor3.turnOff();
florine_van 2:c537f1ebad7b 28 sensor4.turnOff();
florine_van 1:d5adc483bce0 29 wait_ms(0.5);
florine_van 0:57855aafa907 30
florine_van 1:d5adc483bce0 31 sensor1.turnOff();
florine_van 1:d5adc483bce0 32 sensor2.turnOn();
florine_van 2:c537f1ebad7b 33 sensor3.turnOff();
florine_van 2:c537f1ebad7b 34 sensor4.turnOff();
florine_van 0:57855aafa907 35
florine_van 1:d5adc483bce0 36 sensor2.init();
florine_van 2:c537f1ebad7b 37 sensor2.changeAddress(addr2);
florine_van 2:c537f1ebad7b 38
florine_van 1:d5adc483bce0 39 sensor3.turnOn();
florine_van 1:d5adc483bce0 40 sensor3.init();
florine_van 2:c537f1ebad7b 41 sensor3.changeAddress(addr3);
florine_van 2:c537f1ebad7b 42
florine_van 1:d5adc483bce0 43 sensor4.turnOn();
florine_van 1:d5adc483bce0 44 sensor4.init();
florine_van 2:c537f1ebad7b 45 sensor4.changeAddress(addr4);
florine_van 2:c537f1ebad7b 46
florine_van 1:d5adc483bce0 47 sensor1.turnOn();
florine_van 1:d5adc483bce0 48 sensor1.init();
florine_van 0:57855aafa907 49 /*
florine_van 0:57855aafa907 50 // load settings onto VL6180X sensors
florine_van 0:57855aafa907 51 VL6180_Init(ADDR1);
florine_van 0:57855aafa907 52 // change default address of sensor 2
florine_van 0:57855aafa907 53 WriteByte(0x212, addr2, ADDR1);
florine_van 0:57855aafa907 54
florine_van 0:57855aafa907 55 SHDN_3 = 1;
florine_van 0:57855aafa907 56 VL6180_Init(ADDR1);
florine_van 0:57855aafa907 57 // change default address of sensor 3
florine_van 0:57855aafa907 58 WriteByte(0x212, addr3, ADDR1);
florine_van 0:57855aafa907 59
florine_van 0:57855aafa907 60 SHDN_4 = 1;
florine_van 0:57855aafa907 61 VL6180_Init(ADDR1);
florine_van 0:57855aafa907 62 // change default address of sensor 4
florine_van 0:57855aafa907 63 WriteByte(0x212, addr4, ADDR1);
florine_van 0:57855aafa907 64
florine_van 0:57855aafa907 65 SHDN_1 = 1;
florine_van 0:57855aafa907 66 VL6180_Init(ADDR1);
florine_van 0:57855aafa907 67 */
florine_van 0:57855aafa907 68
florine_van 0:57855aafa907 69 while (1)
florine_van 0:57855aafa907 70 {
florine_van 0:57855aafa907 71 range1 = sensor1.read();
florine_van 0:57855aafa907 72 range2 = sensor2.read();
florine_van 2:c537f1ebad7b 73 range3 = sensor3.read();
florine_van 2:c537f1ebad7b 74 range4 = sensor4.read();
florine_van 0:57855aafa907 75
florine_van 0:57855aafa907 76 pc.printf("Range one = %d | range two = %d | range three = %d | range four = %d\r\n",range1, range2, range3, range4);
florine_van 0:57855aafa907 77 wait_ms(100);
florine_van 0:57855aafa907 78 }
florine_van 0:57855aafa907 79 }
florine_van 0:57855aafa907 80