Soft robot team / Mbed OS Robot_team1

Robot team project

Dependencies:   QEI Motordriver ros_lib_melodic

main.cpp@5:8ef79eebbc97, 2019-10-22 (annotated)

Committer:
florine_van
Date:
Tue Oct 22 15:06:44 2019 +0000
Revision:
5:8ef79eebbc97
Parent:
4:cb50c6fa340b
Child:
6:858a5116688e
Implementation of motor class; Link to sensors in main method

Who changed what in which revision?

UserRevisionLine numberNew contents of line
florine_van 0:57855aafa907 1 #include "mbed.h"
florine_van 5:8ef79eebbc97 2
florine_van 0:57855aafa907 3 #include "Sensor.h"
florine_van 5:8ef79eebbc97 4 #include "Motor.h"
florine_van 0:57855aafa907 5
florine_van 0:57855aafa907 6 // Set up serial to pc
florine_van 0:57855aafa907 7 Serial pc(SERIAL_TX, SERIAL_RX);
florine_van 0:57855aafa907 8
florine_van 0:57855aafa907 9 // Set up I²C on the STM32 NUCLEO-401RE
florine_van 0:57855aafa907 10 #define addr1 (0x29)
florine_van 0:57855aafa907 11 #define addr2 (0x2A)
florine_van 0:57855aafa907 12 #define addr3 (0x2B)
florine_van 0:57855aafa907 13 #define addr4 (0x2C)
florine_van 0:57855aafa907 14
florine_van 5:8ef79eebbc97 15 // VL6180x sensors
florine_van 5:8ef79eebbc97 16 Sensor sensor1(I2C_SDA, I2C_SCL, PC_9);
florine_van 5:8ef79eebbc97 17 Sensor sensor2(I2C_SDA, I2C_SCL, PC_11);
florine_van 5:8ef79eebbc97 18 Sensor sensor3(I2C_SDA, I2C_SCL, PD_2);
florine_van 5:8ef79eebbc97 19 Sensor sensor4(I2C_SDA, I2C_SCL, PG_3);
florine_van 5:8ef79eebbc97 20
florine_van 5:8ef79eebbc97 21 // Motors
florine_van 5:8ef79eebbc97 22 Motor motor1(PC_6, PB_15, PB_13);
florine_van 5:8ef79eebbc97 23 Motor motor2(PA_15, PC_7, PB_4);
florine_van 5:8ef79eebbc97 24
florine_van 5:8ef79eebbc97 25 void CheckObstacle()
florine_van 5:8ef79eebbc97 26 {
florine_van 5:8ef79eebbc97 27 // BY DEFAULT, WHEN OBSTACLE, TURN TO THE RIGHT
florine_van 5:8ef79eebbc97 28
florine_van 5:8ef79eebbc97 29 // When obstacle ahead
florine_van 5:8ef79eebbc97 30 if ( (sensor1.getIsObstacle()))
florine_van 5:8ef79eebbc97 31 {
florine_van 5:8ef79eebbc97 32 //Turn to the right
florine_van 5:8ef79eebbc97 33 motor1.turnRight();
florine_van 5:8ef79eebbc97 34 motor2.turnRight();
florine_van 5:8ef79eebbc97 35 }
florine_van 5:8ef79eebbc97 36 // When obstacle ahead and to the right
florine_van 5:8ef79eebbc97 37 else if ( (sensor1.getIsObstacle()) && (sensor2.getIsObstacle()) )
florine_van 5:8ef79eebbc97 38 {
florine_van 5:8ef79eebbc97 39 //Turn to the left
florine_van 5:8ef79eebbc97 40 motor1.turnLeft();
florine_van 5:8ef79eebbc97 41 motor2.turnLeft();
florine_van 5:8ef79eebbc97 42 }
florine_van 5:8ef79eebbc97 43 // No obstacle
florine_van 5:8ef79eebbc97 44 else
florine_van 5:8ef79eebbc97 45 {
florine_van 5:8ef79eebbc97 46 motor1.moveForward();
florine_van 5:8ef79eebbc97 47 motor2.moveForward();
florine_van 5:8ef79eebbc97 48 }
florine_van 5:8ef79eebbc97 49 }
florine_van 5:8ef79eebbc97 50
florine_van 5:8ef79eebbc97 51 int main()
florine_van 5:8ef79eebbc97 52 {
florine_van 0:57855aafa907 53 int range1;
florine_van 1:d5adc483bce0 54 int range2;
florine_van 2:c537f1ebad7b 55 int range3;
florine_van 2:c537f1ebad7b 56 int range4;
florine_van 3:a3144a45f44c 57
florine_van 3:a3144a45f44c 58 // load settings onto VL6180X sensors
florine_van 1:d5adc483bce0 59 sensor2.init();
florine_van 3:a3144a45f44c 60 // change default address of sensor 2
florine_van 2:c537f1ebad7b 61 sensor2.changeAddress(addr2);
florine_van 2:c537f1ebad7b 62
florine_van 1:d5adc483bce0 63 sensor3.init();
florine_van 3:a3144a45f44c 64 // change default address of sensor 3
florine_van 2:c537f1ebad7b 65 sensor3.changeAddress(addr3);
florine_van 2:c537f1ebad7b 66
florine_van 1:d5adc483bce0 67 sensor4.init();
florine_van 3:a3144a45f44c 68 // change default address of sensor 4
florine_van 2:c537f1ebad7b 69 sensor4.changeAddress(addr4);
florine_van 2:c537f1ebad7b 70
florine_van 1:d5adc483bce0 71 sensor1.init();
florine_van 0:57855aafa907 72
florine_van 0:57855aafa907 73 while (1)
florine_van 0:57855aafa907 74 {
florine_van 0:57855aafa907 75 range1 = sensor1.read();
florine_van 0:57855aafa907 76 range2 = sensor2.read();
florine_van 2:c537f1ebad7b 77 range3 = sensor3.read();
florine_van 2:c537f1ebad7b 78 range4 = sensor4.read();
florine_van 0:57855aafa907 79
florine_van 5:8ef79eebbc97 80 pc.printf("Range one = %d | range two = %d | range three = %d | range four = %d\r\n",range1, range2, range3, range4);
florine_van 5:8ef79eebbc97 81
florine_van 5:8ef79eebbc97 82 // TODO : better name for this method ??
florine_van 5:8ef79eebbc97 83 CheckObstacle();
florine_van 5:8ef79eebbc97 84
florine_van 4:cb50c6fa340b 85 wait_ms(10);
florine_van 0:57855aafa907 86 }
florine_van 0:57855aafa907 87 }
florine_van 0:57855aafa907 88
florine_van 5:8ef79eebbc97 89
florine_van 5:8ef79eebbc97 90