Código Control diferencial por joystick de un robot

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);
}