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