passiamo al codice per il nunchuk.
Prima di tutto specifichiamo che i codici di inizializzazione sono modificati per funzionare con un nunchuk non originale.
La classe di partenza è tratta da:
http://www.arduino.cc/playground/Main/WiiChuckClassmodifiche:
aggiunto inizializzazione per nunchuk farlocchi
aggiunto nunchuck_send_request(); a fine init altrimenti il primo valore letto era sempre 0,0,0
nella funzione nunchuk_elaborate_data(), commentato, potete trovare del codice che si occupa della probabile non linearità del nunchuk: in pratica se fate caso, la gravità che legge il nunckuk da fermo non è uguale se inclinato in vari modi.
Attraverso quelle operazioni, è possibile "allungare" il vettore gravità fino a corrispondere alla realtà, senza perdere di precisione sulla "inclinazione" del vettore.
NunchukFarlocco.h
#ifndef NunchukFarlocco_h
#define NunchukFarlocco_h
#include "WProgram.h"
#include "Wire.h"
class NunchukFarlocco{
public:
void nunchuck_init(); // send the initilization handshake
int nunchuck_get_data();
float x, y, z;
private:
char nunchuk_decode_byte (char x);
void nunchuck_send_request();
void nunchuk_elaborate_data();
uint8_t nunchuck_buf[6]; // array to store nunchuck data,
uint8_t ctrlr_type[6];
};
#endif
NunchukFarlocco.cpp
#include "NunchukFarlocco.h"
#define ACC_ZERO_X 484
#define ACC_ZERO_Y 530
#define ACC_ZERO_Z 305
int NunchukFarlocco::nunchuck_get_data(){
int cnt=0;
Wire.requestFrom (0x52, 6); // request data from nunchuck
while (Wire.available ()) {
// receive byte as an integer
nunchuck_buf[cnt] = nunchuk_decode_byte(Wire.receive());
cnt++;
}
nunchuck_send_request(); // send request for next data payload
// If we recieved the 6 bytes, then go elaborate them
if (cnt >= 5) {
nunchuk_elaborate_data();
return 1; // success
}
return 0; //failure
}
float gSquared=0, g, minE, maxE;
void NunchukFarlocco::nunchuk_elaborate_data(){
int accel_x_axis = nunchuck_buf[2] * 2 * 2;
int accel_y_axis = nunchuck_buf[3] * 2 * 2;
int accel_z_axis = nunchuck_buf[4] * 2 * 2;
// byte nunchuck_buf[5] contains bits for z and c buttons
// it also contains the least significant bits for the accelerometer data
// so we have to check each bit of byte outbuf[5]
if ((nunchuck_buf[5] >> 2) & 1)
accel_x_axis += 2;
if ((nunchuck_buf[5] >> 3) & 1)
accel_x_axis += 1;
if ((nunchuck_buf[5] >> 4) & 1)
accel_y_axis += 2;
if ((nunchuck_buf[5] >> 5) & 1)
accel_y_axis += 1;
if ((nunchuck_buf[5] >> 6) & 1)
accel_z_axis += 2;
if ((nunchuck_buf[5] >> 7) & 1)
accel_z_axis += 1;
x=accel_x_axis-ACC_ZERO_X;
//this is not an error, IMU use different reference system
z=accel_y_axis-ACC_ZERO_Y;
y=accel_z_axis-ACC_ZERO_Z;
/*
float t = x*x+y*y+z*z;
Serial.print( "t:" );
Serial.print( sqrt(t) );
Serial.print("x:");
Serial.print(x);
Serial.print("y:");
Serial.print(y);
Serial.print("z:");
Serial.println(z);
*/
/*
if (gSquared==0){
g = x*x+y*y+z*z;
gSquared=sqrt(g);
minE = g-(g/5);
maxE = g+(g/5);
}else{
float t = x*x+y*y+z*z;
/*
Serial.print( "t:" );
Serial.print( sqrt(t) );
Serial.print(" should be:");
Serial.println( gSquared );
if (t!=g){
if (t >= minE && t <= maxE){
double phi = atan2(y, x);
double theta = acos( z/sqrt(t) );
x=gSquared*sin(theta)*cos(phi);
y=gSquared*sin(theta)*sin(phi);
z=gSquared*cos(theta);
/*
Serial.print("old distance:");
Serial.print( sqrt(t) );
Serial.print(" new distance:");
Serial.print( sqrt(x*x+y*y+z*z) );
Serial.print(" should be:");
Serial.println( gSquared );
}else{
/*
Serial.print( "acc no good data; t:" );
Serial.print( sqrt(t) );
Serial.print(" should be:");
Serial.println( gSquared );
}
}
}
*/
}
// Encode data to format that most wiimote drivers except
// only needed if you use one of the regular wiimote drivers
char NunchukFarlocco::nunchuk_decode_byte (char x){
x = (x ^ 0x17) + 0x17;
return x;
}
void NunchukFarlocco::nunchuck_send_request(){
Wire.beginTransmission(0x52); // transmit to device 0x52
Wire.send(0x00); // sends one byte
Wire.endTransmission(); // stop transmitting
}
void NunchukFarlocco::nunchuck_init(){
/*
//
// FOR ORIGINAL NUNCHUCK
//
Wire.begin(); // join i2c bus as master
Wire.beginTransmission(0x52); // transmit to device 0x52
Wire.send(0x40); // sends memory address
Wire.send(0x00); // sends sent a zero.
Wire.endTransmission(); // stop transmitting
//
// END FOR ORIGINAL NUNCHUCK
//
*/
//
// FOR FAKE NUNCHUCK
//
byte cnt;
//Serial.print ("Begin3\n");
Wire.begin();
// Serial.print ("Begin4\n");
// init controller
delay(1);
Wire.beginTransmission(0x52); // device address
// Serial.print ("Begin40\n");
Wire.send(0xF0); // 1st initialisation register
// Serial.print ("Begin40\n");
Wire.send(0x55); // 1st initialisation value
Serial.print ("Begin40\n");
Wire.endTransmission();
Serial.print ("Begin41\n");
delay(1);
Wire.beginTransmission(0x52);
Wire.send(0xFB); // 2nd initialisation register
Wire.send(0x00); // 2nd initialisation value
Serial.print ("Begin42\n");
Wire.endTransmission();
delay(1);
Serial.print ("Begin5\n");
// read the extension type from the register block
Wire.beginTransmission(0x52);
Wire.send(0xFA); // extension type register
Wire.endTransmission();
Wire.beginTransmission(0x52);
Wire.requestFrom(0x52, 6); // request data from controller
Serial.print ("Begin6\n");
for (cnt = 0; cnt < 6; cnt++) {
if (Wire.available()) {
ctrlr_type[cnt] = Wire.receive(); // Should be 0x0000 A420 0101 for Classic Controller, 0x0000 A420 0000 for nunchuck
}
Serial.print ("Begin7\n");
}
Wire.endTransmission();
delay(1);
// send the crypto key (zeros), in 3 blocks of 6, 6 & 4.
Wire.beginTransmission(0x52);
Wire.send(0xF0); // crypto key command register
Wire.send(0xAA); // sends crypto enable notice
Wire.endTransmission();
delay(1);
Wire.beginTransmission(0x52);
Wire.send(0x40); // crypto key data address
for (cnt = 0; cnt < 6; cnt++) {
Wire.send(0x00); // sends 1st key block (zeros)
}
Wire.endTransmission();
Wire.beginTransmission(0x52);
Wire.send(0x40); // sends memory address
for (cnt = 6; cnt < 12; cnt++) {
Wire.send(0x00); // sends 2nd key block (zeros)
}
Wire.endTransmission();
Wire.beginTransmission(0x52);
Wire.send(0x40); // sends memory address
for (cnt = 12; cnt < 16; cnt++) {
Wire.send(0x00); // sends 3rd key block (zeros)
}
Wire.endTransmission();
delay(1);
//end device init
//
// END FAKE NUNCHUCK
//
nunchuck_send_request();
}
ATTENZIONE: modificare i valori di ACC_ZERO_X, ACC_ZERO_Y e ACC_ZERO_Z in questo modo: leggete il valore più alto e più basso (da fermo, se no leggete anche le accelerazioni) e trovate il valore a metà.
Oppure se l'avete già smontato, potete mettere la scheda più piatta possibile, in questo modo leggete ACC_ZERO_X, ACC_ZERO_Y.
Ora mettete il nun a 90 gradi, o ACC_ZERO_X o ACC_ZERO_Y dovrebbe mantenere lo stesso valore(l'altro dovrebbe invece cambiare) e leggete il valore di ACC_ZERO_Z
per testare la linearità del nun e la valodità dei valori che avete trovato stampate a video la variabile t: a nunckuk fisso non dovrebbe variare troppo(anche se personalmente, forse data la natura farlocca del nun, ho visto errori > 200)
E' normale che col nun in movimento t cambi, perchè legge anche le accelerazioni!!! ecco il motivo per cui la normalizzazione del vettore (codice commentato) viene effettuata solo se il vettore è errato meno di 1/5 del valore corretto.
ATTENZIONE2: All'inizializzazione del nun, deve essere fermo se si vuole controllare il valore t o effettuare la correzione del vettore, altrimenti sfasa tutto
| Arduino Forum :: Forum 2005-2010 (read only) :: International :: Italiano :: [parte 3]nunckuck non originale!