Originalmente inviato da CarloRoma63  Ecco uno sketch che gestisce ben due Nunchuck su un arduino mega 2560. Lo commento nel prossimo post.
Carlo
__________________________________________________ _________________
#include <Servo.h>
#include <SoftI2CMaster.h> Questa libreria mi permette di usare qualsiasi pin come porta TWI, superando i problemi di quella nativa del 2560 (vedi il forum di arduino per vedere quanti hanno avuto problemi a farla funzionare)
#include <LiquidCrystal.h>
LiquidCrystal lcd(8, 13, 9, 4, 5, 6, 7);
Servo myservo; // create servo object to control a servo
const byte sdaPin1 = 3; // digital pin 1 wired to 'd' on BlinkM
const byte sclPin1 = 2; // digital pin 0 wired to 'c' on BlinkM
const byte sdaPin2 = 1; // digital pin 3 wired to 'd' on BlinkM
const byte sclPin2 = 0; // digital pin 2 wired to 'c' on BlinkM
SoftI2CMaster i2c = SoftI2CMaster( sdaPin1,sclPin1 );
SoftI2CMaster i2d = SoftI2CMaster( sdaPin2,sclPin2 );
Qui ho definito due istanze (i2c e i2d) dell'oggetto SoftI2CMaster, sono gli oggetti che permettono la comunicazione con i due nunchuck
int val; // variable to read the value from the analog pin
int loop_cnt;
static uint8_t nunchuck_bufa[6]; // array to store nunchuck data,
static uint8_t nunchuck_bufa1[6]; // array to store nunchuck data,
static uint8_t nunchuck_bufa2[6]; // array to store nunchuck data,
static uint8_t nunchuck_bufa3[10]; // array to store nunchuck data,
static uint8_t nunchuck_bufa4[10]; // array to store nunchuck data,
static uint8_t nunchuck_bufa5[10]; // array to store nunchuck data,
static uint8_t nunchuck_bufb[6]; // array to store nunchuck data,
static uint8_t nunchuck_bufb1[6]; // array to store nunchuck data,
static uint8_t nunchuck_bufb2[6]; // array to store nunchuck data,
static uint8_t nunchuck_bufb3[10]; // array to store nunchuck data,
static uint8_t nunchuck_bufb4[10]; // array to store nunchuck data,
static uint8_t nunchuck_bufb5[10]; // array to store nunchuck data,
int joy_x_axisa;
int joy_y_axisa;
int accel_x_axisa;
int accel_y_axisa;
int accel_z_axisa;
int z_buttona;
int c_buttona;
int joy_x_axisb;
int joy_y_axisb;
int accel_x_axisb;
int accel_y_axisb;
int accel_z_axisb;
int z_buttonb;
int c_buttonb;
Qui ho definito quasi tutte le variabili che mi occorrono. Gli array serviranno per tenere traccia delle letture precedenti, in modo da poter farne una media ed avere dei dati più stabili (soprattutto per gli accelerometri)
static void nunchuck_init()
{
i2c.beginTransmission(0x52);// transmit to device 0x52
i2c.send(0x40);// sends memory address
i2c.send(0x00);// sends sent a zero.
i2c.endTransmission();// stop transmitting
i2d.beginTransmission(0x52);// transmit to device 0x52
i2d.send(0x40);// sends memory address
i2d.send(0x00);// sends sent a zero.
i2d.endTransmission();// stop transmitting
}
Qui ho inizializzato i due canali di trasmissione TWI
static void nunchuck_send_request(byte n)
{
if(n==1){
i2c.beginTransmission(0x52);// transmit to device 0x52
i2c.send(0x00);// sends one byte
i2c.endTransmission();// stop transmitting
}else{
i2d.beginTransmission(0x52);// transmit to device 0x52
i2d.send(0x00);// sends one byte
i2d.endTransmission();// stop transmitting
}
}
Qui richiedo, uno alla volta, i dati ai nunchuck
static int nunchuck_get_data(byte n)
{
if(n==1){
nunchuck_send_request(1); // send request for next data payload
i2c.requestFrom (0x52); // request data from nunchuck
// receive byte as an integer
nunchuck_bufa[0] = i2c.receive();
nunchuck_bufa[1] = i2c.receive();
nunchuck_bufa[2] = i2c.receive();
nunchuck_bufa[3] = i2c.receive();
nunchuck_bufa[4] = i2c.receive();
nunchuck_bufa[5] = i2c.receiveLast();
}else{
nunchuck_send_request(2); // send request for next data payload
i2d.requestFrom (0x52); // request data from nunchuck
// receive byte as an integer
nunchuck_bufb[0] = i2d.receive();
nunchuck_bufb[1] = i2d.receive();
nunchuck_bufb[2] = i2d.receive();
nunchuck_bufb[3] = i2d.receive();
nunchuck_bufb[4] = i2d.receive();
nunchuck_bufb[5] = i2d.receiveLast();
}
}
Ricevo i dati richiesti. Ogni nunckuck restituisce una decina di byte ma solo i primi sei byte sono significativi. I primi due riportano le coordinate x-y del joystick, i tre successivi gli otto bit più significativi dei tre assi dell'accelerometro. Il sesto byte riporta le tre coppie di bit meno significative degli accelerometri e due bit per i due pulsanti. Con il metodo .receiveLast() scarto automaticamente tutti i byte ancora in coda nel buffer, senza informazioni e che mi darebbero fastidio per la lettura successiva.
static void nunchuck_process_data(byte n)
{
static int i=0;
if(n==1){
joy_x_axisa = nunchuck_bufa[0];
joy_y_axisa = nunchuck_bufa[1];
accel_x_axisa = nunchuck_bufa[2]; // * 2 * 2;
accel_y_axisa = nunchuck_bufa[3]; // * 2 * 2;
accel_z_axisa = nunchuck_bufa[4]; // * 2 * 2;
Estraggo le informazioni dai primi 5 byte. Notare che ignoro i due bit meno significativi degli accelerometri, per il mio scopo non mi occorre una grande precisione.
z_buttona = 0;
c_buttona = 0;
// byte nunchuck_bufa[5] contains bits for z and c buttons
switch (nunchuck_bufa[5] & 3) {
case 0:
z_buttona = 1;
c_buttona = 1;
break;
case 1:
z_buttona = 0;
c_buttona = 1;
break;
case 2:
z_buttona = 1;
c_buttona = 0;
break;
case 3:
z_buttona = 0;
c_buttona = 0;
break;
}
Estraggo dagli ultimi due bit del sesto byte le informazioni sui due pulsanti.
// eseguo la media delle ultime 6 o 10 letture, per rendere più stabili i valori
joy_x_axisa=0;
for (i=0;i<5;i++){
nunchuck_bufa1[i]=nunchuck_bufa1[i+1];
joy_x_axisa = joy_x_axisa + nunchuck_bufa1[i+1];
}
nunchuck_bufa1[5]=nunchuck_bufa[0];
joy_x_axisa = (joy_x_axisa + nunchuck_bufa[0])/6;
joy_y_axisa=0;
for (i=0;i<5;i++){
nunchuck_bufa2[i]=nunchuck_bufa2[i+1];
joy_y_axisa = joy_y_axisa + nunchuck_bufa2[i+1];
}
nunchuck_bufa2[5]=nunchuck_bufa[1];
joy_y_axisa = (joy_y_axisa + nunchuck_bufa[1])/6;
accel_x_axisa=0;
for (i=0;i<9;i++){
nunchuck_bufa3[i]=nunchuck_bufa3[i+1];
accel_x_axisa = accel_x_axisa + nunchuck_bufa3[i+1];
}
nunchuck_bufa3[9]=nunchuck_bufa[2];
accel_x_axisa = (accel_x_axisa + nunchuck_bufa[2])/10;
accel_y_axisa=0;
for (i=0;i<9;i++){
nunchuck_bufa4[i]=nunchuck_bufa4[i+1];
accel_y_axisa = accel_y_axisa + nunchuck_bufa4[i+1];
}
nunchuck_bufa4[9]=nunchuck_bufa[3];
accel_y_axisa = (accel_y_axisa + nunchuck_bufa[3])/10;
accel_z_axisa=0;
for (i=0;i<9;i++){
nunchuck_bufa5[i]=nunchuck_bufa5[i+1];
accel_z_axisa = accel_z_axisa + nunchuck_bufa5[i+1];
}
nunchuck_bufa5[9]=nunchuck_bufa[4];
accel_z_axisa = (accel_z_axisa + nunchuck_bufa[4])/10;
Ho stabilizzato i valori letti facendone una media con quelli precedenti. Per le coordinate x-y del joystick ho usato sei letture, per gli accelerometri ne ho usate 10.
}else{
joy_x_axisb = nunchuck_bufb[0];
joy_y_axisb = nunchuck_bufb[1];
accel_x_axisb = nunchuck_bufb[2]; // * 2 * 2;
accel_y_axisb = nunchuck_bufb[3]; // * 2 * 2;
accel_z_axisb = nunchuck_bufb[4]; // * 2 * 2;
z_buttonb = 0;
c_buttonb = 0;
// byte nunchuck_bufa[5] contains bits for z and c buttons
switch (nunchuck_bufb[5] & 3) {
case 0:
z_buttonb = 1;
c_buttonb = 1;
break;
case 1:
z_buttonb = 0;
c_buttonb = 1;
break;
case 2:
z_buttonb = 1;
c_buttonb = 0;
break;
case 3:
z_buttonb = 0;
c_buttonb = 0;
break;
}
// eseguo la media delle ultime 6 o 10 letture, per rendere più stabili i valori
joy_x_axisb=0;
for (i=0;i<5;i++){
nunchuck_bufb1[i]=nunchuck_bufb1[i+1];
joy_x_axisb = joy_x_axisb + nunchuck_bufb1[i+1];
}
nunchuck_bufb1[5]=nunchuck_bufb[0];
joy_x_axisb = (joy_x_axisb + nunchuck_bufb[0])/6;
joy_y_axisb=0;
for (i=0;i<5;i++){
nunchuck_bufb2[i]=nunchuck_bufb2[i+1];
joy_y_axisb = joy_y_axisb + nunchuck_bufb2[i+1];
}
nunchuck_bufb2[5]=nunchuck_bufb[1];
joy_y_axisb = (joy_y_axisb + nunchuck_bufb[1])/6;
accel_x_axisb=0;
for (i=0;i<9;i++){
nunchuck_bufb3[i]=nunchuck_bufb3[i+1];
accel_x_axisb = accel_x_axisb + nunchuck_bufb3[i+1];
}
nunchuck_bufb3[9]=nunchuck_bufb[2];
accel_x_axisb = (accel_x_axisb + nunchuck_bufb[2])/10;
accel_y_axisb=0;
for (i=0;i<9;i++){
nunchuck_bufb4[i]=nunchuck_bufb4[i+1];
accel_y_axisb = accel_y_axisb + nunchuck_bufb4[i+1];
}
nunchuck_bufb4[9]=nunchuck_bufb[3];
accel_y_axisb = (accel_y_axisb + nunchuck_bufb[3])/10;
accel_z_axisb=0;
for (i=0;i<9;i++){
nunchuck_bufb5[i]=nunchuck_bufb5[i+1];
accel_z_axisb = accel_z_axisb + nunchuck_bufb5[i+1];
}
nunchuck_bufb5[9]=nunchuck_bufb[4];
accel_z_axisb = (accel_z_axisb + nunchuck_bufb[4])/10;
}
Idem come sopra ma per il secondo nunchuck
/* // parte necessaria per sfruttare anche i due bit degli accelerometri presenti nel sesto byte. L'ho trovata in rete, non l'ho testata e non la uso in questo progetto..
if ((nunchuck_bufa[5] >> 2) & 1)
accel_x_axis += 2;
if ((nunchuck_bufa[5] >> 3) & 1)
accel_x_axis += 1;
if ((nunchuck_bufa[5] >> 4) & 1)
accel_y_axis += 2;
if ((nunchuck_bufa[5] >> 5) & 1)
accel_y_axis += 1;
if ((nunchuck_bufa[5] >> 6) & 1)
accel_z_axis += 2;
if ((nunchuck_bufa[5] >> 7) & 1)
accel_z_axis += 1;
*/
}
static void nunchuck_print_data()
{
// sets the servo position according to the scaled value
val = joy_x_axisa ; // reads the value of the potentiometer (value between 0 and 1023)
val = map(val, 0, 255, 0, 180); // scale it to use it with the servo (value between 0 and 180)
myservo.write(val);
// mostro sullo schermo LCD i dati ottenuti dal nunchuck
lcd.setCursor(0, 0);
lcd.print(joy_x_axisa);
lcd.print(",");
lcd.print(joy_y_axisa);
lcd.print(" c=");
lcd.print(c_buttona);
lcd.print(" z=");
lcd.print(z_buttona);
lcd.print(" ");
lcd.setCursor(0, 1);
lcd.print(joy_x_axisb);
lcd.print(",");
lcd.print(joy_y_axisb);
lcd.print(" c=");
lcd.print(c_buttonb);
lcd.print(" z=");
lcd.print(z_buttonb);
lcd.print(" ");
delay(1000) ;
lcd.setCursor(0, 0);
lcd.print("x=");
lcd.print(accel_x_axisa);
lcd.print(" y=");
lcd.print(accel_y_axisa);
lcd.print(" z=");
lcd.print(accel_z_axisa);
lcd.print(" ");
lcd.setCursor(0, 1);
lcd.print("x=");
lcd.print(accel_x_axisb);
lcd.print(" y=");
lcd.print(accel_y_axisb);
lcd.print(" z=");
lcd.print(accel_z_axisb);
lcd.print(" ");
delay(1000);
}
Questa routine mi occorre a scopo di debug (non ho un serial monitor), per il corretto funzionamento del programma va disabilitata o vanno ridotti a 10ms i due delay.
void setup() {
nunchuck_init();
lcd.clear();
lcd.begin(16, 2);
myservo.attach(45);
}
void loop() {
if( loop_cnt > 100 ) { // every 100 cicli LOOP legge i dati
loop_cnt = 0;
nunchuck_get_data(1);
nunchuck_process_data(1);
nunchuck_get_data(2);
nunchuck_process_data(2);
nunchuck_print_data();
}
loop_cnt++;
} |