Go Down

Topic: [parte 3]nunckuck non originale! (Read 927 times) previous topic - next topic

lesto

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/WiiChuckClass
modifiche:
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
Code: [Select]

#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
Code: [Select]

#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
sei nuovo? non sai da dove partire? leggi qui: http://playground.arduino.cc/Italiano/Newbie

Go Up