Go Down

Topic: [parte 3]nunckuck non originale! (Read 975 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
 


Please enter a valid email to subscribe

Confirm your email address

We need to confirm your email address.
To complete the subscription, please click the link in the email we just sent you.

Thank you for subscribing!

Arduino
via Egeo 16
Torino, 10131
Italy