Empujar
Podemos mejorar dando más funcionalidades a nuestro robot Otto con servos.
Además del accesorio para empujar:
Puedes descargarla aquí: OttoServosAccesorios
Pinzas o garra.
Podemos añadir a nuestro robot una garra para coger y arrastrar pequeños objetos.
Puedes descargarla aquí: OttoServosAccesorios
Su código es:
#include <Servo.h>
Servo myservo1, myservo2, garra; // crea los objetos servo
// para el sonser SR-04
const int EchoPin = 10;
const int TriggerPin = 9;
int vel1 = 0, vel2 = 0, i, cm; // velocidad del servo
void setup() {
Serial.begin(9600);
myservo1.attach(2); // vincula el servo al pin digital 2
myservo2.attach(3); // vincula el servo al pin digital 3
garra.attach(5); // vincula el servo al pin digital 3
pinMode(TriggerPin, OUTPUT);
pinMode(EchoPin, INPUT);
stop();
garra.write(100);
delay(2000);
}
void loop() {
cm = ping(TriggerPin, EchoPin);
while (cm > 20 && cm != 0) {
Serial.println(cm);
derecha(50, 50);
delay(200);
stop();
delay(500);
cm = ping(TriggerPin, EchoPin);
}
derecha(50, 50);
delay(120);
if (cm <= 20 && cm != 0) {
// he encontrado un objeto
stop();
izquierda(40, 40);
delay(50);
stop();
delay(500);
while (cm > 6 && cm != 0) {
adelante(60, 50);
cm = ping(TriggerPin, EchoPin);
delay(20);
}
stop();
garra.write(20);
delay(500);
atras();
delay(400);
garra.write(100);
atras();
delay(400);
derecha(50,50);
delay(300);
}
}
void stop() {
myservo1.write(90);
myservo2.write(90);
}
void adelante(int v1, int v2) {
myservo1.write(v1 + 90);
myservo2.write(90 – v2);
}
void atras() {
myservo1.write(0);
myservo2.write(180);
}
void derecha(int v1, int v2) {
// de 90 a 180
myservo1.write(v1 + 90);
myservo2.write(v2 + 90);
}
void izquierda(int v1, int v2) {
myservo1.write(90 – v1);
myservo2.write(90 – v2);
}
int ping(int TriggerPin, int EchoPin) {
long duration, distanceCm;
digitalWrite(TriggerPin, LOW); //para generar un pulso limpio ponemos a LOW 4us
delayMicroseconds(4);
digitalWrite(TriggerPin, HIGH); //generamos Trigger (disparo) de 10us
delayMicroseconds(10);
digitalWrite(TriggerPin, LOW);
duration = pulseIn(EchoPin, HIGH); //medimos el tiempo entre pulsos, en microsegundos
distanceCm = duration * 10 / 292 / 2; //convertimos a distancia, en cm
return distanceCm;
}
Sensores infrarrojos
O podemos ponerle dos sensores infrarrojos para hacer un siguelíneas:
Utilizaremos el módulo Sensor Siguelíneas IR TCRT5000, sensor infrarrojos formato “Slim”
Y el código:
Hay que tener encuenta que dependiendo de la velocidad, del grosor de la línea negra y de la curvo que intente realizar hay que ajustar los tiempos y las velocidades para que se realize con éxito.
#include <Servo.h>
Servo myservo1, myservo2, garra; // crea los objetos servo
// para el sonser SR-04
const int EchoPin = 10;
const int TriggerPin = 9;
const int sensorD = A1;
const int sensorI = A2;
int vel1 = 50, vel2 = 40, i, cm; // velocidad del servo
int v1, v2; // leer sensores
void setup() {
Serial.begin(9600);
myservo1.attach(2); // vincula el servo al pin digital 2
myservo2.attach(3); // vincula el servo al pin digital 3
pinMode(TriggerPin, OUTPUT);
pinMode(EchoPin, INPUT);
stop();
delay(2000);
}
void loop() {
adelante(vel1, vel2);
v1 = analogRead(sensorI); // realizar la lectura
v2 = analogRead(sensorD); // realizar la lectura
Serial.print(«izquierda: «);
Serial.print(v1);
Serial.print(» derecha: «);
Serial.println(v2);
if (v1 < 100) { // detecta linea negra
Serial.println(«Sobre linea 1»);
stop();
delay(3000);
derecha(vel1, vel2);
delay(200);
stop();
delay(3000);
}
else if (v2 < 100) { // detecta linea negra
Serial.println(«Sobre linea 2»);
stop();
delay(3000);
izquierda(vel1, vel2);
delay(200);
stop();
delay(3000);
}
}
void stop() {
myservo1.write(90);
myservo2.write(90);
}
void adelante(int v1, int v2) {
myservo1.write(v1 + 90);
myservo2.write(90 – v2);
}
void atras(int v1, int v2) {
myservo1.write(90 – v1);
myservo2.write(90 + v2);
}
void derecha(int v1, int v2) {
// de 90 a 180
myservo1.write(v1 + 90);
myservo2.write(v2 + 90);
}
void izquierda(int v1, int v2) {
myservo1.write(90 – v1);
myservo2.write(90 – v2);
}
int ping(int TriggerPin, int EchoPin) {
long duration, distanceCm;
digitalWrite(TriggerPin, LOW); //para generar un pulso limpio ponemos a LOW 4us
delayMicroseconds(4);
digitalWrite(TriggerPin, HIGH); //generamos Trigger (disparo) de 10us
delayMicroseconds(10);
digitalWrite(TriggerPin, LOW);
duration = pulseIn(EchoPin, HIGH); //medimos el tiempo entre pulsos, en microsegundos
distanceCm = duration * 10 / 292 / 2; //convertimos a distancia, en cm
return distanceCm;
}