En el tutorial anterior, completamos el código de nuestro robot Beetle para crear una comunicación Bluetooth con varias aplicaciones.
Para ello utilizabamos una aplicación llamada Arduparser que nos daba la posibilidad de leer cadenas de texto que contienen información que es procesada en trozos divididos por delimitadores.
Con respecto al programa anterior vamos a realizar algunas modificaciones.
- Calibración del nivel de luz para ordenar la ejecución de movimiento
- Calibración de la diferencia de luz medida entre los dos sensores
- Modificación de la velocidad en dirección hacia delante y hacia atras
- Modificación de la velocidad de giro
- Obtención de información del robot
- Obtención del umbral de luz
- Obtención de la diferencia de luz entre sensores
- Obtención del estado de movimiento
Para realizar todo esto, es recomendable haber leido detenidamente los tutoriales anteriores.
Movimiento de servomotores con parámetros de entrada para calibración remota.
Para realizar este paso hemos de configurar la aplicación de App Inventor Beetle de la siguiente manera.
Y para obtener los datos que requerimos primero hemos de recopilar la información que llega de nuestro robot mediante un reloj timer o clock en la aplicación. Cada vez que haya un dato que podamos leer a través del Bluetooth, y esa cadena leida sea mayor que 0, nos lo escribirá por pantalla.
#include <Servo.h>
#include <SoftwareSerial.h>
#include <ArduParser.h>
int vel;
String status;
char BTData;
String BTString;
String ParserData1;
String ParserData2;
String ParserData3;
int LDR_diff;
int threshold;
int diff_threshold;
int ClawMin;
int ClawMax;
int FWDVEL;
int TURNVEL;
int letter;
int ClawValue;
int clawdegrees;
int LDR_Left;
int LDR_Right;
Servo LeftServo;
Servo RightServo;
SoftwareSerial comm(2,3);
arduParser BTParser("<","|",">");
Servo ClawServo;
// Describe this function...
void forward(int vel) {
Servo_attach();
RightServo.write((map(vel,0, 100, 90 , 180)));
LeftServo.write((map(vel,0, 100, 90 , 0)));
}
// Describe this function...
void backwards(int vel) {
Servo_attach();
RightServo.write((map(vel,0, 100, 90 , 0)));
LeftServo.write((map(vel,0, 100, 90 , 180)));
}
// Describe this function...
void turn_left(int vel) {
Servo_attach();
LeftServo.write((map(vel,0, 100, 90 , 180)));
RightServo.write((map(vel,0, 100, 90 , 180)));
}
// Describe this function...
void turn_right(int vel) {
Servo_attach();
LeftServo.write((map(vel,0, 100, 90 , 0)));
RightServo.write((map(vel,0, 100, 90 , 0)));
}
// Describe this function...
void readBTChar(char letter) {
if (letter == ('I')) {
Serial.println("BEETLE MODE AUTOMATIC");
status = "BEETLE";
} else if (letter == ('G')) {
Serial.println("FOLLOWLINES MODE AUTOMATIC");
status = "FOLLOW";
} else if (letter == ('M')) {
Serial.println("MANUAL CONTROL MODE ON");
status = "MANUAL";
} else if (letter == ('U')) {
forward(FWDVEL);
} else if (letter == ('D')) {
backwards(FWDVEL);
} else if (letter == ('L')) {
turn_left(TURNVEL);
} else if (letter == ('R')) {
turn_right(TURNVEL);
} else if (letter == ('S')) {
stop();
} else if (letter == ('C')) {
ClawValue = (int)(((comm.readString()).toInt()));
Serial.println(ClawValue);
LeftServo.write((map(ClawValue,10, 55, ClawMin , ClawMax)));
}
}
// Describe this function...
void stop() {
LeftServo.write(90);
RightServo.write(90);
Servo_detach();
}
// Describe this function...
void Servo_detach() {
LeftServo.detach();
RightServo.detach();
}
// Describe this function...
void Servo_attach() {
LeftServo.attach(9);
RightServo.attach(10);
}
// Describe this function...
void moveClaw(int clawdegrees) {
ClawServo.write((map(clawdegrees,10, 55, ClawMin , ClawMax)));
}
// Describe this function...
void FollowLineMode() {
if (digitalRead(4)) {
RightServo.write(90);
RightServo.detach();
} else {
RightServo.attach(10);
RightServo.write(180);
}
if (digitalRead(5)) {
LeftServo.write(90);
LeftServo.detach();
} else {
LeftServo.attach(9);
LeftServo.write(0);
}
}
// Describe this function...
void readBTParser() {
if (BTParser.entry) {
ParserData1 = (BTParser.data[0]);
ParserData2 = (BTParser.data[1]);
if (ParserData1 == "MOVE") {
status = "MANUAL";
if (ParserData2 == "FORWARD") {
forward(FWDVEL);
} else if (ParserData2 == "BACKWARDS") {
backwards(FWDVEL);
} else if (ParserData2 == "LEFT") {
turn_left(TURNVEL);
} else if (ParserData2 == "RIGHT") {
turn_right(TURNVEL);
} else if (ParserData2 == "STOP") {
stop();
}
} else if (ParserData1 == "STATUS") {
status = ParserData2;
} else if (ParserData1 == "CLAW") {
moveClaw(BTParser.getInt(1));
} else if (ParserData1 == "SET") {
Serial.print("SET ");
if (ParserData2 == "THRESHOLD") {
Serial.print("Threshold: ");
int datathreshold = BTParser.getInt(2);
Serial.println(datathreshold);
threshold = (datathreshold);
} else if (ParserData2 == "DIFF") {
Serial.print("DIFF: ");
int dataDiff = BTParser.getInt(2);
diff_threshold = (dataDiff);
Serial.println(diff_threshold);
}else if (ParserData2 == "FWDVEL") {
Serial.println("FWDVEL: ");
int dataVel = BTParser.getInt(2);
FWDVEL = (dataVel);
Serial.println(FWDVEL);
} else if (ParserData2 == "TURNVEL") {
Serial.println("TURNVEL: ");
int dataVel = BTParser.getInt(2);
TURNVEL = (dataVel);
Serial.println(TURNVEL);
}
} else if (ParserData1 == "GET") {
Serial.print("GET ");
if (ParserData2 == "THRESHOLD") {
Serial.println("THRESHOLD: ");
Serial.println(threshold);
comm.println(threshold);
} else if (ParserData2 == "FWDVEL") {
} else if (ParserData2 == "TURNVEL") {
}
}
}
}
// Describe this function...
void LightMode() {
LDR_Left = (int)(analogRead(A0));
LDR_Right = (int)(analogRead(A1));
LDR_diff = LDR_Left - LDR_Right;
Serial.print(LDR_Left);
Serial.print(",");
Serial.println(LDR_Right);
if (abs(LDR_diff) > threshold) {
if (LDR_diff > diff_threshold) {
turn_left(TURNVEL);
} else if (LDR_diff < diff_threshold * -1) { turn_right(TURNVEL); } } else if (LDR_Left > threshold && LDR_Right > threshold) {
forward(FWDVEL);
} else if (LDR_Left < threshold && LDR_Right < threshold) {
stop();
}
}
void setup() {
Serial.begin(38400);
comm.begin(38400);
pinMode(4, INPUT);
pinMode(5, INPUT);
pinMode(A0, INPUT);
pinMode(A1, INPUT);
status = (String)("MANUAL");
ClawServo.attach(6);
LeftServo.attach(9);
RightServo.attach(10);
BTData = (char)(('0'));
BTString = (String)("");
ParserData1 = (String)("");
ParserData2 = (String)("");
LDR_diff = (int)(0);
threshold = (int)(300);
diff_threshold = (int)(100);
ClawMin = (int)(40);
ClawMax = (int)(115);
FWDVEL = (int)(100);
TURNVEL = (int)(100);
}
void loop() {
BTParser.entry = LOW;
if (comm.available()) {
BTData = (comm.peek());
if (BTData == ('<')) {
BTString = (comm.readString());
Serial.println(BTString);
BTParser.parser (BTString);
BTParser.entry = HIGH;
} else {
BTData = (comm.read());
Serial.println(BTData);
readBTChar(BTData);
}
}
readBTParser();
if (status == "FOLLOW") {
FollowLineMode();
} else if (status == "BEETLE") {
LightMode();
}
}
1
One comment