mirror of
https://github.com/pascallanger/DIY-Multiprotocol-TX-Module.git
synced 2025-07-12 09:47:54 +00:00
115 lines
3.7 KiB
C++
115 lines
3.7 KiB
C++
#ifdef ENABLE_NUNCHUCK
|
|
#include <Wire.h>
|
|
|
|
#include <Adafruit_NeoPixel.h>
|
|
Adafruit_NeoPixel strip = Adafruit_NeoPixel(1, 3, NEO_GRB + NEO_KHZ800);
|
|
|
|
// Doit etre ajuste en fonction de chaque nunchuck
|
|
#define ZEROX 530
|
|
#define ZEROY 530
|
|
#define ZEROZ 530
|
|
|
|
#define WII_NUNCHUK_I2C_ADDRESS 0x52 // adresse I2C du nunchuck
|
|
int chan;
|
|
uint8_t tpsc=0, tpsz=0;
|
|
uint8_t data[6]; //nunchuck
|
|
boolean c=false, z=false, cl=false, zl=false, op=false;
|
|
|
|
uint8_t batteryTX = 255;
|
|
|
|
|
|
void nunchuck_init() {
|
|
analogReference(INTERNAL);
|
|
pinMode(BUZZER_PIN, OUTPUT); digitalWrite(BUZZER_PIN, BUZZER_INIT);
|
|
|
|
|
|
/** Interrupt routines **/
|
|
TCCR2A = 0b00000010; // COM 2A1, 2A0, 2B1, 2B0, - , - , WGM21, WGM20
|
|
TCCR2B = 0b00000110; // Clock / 256 soit 16 micro-s et WGM22 = 0 // FOC2A, FOC2B, - , - , WGM22, CS22, CS21, CS20
|
|
TIMSK2 = 0b00000010; // Interruption locale autorisée par OCIE2A // - , - , - , - , - , OCIE2B, OCIE2A, TOIE2
|
|
OCR2A = 250; // Interruption du compte à ...
|
|
sei(); // autorisation générale des interruptions
|
|
|
|
|
|
strip.begin();
|
|
strip.setPixelColor(0, color[0], color[0+1], color[0+2]);
|
|
strip.setBrightness(200);
|
|
strip.show();
|
|
|
|
Wire.begin();
|
|
|
|
Wire.beginTransmission(WII_NUNCHUK_I2C_ADDRESS);
|
|
Wire.write(0xF0);
|
|
Wire.write(0x55);
|
|
Wire.endTransmission();
|
|
|
|
Wire.beginTransmission(WII_NUNCHUK_I2C_ADDRESS);
|
|
Wire.write(0xFB);
|
|
Wire.write(0x00);
|
|
Wire.endTransmission();
|
|
nunchuck_read();
|
|
nunchuck_read();
|
|
}
|
|
|
|
void nunchuck_read() {
|
|
// on demande 6 octets au nunchuck
|
|
Wire.requestFrom(WII_NUNCHUK_I2C_ADDRESS, 6);
|
|
|
|
chan = 0;
|
|
// tant qu'il y a des donnees
|
|
while(Wire.available()) { data[chan++] = Wire.read(); } // on recupere les donnees
|
|
|
|
// on reinitialise le nunchuck pour la prochaine demande
|
|
Wire.beginTransmission(WII_NUNCHUK_I2C_ADDRESS);
|
|
Wire.write(0x00);
|
|
Wire.endTransmission();
|
|
}
|
|
void nunchuck_update() {
|
|
nunchuck_read();
|
|
if(chan >= 5) {
|
|
// on extrait les donnees
|
|
// dans mon exemple j'utilise uniquement les donnees d'acceleration sur l'axe Y
|
|
int JX = data[0];
|
|
int JY = data[1];
|
|
// on limite la valeur entre -180 et 180 puis reechantillonnage de 0-255
|
|
double accelX = constrain( ((data[2] << 2) + ((data[5] >> 2) & 0x03) - ZEROX), -180, 180); //droite / gauche
|
|
double accelY = constrain( ((data[3] << 2) + ((data[5] >> 4) & 0x03) - ZEROY), -180, 180); // av / ar
|
|
double accelZ = constrain( ((data[4] << 2) + ((data[5] >> 6) & 0x03) - ZEROZ), -180, 180); // haut / bas
|
|
|
|
// calcul appuis long sur boutton
|
|
if((data[5] >> 0) & 1) { tpsc++; } // compte temps
|
|
else if(tpsc>300) { cl=!cl; tpsc=0; } // RAZ tps + déclaration appuis LONG
|
|
else if(tpsc) { c=!c; tpsc=0; } // RAZ tps + déclaration appuis COURT
|
|
// boolean zButton = !((data[5] >> 0) & 1);
|
|
|
|
if((data[5] >> 1) & 1) { tpsz++; } else if(tpsz>300) { zl=!zl; tpsz=0; } else if(tpsz) { z=!z; tpsz=0; }
|
|
// boolean cButton = !((data[5] >> 1) & 1);
|
|
|
|
Servo_data[THROTTLE] = JY;
|
|
Servo_data[ELEVATOR] = accelY;
|
|
Servo_data[RUDDER] = accelX;
|
|
Servo_data[AILERON] = JX;
|
|
if(!op) {
|
|
Servo_data[AUX1] = z;
|
|
Servo_data[AUX2] = c;
|
|
Servo_data[AUX3] = cl;
|
|
}
|
|
}
|
|
}
|
|
|
|
#define DIVISOR_PERCENTS (32)
|
|
#define PERCENT_TO_BYTE(P) ((int8_t)((((int)P) * DIVISOR_PERCENTS) / 100))
|
|
#define BYTE_TO_PERCENT(P) ((int8_t)((((int)P) * 100) / DIVISOR_PERCENTS))
|
|
int16_t expo(int8_t a, int32_t x) {
|
|
return (((BYTE_TO_PERCENT(a) * x * x) / 100) * x) / (((int32_t)servo_max_125) * servo_max_125)
|
|
+ (100 - BYTE_TO_PERCENT(a)) * x / 100;
|
|
}
|
|
|
|
/** Interrupt routines 4ms **/
|
|
ISR (TIMER2_OVF_vect) {
|
|
batteryTX = map(analogRead(VBAT_PIN), 0,1023, 0,430);
|
|
if(batteryTX < VBAT_LIM) { digitalWrite(BUZZER_PIN, !BUZZER_INIT); }
|
|
else if(batteryTX < VBAT_VAL) { digitalWrite(BUZZER_PIN, !digitalRead(BUZZER_PIN)); }
|
|
nunchuck_update();
|
|
}
|
|
#endif |