El codi per a utilitzar amb la placa motor shield, el més important direccions i velocitats que es poden canviar per qualsevol llibreria. A més es pot veure per Serial les velocitats i quins casos hi ha.
S'ha utilitzat : http://www.banggood.com/Motor-Drive-Shield-L293D-For-Arduino-Duemilanove-Mega-Or-UNO-p-72855.html i l'arduino UNO. Només amb dos motors, però ampliable a 4.
/*
Para entender el código es necesario entender como funciona,
para eso vamos a dividir el quadrado del joystick en 9 zonas diferentes, contando una banda muerta
Estas van a ser las lecturas del potenciómetro respecto a un cuadrado (donde 512 es la posición central):
Y(1023)
|----------|
|----------|
X(0)|---512---|X(1023)
|----------|
|----------|
Y(0)
Este cuadrado lo dividiremos de esta manera:
(11)(12)(13)
(21)(22)(23)
(31)(32)(33)
(11)(12d)(13)
(21d)(22d)(23d)
(31)(32)(33)
como si fuese una matriz.
cuando X sea mayor que 512 + la zona muerta estaremos en la fila superior,
cuanto Y sea menor que 512 - la zona muerta estaremos en la columna de la izquierda
para detectar esta lo tendremos que hacer mediante "if", "else if" e "else".
la cruz central es dentro del rango de la zona muerta.
Una vez detectado el caso tendremos que hacer una cosa u otra.
*/
#include <AFMotor.h>
AF_DCMotor motorD(3);
AF_DCMotor motorI(4);
//declaración variables de entrada:
const byte Xpot=A0;
const byte Ypot=A1;
//declaración de variables:
const int cposX=502, cposY=509; //por defecto el valor del potenciometro es de 512 en el centro
unsigned int Xspeed=0, Yspeed=0; //por defecto 0 es completa parada
const int dZ=5; //ancho de banda muerta necesaria sobrepasar
int caso=0; //caso para saber el cuadrante, variables para calcular
int velIz, velDe;
void setup(){
//declaración de pinModes:
Serial.begin(9600);
pinMode(Xpot, INPUT);
pinMode(Ypot, INPUT);
}
void loop(){
delay(200);
int posX = analogRead(Xpot); //leemos los valores de las dos resisténcias variables
int posY = analogRead(Ypot); //y las guardamos de 0 a 1023
Serial.print ("Lectura X: "), Serial.print(posX), Serial.print(" Lectura Y: "), Serial.println(posY);
//adelante o atrás
if(posY>(cposY+dZ)){ //delante
caso=10;
Yspeed= (posY-(cposY+dZ))/2;
Yspeed=constrain(Yspeed, 0, 255);
}
else if(posY<(cposY-dZ)){ //atrás
Yspeed=((cposY-dZ)-posY)/2;
Yspeed=constrain(Yspeed, 0, 255);
caso=30;
}
else { //centro
caso=20;
}
Serial.print("Caso inicial: "), Serial.println(caso);
//izquierda o derecha
if(posX<(cposX-dZ)) //izquierda
{
caso=caso+1;
Xspeed=((cposX-dZ)-posX)/2;
Xspeed=constrain(Xspeed, 0, 255);
}
else if(posX>(cposX+dZ)){ //derecha
Xspeed=(posX-(cposX+dZ))/2;
Xspeed=constrain(Xspeed, 0, 255);
caso=caso+3;
}
else { //centro
caso=caso+2;
}
Serial.print("Yspeed: "), Serial.print(Yspeed),Serial.print("Xspeed: "), Serial.println(Xspeed);
Serial.print("Caso: "), Serial.println(caso);
switch (caso){
case 11:
if(Yspeed>Xspeed){
motorI.run(FORWARD), motorD.run(FORWARD);
velIz=Yspeed-Xspeed, velDe=Yspeed;
motorI.setSpeed(velIz), motorD.setSpeed(velDe);
}
else{
motorI.run(BACKWARD), motorD.run(FORWARD);
velIz=Xspeed-Yspeed, velDe=Xspeed;
motorI.setSpeed(velIz), motorD.setSpeed(velDe);
}
break;
case 12:
//estamos apuntando hacia delante, así que los dos motores irán hacia adelante con la velocidad Yspeed
motorI.run(FORWARD), motorD.run(FORWARD);
velIz=Yspeed, velDe=Yspeed;
motorI.setSpeed(velIz), motorD.setSpeed(velDe);
break;
case 13:
if(Yspeed>Xspeed){
motorI.run(FORWARD), motorD.run(FORWARD);
velIz=Yspeed, velDe=Yspeed-Xspeed;
motorI.setSpeed(velIz), motorD.setSpeed(velDe);
}
else{
motorI.run(FORWARD), motorD.run(BACKWARD);
velIz=Xspeed, velDe=Xspeed-Yspeed;
motorI.setSpeed(velIz), motorD.setSpeed(velDe);
}
break;
case 21:
//en este caso gira sobre si mismo hacia la izquierda, motor izquierdo atras, motor derecho adelante
//la valocidad va respecto lo lejos que se esté del centro, con una velocidad (512-X)/2
motorI.run(BACKWARD), motorD.run(FORWARD);
velIz=Xspeed, velDe=Xspeed;
motorI.setSpeed(Xspeed), motorD.setSpeed(Xspeed);
break;
case 22:
//al estar en la zona muerta definiremos las velocidades como ZERO:
motorI.run(RELEASE), motorD.run(RELEASE);
velIz=0, velDe=0;
motorI.setSpeed(velIz), motorD.setSpeed(velDe);
break;
case 23:
//en este caso gira sobre si mismo hacia la derecha, motor izquierdo adelante, motor derecho atras
//la valocidad va respecto lo lejos que se esté del centro, con una velocidad (X-512)/2
motorI.run(FORWARD), motorD.run(BACKWARD);
velIz=Xspeed, velDe=Xspeed;
motorI.setSpeed(velIz), motorD.setSpeed(velDe);
break;
case 31:
if(Yspeed>Xspeed){
motorI.run(BACKWARD), motorD.run(BACKWARD);
velIz=Yspeed, velDe=Yspeed-Xspeed;
motorI.setSpeed(velIz), motorD.setSpeed(velDe);
}
else{
motorI.run(BACKWARD), motorD.run(FORWARD);
velIz=Xspeed, velDe=Xspeed-Yspeed;
motorI.setSpeed(velIz), motorD.setSpeed(velDe);
}
break;
case 32:
//estamos apuntando hacia atras, así que los dos motores irán hacia atras con la velocidad Yspeed
motorI.run(BACKWARD), motorD.run(BACKWARD);
velIz=Yspeed, velDe=Yspeed;
motorI.setSpeed(velIz), motorD.setSpeed(velDe);
break;
case 33:
if(Yspeed>Xspeed){
motorI.run(BACKWARD), motorD.run(BACKWARD);
velIz=Yspeed-Xspeed, velDe=Yspeed;
motorI.setSpeed(velIz), motorD.setSpeed(velDe);
}
else{
motorI.run(FORWARD), motorD.run(BACKWARD);
velIz=Xspeed-Yspeed, velDe=Xspeed;
motorI.setSpeed(velIz), motorD.setSpeed(velDe);
}
break;
}
Serial.print("Vel mot Izq: "), Serial.print(velIz), Serial.print(", Vel mot Der: "), Serial.println(velDe);
}
