4.1. Estructura del codi
Aquest codi:
- Llegeix distància del sensor esquerre i dret
- Determina quin sensor detecta l’obstacle més proper
- Mou el servo cap a l’angle corresponent
- Calcula un angle interpolat si vols més precisió
A la plataforma bitbloq, el codi associat en forma de blocs seria el següent:
4.2. Desenvolupament (amb bitbloq Apps)
Secció de definició de variables:
Secció setup:
Secció loop:
4.3. Desenvolupament (en C++)
El codi, en C++, seria el següent:
// Llibreries incloses
#include <BitbloqUS.h>
#include <Servo.h>
#include <BitbloqSoftwareSerial.h>
// Variables i funcions
uint8_t ledL = 13;
uint8_t ledC = 12;
uint8_t ledR = 11;
US ultrasonidosL(7, 7);
US ultrasonidosC(6, 6);
US ultrasonidosR(5, 5);
Servo servo;
bqSoftwareSerial board_bluetooth(0, 1, 115200);
float distanciaL = 0;
float distanciaC = 0;
float distanciaR = 0;
float angle = 90;
// Setup
void setup() {
pinMode(ledL, OUTPUT);
pinMode(ledC, OUTPUT);
pinMode(ledR, OUTPUT);
servo.attach(3);
board_bluetooth.begin(115200);
servo.write(angle);
board_bluetooth.println(angle);
}
// Loop
void loop() {
distanciaL = ultrasonidosL.read();
distanciaC = ultrasonidosC.read();
distanciaR = ultrasonidosR.read();
if (distanciaL == 0) {
distanciaL = 999;
}
if (distanciaC == 0) {
distanciaC = 999;
}
if (distanciaR == 0) {
distanciaR = 999;
}
if (((distanciaL < distanciaC) && (distanciaL <= distanciaR))) {
angle = 45;
digitalWrite(ledL, HIGH);
digitalWrite(ledC, LOW);
digitalWrite(ledR, LOW);
}
else if (((distanciaC >= distanciaL) && (distanciaL <= distanciaR))) {
angle = 67.5;
digitalWrite(ledL, HIGH);
digitalWrite(ledC, HIGH);
digitalWrite(ledR, LOW);
}
else if (((distanciaC >= distanciaL) && (distanciaC <= distanciaR))) {
angle = 90;
digitalWrite(ledC, HIGH);
digitalWrite(ledR, LOW);
digitalWrite(ledL, LOW);
}
else if (((distanciaC >= distanciaL) && (distanciaC > distanciaR))) {
angle = 112.5;
digitalWrite(ledC, HIGH);
digitalWrite(ledR, HIGH);
digitalWrite(ledL, LOW);
}
else if (((distanciaL >= distanciaR) && (distanciaC > distanciaR))) {
angle = 135;
digitalWrite(ledR, HIGH);
digitalWrite(ledC, LOW);
digitalWrite(ledL, LOW);
}
else {
angle = 90;
digitalWrite(ledL, LOW);
digitalWrite(ledC, LOW);
digitalWrite(ledR, LOW);
}
board_bluetooth.println(angle);
delay(20);
}