commit 4f2bfec4b178e0854d2764ffd9a4788cb16d702d
Author: Paco Esteban <paco@e1e0.net>
Date: Tue, 29 Sep 2020 17:12:08 +0200
initial commit
Diffstat:
| A | nunchuk.cpp | | | 289 | ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| A | nunchuk.h | | | 73 | +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
2 files changed, 362 insertions(+), 0 deletions(-)
diff --git a/nunchuk.cpp b/nunchuk.cpp
@@ -0,0 +1,289 @@
+/*
+ * Copyright (c) 2020, Paco Esteban <paco@e1e0.net>
+ * Copyright (c) 2016, Robert Eisele <robert@xarg.org>
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <Wire.h>
+#include "nunchuk.h"
+
+/*
+ * Constructor accets values for the "zero" point of the accelerometers and the
+ * joystick. This is a calibration that should be done for every Nunchuk, so
+ * better to have it configurable on class creation
+ */
+Nunchuk::Nunchuk(uint16_t axz, uint16_t ayz, uint16_t azz,
+ uint16_t jxz, uint16_t jyz) {
+ _accelX_zero = axz;
+ _accelY_zero = ayz;
+ _accelZ_zero = azz;
+ _joystickX_zero = jxz;
+ _joystickY_zero = jyz;
+}
+
+void
+Nunchuk::init() {
+#ifdef NUNCHUK_DISABLE_ENCRYPTION
+ Wire.beginTransmission(NUNCHUK_ADDRESS);
+ Wire.write(0xF0);
+ Wire.write(0x55);
+ Wire.endTransmission(true);
+
+ Wire.beginTransmission(NUNCHUK_ADDRESS);
+ Wire.write(0xFB);
+ Wire.write(0x00);
+ Wire.endTransmission(true);
+#else
+ Wire.beginTransmission(NUNCHUK_ADDRESS);
+ Wire.write(0x40);
+ Wire.write(0x00);
+ Wire.endTransmission(true);
+#endif
+
+#ifdef NUNCHUK_DEBUG
+ /*
+ * 0xA4200000 for Nunchuck, 0xA4200101 for Classic,
+ * 0xA4200402 for Balance
+ */
+ Serial.print("Ident: ");
+
+ Wire.beginTransmission(NUNCHUK_ADDRESS);
+ Wire.write(0xFA);
+ Wire.endTransmission(true);
+
+ Wire.requestFrom(NUNCHUK_ADDRESS, 6);
+ for (uint8_t i = 0; i < 6; i++) {
+ if (Wire.available()) {
+ Serial.print(Wire.read(), HEX);
+ Serial.print(" ");
+ }
+ }
+ Wire.endTransmission(true);
+ Serial.println("");
+
+ delay(100); /* Wait for serial transfer, before loop()ing */
+#endif
+}
+
+/*
+ * Decodes a byte if encryption is used
+ *
+ * @param x The byte to be decoded
+ */
+uint8_t
+Nunchuk::decode_byte(uint8_t x) {
+#ifdef NUNCHUK_DISABLE_ENCRYPTION
+ return x;
+#else
+ return (x ^ 0x17) + 0x17;
+#endif
+}
+
+/*
+ * Central function to read a full chunk of data from Nunchuk
+ *
+ * @return A boolean if the data transfer was successful
+ *
+ * Returned data is organized this way:
+ * Byte 0 --> Stick X position
+ * Byte 1 --> Stick Y position
+ * Byte 2 --> Accel X MSB
+ * Byte 3 --> Accel Y MSB
+ * Byte 4 --> Accel Z MSB
+ * Byte 5 --> | AccelZ LSB | AccelY LSB | AccelX LSB | Button C | Button Z |
+ * | 1 | 0 | 1 | 0 | 1 | 0 | 0 | 0 |
+ * (Button C and Z are "active low")
+ */
+uint8_t
+Nunchuk::read() {
+ uint8_t i;
+ Wire.beginTransmission(NUNCHUK_ADDRESS);
+ Wire.write(0x00);
+
+ Wire.requestFrom(NUNCHUK_ADDRESS, 6);
+ for (i = 0; i < 6 && Wire.available(); i++) {
+ _nunchuk_data[i] = decode_byte(Wire.read());
+ }
+
+ Wire.endTransmission(true);
+ return i == 6;
+}
+
+uint8_t
+Nunchuk::buttonZ() {
+ return (~_nunchuk_data[5] & 0x01);
+}
+
+uint8_t
+Nunchuk::buttonC() {
+ return (~(_nunchuk_data[5] >> 1) & 0x01);
+}
+
+uint8_t
+Nunchuk::joystickX_raw() {
+ return _nunchuk_data[0];
+}
+
+uint8_t
+Nunchuk::joystickY_raw() {
+ return _nunchuk_data[1];
+}
+
+/*
+ * Retrieves the calibrated X-value of the joystick
+ */
+int16_t
+Nunchuk::joystickX() {
+ return (int16_t)joystickX_raw()
+ - (int16_t)_joystickX_zero;
+}
+
+/*
+ * Retrieves the calibrated Y-value of the joystick
+ */
+int16_t
+Nunchuk::joystickY() {
+ return (int16_t)joystickY_raw()
+ - (int16_t)_joystickY_zero;
+}
+
+/*
+ * Calculates the angle of the joystick
+ */
+float
+Nunchuk::joystick_angle() {
+ return atan2((float)joystickY(), (float)joystickX());
+}
+
+/*
+ * Retrieves the raw X-value of the accelerometer
+ */
+uint16_t
+Nunchuk::accelX_raw() {
+ return ((uint16_t)_nunchuk_data[2] << 2)
+ | ((_nunchuk_data[5] >> 2) & 0x03);
+}
+
+/*
+ * Retrieves the raw Y-value of the accelerometer
+ */
+uint16_t
+Nunchuk::accelY_raw() {
+ return ((uint16_t)_nunchuk_data[3] << 2)
+ | ((_nunchuk_data[5] >> 4) & 0x03);
+}
+
+/*
+ * Retrieves the raw Z-value of the accelerometer
+ */
+uint16_t
+Nunchuk::accelZ_raw() {
+ return ((uint16_t)_nunchuk_data[4] << 2)
+ | ((_nunchuk_data[5] >> 6) & 0x03);
+}
+
+/*
+ * Retrieves the calibrated X-value of the accelerometer
+ */
+int16_t
+Nunchuk::accelX() {
+ return (int16_t)accelX_raw() - (int16_t)_accelX_zero;
+}
+
+/*
+ * Retrieves the calibrated Y-value of the accelerometer
+ */
+int16_t
+Nunchuk::accelY() {
+ return (int16_t)accelY_raw() - (int16_t)_accelY_zero;
+}
+
+/*
+ * Retrieves the calibrated Z-value of the accelerometer
+ */
+int16_t
+Nunchuk::accelZ() {
+ return (int16_t)accelZ_raw() - (int16_t)_accelZ_zero;
+}
+
+/*
+ * Calculates the pitch angle THETA around y-axis of the Nunchuk in radians
+ */
+float
+Nunchuk::pitch() { /* tilt-y */
+ return atan2((float)accelY(), (float)accelZ());
+}
+
+/*
+ * Calculates the roll angle PHI around x-axis of the Nunchuk in radians
+ */
+float
+Nunchuk::roll() { /* tilt-x */
+ return atan2((float)accelX(), (float)accelZ());
+}
+
+/*
+ * A handy function to print either verbose information of the Nunchuk or a CSV
+ * stream for Processing
+ */
+void
+Nunchuk::print() {
+#ifdef NUNCHUK_DEBUG
+ char buf[100];
+
+ sprintf(buf,
+ "Joy X=%4d(%4d) Y=%4d(%4d) Ang %3.2f "
+ "Acc X=%4d(%4d) Y=%4d(%4d) Z=%4d(%4d) "
+ "(P=%3.2f R=%3.2f) "
+ "Btn Z=%1d C=%1d ",
+ joystickX(), joystickX_raw(),
+ joystickY(), joystickY_raw(),
+ (joystick_angle()*180)/3.1416,
+ accelX(), accelX_raw(),
+ accelY(), accelY_raw(),
+ accelZ_raw(), accelZ_raw(),
+ (pitch()*180)/3.1416, (roll()*180)/3.1416,
+ buttonZ(), buttonC());
+ Serial.print(buf);
+ Serial.println();
+#else
+ Serial.print(joystickX(), DEC);
+ Serial.print(",");
+ Serial.print(joystickY(), DEC);
+ Serial.print(",");
+ Serial.print(joystick_angle(), DEC);
+ Serial.print(",");
+ Serial.print(accelX(), DEC);
+ Serial.print(",");
+ Serial.print(accelY(), DEC);
+ Serial.print(",");
+ Serial.print(accelZ(), DEC);
+ Serial.print(",");
+ Serial.print(pitch(), DEC);
+ Serial.print(",");
+ Serial.print(roll(), DEC);
+ Serial.print(",");
+ Serial.print(buttonZ(), DEC);
+ Serial.print(",");
+ Serial.print(buttonC(), DEC);
+ Serial.println();
+#endif
+}
+
diff --git a/nunchuk.h b/nunchuk.h
@@ -0,0 +1,73 @@
+/*
+ * Copyright (c) 2020, Paco Esteban <paco@e1e0.net>
+ * Copyright (c) 2016, Robert Eisele <robert@xarg.org>
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef NUNCHUK_H
+#define NUNCHUK_H
+
+#include "Arduino.h"
+#include "Print.h"
+
+/*
+ * Whether to disable encryption. Enabling encryption means that every packet
+ * must be decrypted, which wastes cpu cycles. Cheap Nunchuk clones have
+ * problems with the encrypted init sequence, so be sure you know what you're
+ * doing
+ */
+#define NUNCHUK_DISABLE_ENCRYPTION
+
+/* Print debug information instead of a CSV stream to the serial port */
+/* #define NUNCHUK_DEBUG */
+
+/* The Nunchuk I2C address */
+#define NUNCHUK_ADDRESS 0x52
+
+class Nunchuk {
+ public:
+ Nunchuk(uint16_t, uint16_t, uint16_t, uint16_t, uint16_t);
+ void print();
+ float roll();
+ float pitch();
+ int16_t accelZ();
+ int16_t accelY();
+ int16_t accelX();
+ float joystick_angle();
+ uint8_t buttonC();
+ uint8_t buttonZ();
+ int16_t joystickY();
+ int16_t joystickX();
+ uint8_t read();
+ void init();
+ private:
+ uint8_t _nunchuk_data[6];
+ uint16_t _accelX_zero, _accelY_zero, _accelZ_zero;
+ uint16_t _joystickX_zero, _joystickY_zero;
+
+ uint8_t decode_byte(uint8_t);
+ uint16_t accelZ_raw();
+ uint16_t accelY_raw();
+ uint16_t accelX_raw();
+ uint8_t joystickY_raw();
+ uint8_t joystickX_raw();
+};
+
+#endif
