CocheNunchuk el coche controlado con Nunchuk

He preparado la base de un coche con dos motores de Lego Mindstorm RCX conectados al Arduino UNO mediante el Motor Shield de Adafruit.com. Gracias al Wiichuck Adapter conectamos el Nunchuk y utilizando esta librería preparo el programa con el que controlaremos los motores.

 En el Nunchuk he configurado lo siguiente:

• botón de parada
• botón de arranque
• girandole sobre el eje X giro a derecha o izquierda
• levantándole o bajándole sobre el eje Y se regula la velocidad
• con el joystick que lleva integrado controlo la marcha atrás

Código:

/*
 * CocheNunchukV2 Coche con ArduinoNunchuk y motores Lego
 * Mediante un nunchuk conectado mediante el cable puedo manejar un coche hecho con dos motores Lego
 * En el Nunchuk he configurado lo siguiente:
 * - botón de parada
 * - botón de arranque
 * - girandole sobre el eje X giro a derecha o izquierda
 * - levantándole o bajándole sobre el eje Y se regula la velocidad
 * - con el joystick que lleva integrado controlo la marcha atrás
 *
 * Copyleft 2011 Antonio Garcia Figueras
 *
 * Usando libreria de Gabriel Bianconi,  http://www.gabrielbianconi.com/projects/arduinonunchuk/
 *
 */

#include <Wire.h>
#include "ArduinoNunchuk.h"
#include <AFMotor.h>

#define BAUDRATE 19200
#define PORCENTAJE_MARGENX 20
#define PORCENTAJE_MARGENY 20
ArduinoNunchuk nunchuk = ArduinoNunchuk();

//Servo myservo;

 int minX;
 int maxX;
 int minY;
 int maxY;
 int minZ;
 int maxZ;
 float centroX;
 float margenX;
 int contador;
 float razonY;
 int posXServo;
 boolean swStop;
 int velocidad;

 AF_DCMotor motorIz(1, MOTOR12_64KHZ); // create motor izquierda, 64KHz pwm
 AF_DCMotor motorDe(2, MOTOR12_64KHZ); // create motor derecha, 64KHz pwm

void setup() {
  Serial.begin(BAUDRATE);

  pinMode(13, OUTPUT);    //Iniciamos el led

  Serial.println("Inicio incializacion");
  nunchuk.init();
  Serial.println("Final incializacion"); 

  calibraAccel();
 
  // Averiguamos cual es el valor del punto medio
  centroX = (float)(minX + (maxX -minX)/2);
  Serial.print("centroX:");
  Serial.println(centroX, DEC);

  // Daremos este margen desde el centro para considerar que va recto
  margenX =  (float)PORCENTAJE_MARGENX * (maxX -minX)/100;
  Serial.print("margenX:");
  Serial.println(margenX, DEC); 

//Calculo de la razon para calcular el valor del velocidad (70-255=185)
//en funcion del movimiento del nunchucku en el eje Y
  razonY = (float)185/(maxX - minX); 
  Serial.print("RazonY:");
  Serial.println(razonY, DEC);
 
  swStop = true;
}

void loop() {
     nunchuk.update();
    
     imprimeValoresNunchuk();
//     delay(2000);
    
     if (nunchuk.zButton==1){
      swStop=true;
     }else if (nunchuk.cButton==1){
      swStop=false;
     }    
    
     if (swStop){
       para();
       Serial.println("swStop es true");
     }else{
       determinaMovimiento();
       Serial.println("swStop es false");
     }
//     Serial.println("***************************************");
//     Serial.print("accelX:");
//     Serial.println(nunchuk.accelX, DEC);
    


}

void parpadea(){
  for (int cont = 0; cont < 3; cont++){
    digitalWrite(13, HIGH);   // set the LED on
    delay(500);              // wait for a second
    digitalWrite(13, LOW);    // set the LED off
    delay(500); 
  }
}
 
void determinaVelocidad(){
     // Al multiplicar la posicion Y del nunckuk por la razon
     // lo convertimos a valores entre 70 y 255 para la velocidad del motor
     velocidad = (int)((nunchuk.accelY - (maxY - minY)) * razonY)+70;
    
     motorIz.setSpeed(velocidad);   
     motorDe.setSpeed(velocidad);   
     Serial.print("accelY:");
     Serial.println(nunchuk.accelY, DEC); 
     Serial.print("Velocidad");
     Serial.println(velocidad, DEC);     
}

void determinaMovimiento(){
     determinaVelocidad();
     if (nunchuk.accelX > (centroX + margenX)){
       giroDerecha();
     }else if(nunchuk.accelX < (centroX - margenX)){
       giroIzquierda();
     }else{
       recto();
     } 
}

void giroDerecha(){
  Serial.print("giroDerecha");
  motorDe.run(RELEASE);
  if (nunchuk.analogY < 100){ 
    motorIz.run(BACKWARD);
  }else{
    motorIz.run(FORWARD);
  }   
}

void giroIzquierda(){
  Serial.print("giroIzquierda");
  motorIz.run(RELEASE);
  if (nunchuk.analogY < 100){ 
    motorDe.run(BACKWARD);
  }else{
    motorDe.run(FORWARD);
  }   
}

void recto(){
  Serial.print("recto");
  if (nunchuk.analogY < 100){
    motorIz.run(BACKWARD);
    motorDe.run(BACKWARD);
  }else{
    motorIz.run(FORWARD);
    motorDe.run(FORWARD);
  }
} 

void para(){
  motorIz.run(RELEASE);
  motorDe.run(RELEASE);
} 

// Calibra todos los acelerometros para saber sus
// valores minimos y maximos
void calibraAccel()  {
 parpadea(); // parpadea cuando va a comenzar la calibración
 nunchuk.update();
 minX = nunchuk.accelX;
 maxX = nunchuk.accelX;
 minY = nunchuk.accelY;
 maxY = nunchuk.accelY;
 minZ = nunchuk.accelZ;
 maxZ = nunchuk.accelZ;
 contador=0;
 
 Serial.println("Inicio calibracion");
 
 while (contador<=200){
     Serial.print("Paso=");
     Serial.println(contador);
     nunchuk.update();
     if (nunchuk.accelX < minX){
       minX=nunchuk.accelX;
     }
     if (nunchuk.accelX > maxX){
       maxX=nunchuk.accelX;
     }
    
     if (nunchuk.accelY < minY){
       minY=nunchuk.accelY;
     }
     if (nunchuk.accelY > maxY){
       maxY=nunchuk.accelY;
     }

     if (nunchuk.accelZ < minZ){
       minX=nunchuk.accelZ;
     }
     if (nunchuk.accelZ > maxZ){
       maxZ=nunchuk.accelZ;
     }    
     delay(10);
     contador++;
 }
 Serial.print("Resultado calibracion X. MinX=");
 Serial.print(minX, DEC);
 Serial.print(" MaxX=");
 Serial.print(maxX, DEC);
 Serial.print(" Diferencia=");
 Serial.println(maxX - minX);
 
 Serial.print("Resultado calibracion Y. MinY=");
 Serial.print(minY, DEC);
 Serial.print(" MaxY=");
 Serial.print(maxY, DEC);
 Serial.print(" Diferencia=");
 Serial.println(maxY - minY);
 
 Serial.print("Resultado calibracion Z. MinZ=");
 Serial.print(minZ, DEC);
 Serial.print(" MaxZ=");
 Serial.print(maxZ, DEC);
 Serial.print(" Diferencia=");
 Serial.println(maxZ - minZ);
 
 parpadea();// parpadea al finalizar la calibración
}


void imprimeValoresNunchuk(){
  Serial.print(nunchuk.analogX, DEC);
  Serial.print(' ');
  Serial.print(nunchuk.analogY, DEC);
  Serial.print(' ');
  Serial.print(nunchuk.accelX, DEC);
  Serial.print(' ');
  Serial.print(nunchuk.accelY, DEC);
  Serial.print(' ');
  Serial.print(nunchuk.accelZ, DEC);
  Serial.print(' ');
  Serial.print(nunchuk.zButton, DEC);
  Serial.print(' ');
  Serial.println(nunchuk.cButton, DEC);
}