nunchuk.cpp (6793B)
1 /* 2 * Copyright (c) 2020, Paco Esteban <paco@e1e0.net> 3 * Copyright (c) 2016, Robert Eisele <robert@xarg.org> 4 * 5 * Permission is hereby granted, free of charge, to any person obtaining a copy 6 * of this software and associated documentation files (the "Software"), to deal 7 * in the Software without restriction, including without limitation the rights 8 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 * copies of the Software, and to permit persons to whom the Software is 10 * furnished to do so, subject to the following conditions: 11 * 12 * The above copyright notice and this permission notice shall be included in 13 * all copies or substantial portions of the Software. 14 * 15 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 * SOFTWARE. 22 */ 23 24 #include <Wire.h> 25 #include "nunchuk.h" 26 27 /* 28 * Constructor accets values for the "zero" point of the accelerometers and the 29 * joystick. This is a calibration that should be done for every Nunchuk, so 30 * better to have it configurable on class creation 31 */ 32 Nunchuk::Nunchuk(uint16_t axz, uint16_t ayz, uint16_t azz, 33 uint16_t jxz, uint16_t jyz) { 34 _accelX_zero = axz; 35 _accelY_zero = ayz; 36 _accelZ_zero = azz; 37 _joystickX_zero = jxz; 38 _joystickY_zero = jyz; 39 } 40 41 void 42 Nunchuk::init() { 43 #ifdef NUNCHUK_DISABLE_ENCRYPTION 44 Wire.beginTransmission(NUNCHUK_ADDRESS); 45 Wire.write(0xF0); 46 Wire.write(0x55); 47 Wire.endTransmission(true); 48 49 Wire.beginTransmission(NUNCHUK_ADDRESS); 50 Wire.write(0xFB); 51 Wire.write(0x00); 52 Wire.endTransmission(true); 53 #else 54 Wire.beginTransmission(NUNCHUK_ADDRESS); 55 Wire.write(0x40); 56 Wire.write(0x00); 57 Wire.endTransmission(true); 58 #endif 59 60 #ifdef NUNCHUK_DEBUG 61 /* 62 * 0xA4200000 for Nunchuck, 0xA4200101 for Classic, 63 * 0xA4200402 for Balance 64 */ 65 Serial.print("Ident: "); 66 67 Wire.beginTransmission(NUNCHUK_ADDRESS); 68 Wire.write(0xFA); 69 Wire.endTransmission(true); 70 71 Wire.requestFrom(NUNCHUK_ADDRESS, 6); 72 for (uint8_t i = 0; i < 6; i++) { 73 if (Wire.available()) { 74 Serial.print(Wire.read(), HEX); 75 Serial.print(" "); 76 } 77 } 78 Wire.endTransmission(true); 79 Serial.println(""); 80 81 delay(100); /* Wait for serial transfer, before loop()ing */ 82 #endif 83 } 84 85 /* 86 * Decodes a byte if encryption is used 87 * 88 * @param x The byte to be decoded 89 */ 90 uint8_t 91 Nunchuk::decode_byte(uint8_t x) { 92 #ifdef NUNCHUK_DISABLE_ENCRYPTION 93 return x; 94 #else 95 return (x ^ 0x17) + 0x17; 96 #endif 97 } 98 99 /* 100 * Central function to read a full chunk of data from Nunchuk 101 * 102 * @return A boolean if the data transfer was successful 103 * 104 * Returned data is organized this way: 105 * Byte 0 --> Stick X position 106 * Byte 1 --> Stick Y position 107 * Byte 2 --> Accel X MSB 108 * Byte 3 --> Accel Y MSB 109 * Byte 4 --> Accel Z MSB 110 * Byte 5 --> | AccelZ LSB | AccelY LSB | AccelX LSB | Button C | Button Z | 111 * | 1 | 0 | 1 | 0 | 1 | 0 | 0 | 0 | 112 * (Button C and Z are "active low") 113 */ 114 uint8_t 115 Nunchuk::read() { 116 uint8_t i; 117 Wire.beginTransmission(NUNCHUK_ADDRESS); 118 Wire.write(0x00); 119 120 Wire.requestFrom(NUNCHUK_ADDRESS, 6); 121 for (i = 0; i < 6 && Wire.available(); i++) { 122 _nunchuk_data[i] = decode_byte(Wire.read()); 123 } 124 125 Wire.endTransmission(true); 126 return i == 6; 127 } 128 129 uint8_t 130 Nunchuk::buttonZ() { 131 return (~_nunchuk_data[5] & 0x01); 132 } 133 134 uint8_t 135 Nunchuk::buttonC() { 136 return (~(_nunchuk_data[5] >> 1) & 0x01); 137 } 138 139 uint8_t 140 Nunchuk::joystickX_raw() { 141 return _nunchuk_data[0]; 142 } 143 144 uint8_t 145 Nunchuk::joystickY_raw() { 146 return _nunchuk_data[1]; 147 } 148 149 /* 150 * Retrieves the calibrated X-value of the joystick 151 */ 152 int16_t 153 Nunchuk::joystickX() { 154 return (int16_t)joystickX_raw() 155 - (int16_t)_joystickX_zero; 156 } 157 158 /* 159 * Retrieves the calibrated Y-value of the joystick 160 */ 161 int16_t 162 Nunchuk::joystickY() { 163 return (int16_t)joystickY_raw() 164 - (int16_t)_joystickY_zero; 165 } 166 167 /* 168 * Calculates the angle of the joystick 169 */ 170 float 171 Nunchuk::joystick_angle() { 172 return atan2((float)joystickY(), (float)joystickX()); 173 } 174 175 /* 176 * Retrieves the raw X-value of the accelerometer 177 */ 178 uint16_t 179 Nunchuk::accelX_raw() { 180 return ((uint16_t)_nunchuk_data[2] << 2) 181 | ((_nunchuk_data[5] >> 2) & 0x03); 182 } 183 184 /* 185 * Retrieves the raw Y-value of the accelerometer 186 */ 187 uint16_t 188 Nunchuk::accelY_raw() { 189 return ((uint16_t)_nunchuk_data[3] << 2) 190 | ((_nunchuk_data[5] >> 4) & 0x03); 191 } 192 193 /* 194 * Retrieves the raw Z-value of the accelerometer 195 */ 196 uint16_t 197 Nunchuk::accelZ_raw() { 198 return ((uint16_t)_nunchuk_data[4] << 2) 199 | ((_nunchuk_data[5] >> 6) & 0x03); 200 } 201 202 /* 203 * Retrieves the calibrated X-value of the accelerometer 204 */ 205 int16_t 206 Nunchuk::accelX() { 207 return (int16_t)accelX_raw() - (int16_t)_accelX_zero; 208 } 209 210 /* 211 * Retrieves the calibrated Y-value of the accelerometer 212 */ 213 int16_t 214 Nunchuk::accelY() { 215 return (int16_t)accelY_raw() - (int16_t)_accelY_zero; 216 } 217 218 /* 219 * Retrieves the calibrated Z-value of the accelerometer 220 */ 221 int16_t 222 Nunchuk::accelZ() { 223 return (int16_t)accelZ_raw() - (int16_t)_accelZ_zero; 224 } 225 226 /* 227 * Calculates the pitch angle THETA around y-axis of the Nunchuk in radians 228 */ 229 float 230 Nunchuk::pitch() { /* tilt-y */ 231 return atan2((float)accelY(), (float)accelZ()); 232 } 233 234 /* 235 * Calculates the roll angle PHI around x-axis of the Nunchuk in radians 236 */ 237 float 238 Nunchuk::roll() { /* tilt-x */ 239 return atan2((float)accelX(), (float)accelZ()); 240 } 241 242 /* 243 * A handy function to print either verbose information of the Nunchuk or a CSV 244 * stream for Processing 245 */ 246 void 247 Nunchuk::print() { 248 #ifdef NUNCHUK_DEBUG 249 char buf[100]; 250 251 sprintf(buf, 252 "Joy X=%4d(%4d) Y=%4d(%4d) Ang %3.2f " 253 "Acc X=%4d(%4d) Y=%4d(%4d) Z=%4d(%4d) " 254 "(P=%3.2f R=%3.2f) " 255 "Btn Z=%1d C=%1d ", 256 joystickX(), joystickX_raw(), 257 joystickY(), joystickY_raw(), 258 (joystick_angle()*180)/3.1416, 259 accelX(), accelX_raw(), 260 accelY(), accelY_raw(), 261 accelZ_raw(), accelZ_raw(), 262 (pitch()*180)/3.1416, (roll()*180)/3.1416, 263 buttonZ(), buttonC()); 264 Serial.print(buf); 265 Serial.println(); 266 #else 267 Serial.print(joystickX(), DEC); 268 Serial.print(","); 269 Serial.print(joystickY(), DEC); 270 Serial.print(","); 271 Serial.print(joystick_angle(), DEC); 272 Serial.print(","); 273 Serial.print(accelX(), DEC); 274 Serial.print(","); 275 Serial.print(accelY(), DEC); 276 Serial.print(","); 277 Serial.print(accelZ(), DEC); 278 Serial.print(","); 279 Serial.print(pitch(), DEC); 280 Serial.print(","); 281 Serial.print(roll(), DEC); 282 Serial.print(","); 283 Serial.print(buttonZ(), DEC); 284 Serial.print(","); 285 Serial.print(buttonC(), DEC); 286 Serial.println(); 287 #endif 288 } 289