Soft robot team / Mbed OS Robot_team1

Robot team project

Dependencies:   QEI Motordriver ros_lib_melodic

main.cpp@6:858a5116688e, 2019-10-23 (annotated)

Committer:
florine_van
Date:
Wed Oct 23 10:38:36 2019 +0000
Revision:
6:858a5116688e
Parent:
5:8ef79eebbc97
Child:
7:2cf57f28255d
Correct motors issue; Clean code

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 6:858a5116688e 15 #define S1 PC_8
florine_van 6:858a5116688e 16 #define S2 PC_9
florine_van 6:858a5116688e 17 #define S3 PC_10
florine_van 6:858a5116688e 18 #define S4 PC_11
florine_van 6:858a5116688e 19 #define S5 PC_12
florine_van 6:858a5116688e 20 #define S6 PD_2
florine_van 6:858a5116688e 21 #define S7 PG_2
florine_van 6:858a5116688e 22 #define S8 PG_3
florine_van 6:858a5116688e 23
florine_van 5:8ef79eebbc97 24 // VL6180x sensors
florine_van 6:858a5116688e 25 Sensor sensor_forward(I2C_SDA, I2C_SCL, S1);
florine_van 6:858a5116688e 26 Sensor sensor_right(I2C_SDA, I2C_SCL, S3);
florine_van 6:858a5116688e 27 Sensor sensor_back(I2C_SDA, I2C_SCL, S5);
florine_van 6:858a5116688e 28 Sensor sensor_left(I2C_SDA, I2C_SCL, S7);
florine_van 5:8ef79eebbc97 29
florine_van 5:8ef79eebbc97 30 // Motors
florine_van 6:858a5116688e 31 Motor motor_left(PC_6, PB_15, PB_13);
florine_van 6:858a5116688e 32 Motor motor_right(PA_15, PC_7, PB_4);
florine_van 5:8ef79eebbc97 33
florine_van 5:8ef79eebbc97 34 void CheckObstacle()
florine_van 6:858a5116688e 35 {
florine_van 5:8ef79eebbc97 36 // When obstacle ahead
florine_van 6:858a5116688e 37 if (sensor_forward.getIsObstacle())
florine_van 5:8ef79eebbc97 38 {
florine_van 6:858a5116688e 39 if ( (sensor_right.getIsObstacle()) && (sensor_left.getIsObstacle()) )
florine_van 6:858a5116688e 40 {
florine_van 6:858a5116688e 41 //Turn backward
florine_van 6:858a5116688e 42 while(!sensor_back.getIsObstacle())
florine_van 6:858a5116688e 43 {
florine_van 6:858a5116688e 44 motor_left.moveForward();
florine_van 6:858a5116688e 45 motor_right.moveBackward();
florine_van 6:858a5116688e 46 }
florine_van 6:858a5116688e 47 }
florine_van 6:858a5116688e 48 if (sensor_left.getIsObstacle())
florine_van 6:858a5116688e 49 {
florine_van 6:858a5116688e 50 //Turn to the right
florine_van 6:858a5116688e 51 motor_left.moveForward();
florine_van 6:858a5116688e 52 motor_right.moveBackward();
florine_van 6:858a5116688e 53 }
florine_van 6:858a5116688e 54 else
florine_van 6:858a5116688e 55 {
florine_van 6:858a5116688e 56 // By default : turn to the left
florine_van 6:858a5116688e 57 motor_left.moveBackward();
florine_van 6:858a5116688e 58 motor_right.moveForward();
florine_van 6:858a5116688e 59 }
florine_van 5:8ef79eebbc97 60 }
florine_van 5:8ef79eebbc97 61 // No obstacle
florine_van 5:8ef79eebbc97 62 else
florine_van 5:8ef79eebbc97 63 {
florine_van 6:858a5116688e 64 motor_left.moveForward();
florine_van 6:858a5116688e 65 motor_right.moveForward();
florine_van 5:8ef79eebbc97 66 }
florine_van 5:8ef79eebbc97 67 }
florine_van 5:8ef79eebbc97 68
florine_van 5:8ef79eebbc97 69 int main()
florine_van 5:8ef79eebbc97 70 {
florine_van 0:57855aafa907 71 int range1;
florine_van 1:d5adc483bce0 72 int range2;
florine_van 2:c537f1ebad7b 73 int range3;
florine_van 2:c537f1ebad7b 74 int range4;
florine_van 3:a3144a45f44c 75
florine_van 3:a3144a45f44c 76 // load settings onto VL6180X sensors
florine_van 6:858a5116688e 77 sensor_forward.init();
florine_van 3:a3144a45f44c 78 // change default address of sensor 2
florine_van 6:858a5116688e 79 sensor_forward.changeAddress(addr2);
florine_van 2:c537f1ebad7b 80
florine_van 6:858a5116688e 81 sensor_right.init();
florine_van 3:a3144a45f44c 82 // change default address of sensor 3
florine_van 6:858a5116688e 83 sensor_right.changeAddress(addr3);
florine_van 2:c537f1ebad7b 84
florine_van 6:858a5116688e 85 sensor_back.init();
florine_van 3:a3144a45f44c 86 // change default address of sensor 4
florine_van 6:858a5116688e 87 sensor_back.changeAddress(addr4);
florine_van 6:858a5116688e 88
florine_van 6:858a5116688e 89 sensor_left.init();
florine_van 2:c537f1ebad7b 90
florine_van 6:858a5116688e 91 //Set Speeds
florine_van 6:858a5116688e 92 motor_left.setSpeed(0.5f);
florine_van 6:858a5116688e 93 motor_right.setSpeed(0.5f);
florine_van 0:57855aafa907 94
florine_van 0:57855aafa907 95 while (1)
florine_van 0:57855aafa907 96 {
florine_van 6:858a5116688e 97 range1 = sensor_forward.read();
florine_van 6:858a5116688e 98 range2 = sensor_right.read();
florine_van 6:858a5116688e 99 range3 = sensor_back.read();
florine_van 6:858a5116688e 100 range4 = sensor_left.read();
florine_van 0:57855aafa907 101
florine_van 6:858a5116688e 102 pc.printf("Range forward = %d | range back = %d | range right = %d | range left = %d\r\n",range1, range3, range2, range4);
florine_van 5:8ef79eebbc97 103
florine_van 5:8ef79eebbc97 104 // TODO : better name for this method ??
florine_van 5:8ef79eebbc97 105 CheckObstacle();
florine_van 5:8ef79eebbc97 106
florine_van 4:cb50c6fa340b 107 wait_ms(10);
florine_van 0:57855aafa907 108 }
florine_van 0:57855aafa907 109 }
florine_van 0:57855aafa907 110
florine_van 5:8ef79eebbc97 111
florine_van 5:8ef79eebbc97 112