Paso 4: codificación
Nuestro código realiza las siguientes funciones:
- La robótica tiene una posición inicial fija. Se mueve de ahí a agarrar una botella de pintura de aceite del soporte (que está al lado de él), se abre a la pinza, capturas asimiento de la botella y se aprieta el mango de la botella.
- Desde aquí, se mueve a la posición de la tina del agua, justo debajo de la garra, se mueve hasta el final de la derecha de la piscina de agua, aprieta la botella una vez, se mueve hacia el centro, aprieta, se mueve a la posición más a la izquierda y cae otra gota de pintura.
- Luego vuelve a su posición inicial
- A continuación vuelve a tomar otra pintura de color
- Y (2) otra vez, pero esta vez gotas de sólo 2 gotas de pintura en puntos diferentes de donde había caído la pintura previamente.
/ * Referencias: Adafruit motor ejemplo de Motor de la C.C. de escudo llamado MotorTest
BitVoicer programa de ejemplo para cambiar LED. Utilizamos los objetos de este ejemplo sólo para la creación de instancias * /
#include < Wire.h >
#include < Adafruit_MotorShield.h >
#include "utility/Adafruit_PWMServoDriver.h"
#include < BitVoicer11.h >
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
Adafruit_DCMotor * myMotor1 = AFMS.getMotor(1);
Adafruit_DCMotor * myMotor2 = AFMS.getMotor(2);
Adafruit_DCMotor * myMotor3 = AFMS.getMotor(3);
Adafruit_DCMotor * myMotor4 = AFMS.getMotor(4);
BitVoicerSerial bvSerial = BitVoicerSerial();
sampleTest boolean = false;
tipo de datos byte = 0;
void setup() {}
Serial.Begin(9600);
Serial.println ("Adafruit Motorshield v2 - prueba del DC Motor!");
AFMS.begin();
myMotor4 -> setSpeed(150);
myMotor4 -> run(FORWARD);
encender motor
myMotor4 -> run(RELEASE);
myMotor2 -> setSpeed(150);
myMotor2 -> run(FORWARD);
encender motor
myMotor2 -> run(RELEASE);
bvSerial.setAnalogReference(BV_EXTERNAL);
bvSerial.setAudioInput(0);
Serial.Begin(9600);
}
void loop() {}
Si (sampleTest == true)
{
bvSerial.processAudio(46);
}
Si (bvSerial.engineRunning)
{
bvSerial.processAudio(46);
}
}
void serialEvent()
{
tipo de datos = bvSerial.getData();
Si (dataType == BV_COMMAND)
sampleTest = bvSerial.cmdData;
Si (dataType == BV_STATUS & & bvSerial.engineRunning == true)
bvSerial.startStopListening();
Si (dataType == BV_STR)
FUNC1();
}
void func1 () ///3 gotas
{
Si (bvSerial.strData == "5")
{
myMotor4 -> run(FORWARD); viene a recoger a color
myMotor4 -> setSpeed(150);
Delay(3000);
myMotor4 -> setSpeed(0);
Delay(300);
myMotor2 -> Ejecutar (al revés); //opening la garra
myMotor2 -> setSpeed(150);
Delay(380);
myMotor2 -> setSpeed(0);
Delay(200);
myMotor2 -> Ejecutar (en adelante); //closing la garra
myMotor2 -> setSpeed(150);
Delay(600);
myMotor2 -> setSpeed(0);
Delay(400);
myMotor3 -> run(BACKWARD); mueve a la pinza en la parte superior
myMotor3 -> setSpeed(150);
Delay(600);
myMotor3 -> setSpeed(0);
Delay(300);
myMotor4 -> run(BACKWARD);
myMotor4 -> setSpeed(150);
Delay(4000);
myMotor4 -> setSpeed(0);
Delay(300);
myMotor2 -> Ejecutar (en adelante); //squeezing la garra
myMotor2 -> setSpeed(200);
Delay(300);
myMotor2 -> setSpeed(0);
Delay(200);
myMotor2 -> Ejecutar (al revés); //Unsqueezing
myMotor2 -> setSpeed(250);
Delay(50);
myMotor2 -> setSpeed(0);
Delay(200);
myMotor4 -> run(FORWARD);
myMotor4 -> setSpeed(150);
Delay(1000);
myMotor4 -> setSpeed(0);
Delay(300);
myMotor2 -> Ejecutar (en adelante); //squeezing la garra
myMotor2 -> setSpeed(150);
Delay(300);
myMotor2 -> setSpeed(0);
Delay(200);
myMotor2 -> Ejecutar (al revés); //Unsqueezing
myMotor2 -> setSpeed(150);
Delay(50);
myMotor2 -> setSpeed(0);
Delay(200);
myMotor4 -> run(FORWARD);
myMotor4 -> setSpeed(150);
Delay(1000);
myMotor4 -> setSpeed(0);
Delay(300);
myMotor2 -> Ejecutar (en adelante); //squeezing la garra
myMotor2 -> setSpeed(150);
Delay(300);
myMotor2 -> setSpeed(0);
Delay(200);
myMotor2 -> Ejecutar (al revés); //Unsqueezing
myMotor2 -> setSpeed(150);
Delay(50);
myMotor2 -> setSpeed(0);
Delay(200);
myMotor4 -> run(FORWARD);
myMotor4 -> setSpeed(150);
Delay(2000);
myMotor4 -> setSpeed(0);
Delay(300);
myMotor3 -> run(FORWARD);
myMotor3 -> setSpeed(150);
Delay(600);
myMotor3 -> setSpeed(0);
Delay(300);
myMotor2 -> Ejecutar (al revés); //opening la garra
myMotor2 -> setSpeed(150);
Delay(400);
myMotor2 -> setSpeed(0);
Delay(400);
myMotor4 -> Ejecutar (al revés); //opening la garra
myMotor4 -> setSpeed(150);
Delay(4000);
myMotor4 -> setSpeed(0);
Delay(400);
func2();
}
else if (bvSerial.strData == "dos")
{
two_drops();
}
otra cosa
{
bvSerial.startStopListening();
bvSerial.sendToBV ("ERROR:" + bvSerial.strData);
bvSerial.startStopListening();
}
}
void func2 () ///2 gotas
{
myMotor4 -> run(FORWARD); viene a recoger a color
myMotor4 -> setSpeed(150);
Delay(4000);
myMotor4 -> setSpeed(0);
Delay(300);
myMotor2 -> Ejecutar (al revés); //opening la garra
myMotor2 -> setSpeed(150);
Delay(380);
myMotor2 -> setSpeed(0);
Delay(200);
myMotor2 -> Ejecutar (en adelante); //closing la garra
myMotor2 -> setSpeed(150);
Delay(600);
myMotor2 -> setSpeed(0);
Delay(400);
myMotor3 -> run(BACKWARD); mueve a la pinza en la parte superior
myMotor3 -> setSpeed(150);
Delay(600);
myMotor3 -> setSpeed(0);
Delay(300);
myMotor4 -> run(BACKWARD);
myMotor4 -> setSpeed(150);
Delay(3500);
myMotor4 -> setSpeed(0);
Delay(300);
myMotor2 -> Ejecutar (en adelante); //squeezing la garra
myMotor2 -> setSpeed(150);
Delay(300);
myMotor2 -> setSpeed(0);
Delay(200);
myMotor2 -> Ejecutar (al revés); //Unsqueezing
myMotor2 -> setSpeed(150);
Delay(50);
myMotor2 -> setSpeed(0);
Delay(200);
myMotor4 -> run(BACKWARD);
myMotor4 -> setSpeed(150);
Delay(1000);
myMotor4 -> setSpeed(0);
Delay(400);
myMotor4 -> run(FORWARD);
myMotor4 -> setSpeed(150);
Delay(2000);
myMotor4 -> setSpeed(0);
Delay(400);
myMotor3 -> run(FORWARD); mueve a la pinza en la parte superior
myMotor3 -> setSpeed(150);
Delay(600);
myMotor3 -> setSpeed(0);
Delay(300);
myMotor3 -> run(BACKWARD); se mueve a la pinza por debajo de
myMotor3 -> setSpeed(150);
Delay(600);
myMotor3 -> setSpeed(0);
Delay(300);
myMotor3 -> run(FORWARD); mueve a la pinza en la parte superior
myMotor3 -> setSpeed(150);
Delay(600);
myMotor3 -> setSpeed(0);
Delay(300);
myMotor3 -> run(BACKWARD); se mueve a la pinza por debajo de
myMotor3 -> setSpeed(150);
Delay(600);
myMotor3 -> setSpeed(0);
Delay(300);
myMotor3 -> run(FORWARD); mueve a la pinza en la parte superior
myMotor3 -> setSpeed(150);
Delay(600);
myMotor3 -> setSpeed(0);
Delay(300);
myMotor3 -> run(BACKWARD); se mueve a la pinza por debajo de
myMotor3 -> setSpeed(150);
Delay(600);
myMotor3 -> setSpeed(0);
Delay(300);
myMotor4 -> run(FORWARD);
myMotor4 -> setSpeed(150);
Delay(1000);
myMotor4 -> setSpeed(0);
Delay(300);
myMotor2 -> Ejecutar (en adelante); //squeezing la garra
myMotor2 -> setSpeed(150);
Delay(300);
myMotor2 -> setSpeed(0);
Delay(200);
myMotor2 -> Ejecutar (al revés); //Unsqueezing
myMotor2 -> setSpeed(150);
Delay(50);
myMotor2 -> setSpeed(0);
Delay(200);
myMotor4 -> run(FORWARD);
myMotor4 -> setSpeed(150);
Delay(2500);
myMotor4 -> setSpeed(0);
Delay(300);
myMotor3 -> run(FORWARD);
myMotor3 -> setSpeed(150);
Delay(600);
myMotor3 -> setSpeed(0);
Delay(300);
myMotor2 -> Ejecutar (al revés); //opening la garra
myMotor2 -> setSpeed(150);
Delay(400);
myMotor2 -> setSpeed(0);
Delay(400);
myMotor4 -> run(BACKWARD);
myMotor4 -> setSpeed(150);
Delay(3000);
myMotor4 -> setSpeed(0);
Delay(400);
}
void two_drops()
{
myMotor4 -> run(FORWARD); viene a recoger a color
myMotor4 -> setSpeed(150);
Delay(3000);
myMotor4 -> setSpeed(0);
Delay(300);
myMotor2 -> Ejecutar (al revés); //opening la garra
myMotor2 -> setSpeed(150);
Delay(380);
myMotor2 -> setSpeed(0);
Delay(200);
myMotor2 -> Ejecutar (en adelante); //closing la garra
myMotor2 -> setSpeed(150);
Delay(600);
myMotor2 -> setSpeed(0);
Delay(400);
myMotor3 -> run(BACKWARD); mueve a la pinza en la parte superior
myMotor3 -> setSpeed(150);
Delay(600);
myMotor3 -> setSpeed(0);
Delay(300);
myMotor4 -> run(BACKWARD);
myMotor4 -> setSpeed(150);
Delay(3500);
myMotor4 -> setSpeed(0);
Delay(300);
myMotor2 -> Ejecutar (en adelante); //squeezing la garra
myMotor2 -> setSpeed(150);
Delay(300);
myMotor2 -> setSpeed(0);
Delay(200);
myMotor2 -> Ejecutar (al revés); //Unsqueezing
myMotor2 -> setSpeed(150);
Delay(50);
myMotor2 -> setSpeed(0);
Delay(200);
myMotor4 -> run(FORWARD);
myMotor4 -> setSpeed(150);
Delay(1500);
myMotor4 -> setSpeed(0);
Delay(300);
myMotor2 -> Ejecutar (en adelante); //squeezing la garra
myMotor2 -> setSpeed(150);
Delay(300);
myMotor2 -> setSpeed(0);
Delay(200);
myMotor2 -> Ejecutar (al revés); //Unsqueezing
myMotor2 -> setSpeed(150);
Delay(50);
myMotor2 -> setSpeed(0);
Delay(200);
myMotor4 -> run(FORWARD);
myMotor4 -> setSpeed(150);
Delay(2000);
myMotor4 -> setSpeed(0);
Delay(300);
myMotor3 -> run(FORWARD);
myMotor3 -> setSpeed(150);
Delay(600);
myMotor3 -> setSpeed(0);
Delay(300);
myMotor2 -> Ejecutar (al revés); //opening la garra
myMotor2 -> setSpeed(150);
Delay(400);
myMotor2 -> setSpeed(0);
Delay(400);
myMotor4 -> run(BACKWARD);
myMotor4 -> setSpeed(150);
Delay(3000);
myMotor4 -> setSpeed(0);
Delay(400);
}
void brush()
{
myMotor4 -> run(BACKWARD);
myMotor4 -> setSpeed(150);
Delay(3000);
myMotor4 -> setSpeed(0);
Delay(400);
myMotor2 -> Ejecutar (al revés); //opening la garra
myMotor2 -> setSpeed(150);
Delay(380);
myMotor2 -> setSpeed(0);
Delay(200);
myMotor2 -> Ejecutar (en adelante); //closing la garra
myMotor2 -> setSpeed(150);
Delay(600);
myMotor2 -> setSpeed(0);
Delay(400);
myMotor4 -> run(FORWARD);
myMotor4 -> setSpeed(150);
Delay(3000);
myMotor4 -> setSpeed(0);
Delay(400);
myMotor1 -> run(BACKWARD);
myMotor1 -> setSpeed(150);
Delay(600);
myMotor1 -> setSpeed(0);
Delay(400);
myMotor4 -> run(BACKWARD);
myMotor4 -> setSpeed(150);
Delay(1000);
myMotor4 -> setSpeed(0);
Delay(400);
myMotor4 -> run(FORWARD);
myMotor4 -> setSpeed(150);
Delay(2000);
myMotor4 -> setSpeed(0);
Delay(400);
myMotor1 -> run(FORWARD);
myMotor1 -> setSpeed(150);
Delay(600);
myMotor1 -> setSpeed(0);
Delay(400);
myMotor4 -> run(BACKWARD);
myMotor4 -> setSpeed(150);
Delay(4000);
myMotor4 -> setSpeed(0);
Delay(400);
myMotor2 -> Ejecutar (al revés); //opening la garra
myMotor2 -> setSpeed(150);
Delay(380);
myMotor2 -> setSpeed(0);
Delay(200);
myMotor4 -> run(FORWARD);
myMotor4 -> setSpeed(150);
Delay(3000);
myMotor4 -> setSpeed(0);
Delay(400);
}