1 /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.
3 This software may be distributed and modified under the terms of the GNU
4 General Public License version 2 (GPL2) as published by the Free Software
5 Foundation and appearing in the file GPL2.TXT included in the packaging of
6 this file. Please note that GPL2 Section 2[b] requires that all works based
7 on this software must also be made publicly available under the terms of
13 Kristian Lauszus, TKJ Electronics
14 Web : http://www.tkjelectronics.com
15 e-mail : kristianl@tkjelectronics.com
17 IR camera support added by Allan Glover (adglover9.81@gmail.com) and Kristian Lauszus
24 #include "controllerEnums.h"
27 #define WII_FLAG_MOTION_PLUS_CONNECTED (1 << 0)
28 #define WII_FLAG_NUNCHUCK_CONNECTED (1 << 1)
29 #define WII_FLAG_CALIBRATE_BALANCE_BOARD (1 << 2)
31 #define wii_check_flag(flag) (wii_event_flag & (flag))
32 #define wii_set_flag(flag) (wii_event_flag |= (flag))
33 #define wii_clear_flag(flag) (wii_event_flag &= ~(flag))
35 /** Enum used to read the joystick on the Nunchuck. */
37 /** Read the x-axis on the Nunchuck joystick. */
39 /** Read the y-axis on the Nunchuck joystick. */
43 /** Enum used to read the weight on Wii Balance Board. */
44 enum BalanceBoardEnum {
52 * This BluetoothService class implements support for the Wiimote including the Nunchuck and Motion Plus extension.
54 * It also support the Wii U Pro Controller.
56 class WII : public BluetoothService {
59 * Constructor for the WII class.
60 * @param p Pointer to BTD class instance.
61 * @param pair Set this to true in order to pair with the Wiimote. If the argument is omitted then it won't pair with it.
62 * One can use ::PAIR to set it to true.
64 WII(BTD *p, bool pair = false);
66 /** @name BluetoothService implementation */
67 /** Used this to disconnect any of the controllers. */
71 /** @name Wii Controller functions */
73 * getButtonPress(Button b) will return true as long as the button is held down.
75 * While getButtonClick(Button b) will only return it once.
77 * So you instance if you need to increase a variable once you would use getButtonClick(Button b),
78 * but if you need to drive a robot forward you would use getButtonPress(Button b).
79 * @param b ::ButtonEnum to read.
80 * @return getButtonPress(ButtonEnum b) will return a true as long as a button is held down, while getButtonClick(ButtonEnum b) will return true once for each button press.
82 bool getButtonPress(ButtonEnum b);
83 bool getButtonClick(ButtonEnum b);
86 /** @name Wii Controller functions */
88 /** Call this to start the paring sequence with a controller */
91 pBtd->pairWithWiimote();
94 * Used to read the joystick of the Nunchuck.
95 * @param a Either ::HatX or ::HatY.
96 * @return Return the analog value in the range from approximately 25-230.
98 uint8_t getAnalogHat(HatEnum a);
100 * Used to read the joystick of the Wii U Pro Controller.
101 * @param a Either ::LeftHatX, ::LeftHatY, ::RightHatX or ::RightHatY.
102 * @return Return the analog value in the range from approximately 800-3200.
104 uint16_t getAnalogHat(AnalogHatEnum a);
107 * Pitch calculated from the Wiimote. A complimentary filter is used if the Motion Plus is connected.
108 * @return Pitch in the range from 0-360.
111 if(motionPlusConnected)
113 return getWiimotePitch();
117 * Roll calculated from the Wiimote. A complimentary filter is used if the Motion Plus is connected.
118 * @return Roll in the range from 0-360.
121 if(motionPlusConnected)
123 return getWiimoteRoll();
127 * This is the yaw calculated by the gyro.
129 * <B>NOTE:</B> This angle will drift a lot and is only available if the Motion Plus extension is connected.
130 * @return The angle calculated using the gyro.
136 /** Used to set all LEDs and rumble off. */
138 /** Turn off rumble. */
140 /** Turn on rumble. */
142 /** Toggle rumble. */
143 void setRumbleToggle();
146 * Set LED value without using the ::LEDEnum.
147 * @param value See: ::LEDEnum.
149 void setLedRaw(uint8_t value);
151 /** Turn all LEDs off. */
156 * Turn the specific ::LEDEnum off.
157 * @param a The ::LEDEnum to turn off.
159 void setLedOff(LEDEnum a);
161 * Turn the specific ::LEDEnum on.
162 * @param a The ::LEDEnum to turn on.
164 void setLedOn(LEDEnum a);
166 * Toggle the specific ::LEDEnum.
167 * @param a The ::LEDEnum to toggle.
169 void setLedToggle(LEDEnum a);
171 * This will set the LEDs, so the user can see which connections are active.
173 * The first ::LEDEnum indicate that the Wiimote is connected,
174 * the second ::LEDEnum indicate indicate that a Motion Plus is also connected
175 * the third ::LEDEnum will indicate that a Nunchuck controller is also connected.
180 * Return the battery level of the Wiimote.
181 * @return The battery level in the range 0-255.
183 uint8_t getBatteryLevel();
186 * Return the Wiimote state.
187 * @return See: http://wiibrew.org/wiki/Wiimote#0x20:_Status.
189 uint8_t getWiiState() {
195 /** Variable used to indicate if a Wiimote is connected. */
196 bool wiimoteConnected;
197 /** Variable used to indicate if a Nunchuck controller is connected. */
198 bool nunchuckConnected;
199 /** Variable used to indicate if a Nunchuck controller is connected. */
200 bool motionPlusConnected;
201 /** Variable used to indicate if a Wii U Pro controller is connected. */
202 bool wiiUProControllerConnected;
203 /** Variable used to indicate if a Wii Balance Board is connected. */
204 bool wiiBalanceBoardConnected;
207 /* IMU Data, might be usefull if you need to do something more advanced than just calculating the angle */
211 /** Pitch and roll calculated from the accelerometer inside the Wiimote. */
212 double getWiimotePitch() {
213 return (atan2(accYwiimote, accZwiimote) + PI) * RAD_TO_DEG;
216 double getWiimoteRoll() {
217 return (atan2(accXwiimote, accZwiimote) + PI) * RAD_TO_DEG;
223 /** Pitch and roll calculated from the accelerometer inside the Nunchuck. */
224 double getNunchuckPitch() {
225 return (atan2(accYnunchuck, accZnunchuck) + PI) * RAD_TO_DEG;
228 double getNunchuckRoll() {
229 return (atan2(accXnunchuck, accZnunchuck) + PI) * RAD_TO_DEG;
234 /** Accelerometer values used to calculate pitch and roll. */
235 int16_t accXwiimote, accYwiimote, accZwiimote;
236 int16_t accXnunchuck, accYnunchuck, accZnunchuck;
239 /* Variables for the gyro inside the Motion Plus */
240 /** This is the pitch calculated by the gyro - use this to tune WII#pitchGyroScale. */
242 /** This is the roll calculated by the gyro - use this to tune WII#rollGyroScale. */
244 /** This is the yaw calculated by the gyro - use this to tune WII#yawGyroScale. */
248 /** The speed in deg/s from the gyro. */
249 double pitchGyroSpeed;
250 double rollGyroSpeed;
255 /** You might need to fine-tune these values. */
256 uint16_t pitchGyroScale;
257 uint16_t rollGyroScale;
258 uint16_t yawGyroScale;
262 /** Raw value read directly from the Motion Plus. */
265 int16_t gyroPitchRaw;
269 /** These values are set when the controller is first initialized. */
271 int16_t gyroRollZero;
272 int16_t gyroPitchZero;
275 /** @name Wii Balance Board functions */
278 * Used to get the weight at the specific position on the Wii Balance Board.
279 * @param ::BalanceBoardEnum to read from.
280 * @return Returns the weight in kg.
282 float getWeight(BalanceBoardEnum pos);
285 * Used to get total weight on the Wii Balance Board.
286 * @returnReturns the weight in kg.
288 float getTotalWeight();
291 * Used to get the raw reading at the specific position on the Wii Balance Board.
292 * @param ::BalanceBoardEnum to read from.
293 * @return Returns the raw reading.
295 uint16_t getWeightRaw(BalanceBoardEnum pos) {
296 return wiiBalanceBoardRaw[pos];
301 /** @name Wiimote IR camera functions
302 * You will have to set ::ENABLE_WII_IR_CAMERA in settings.h to 1 in order use the IR camera.
304 /** Initialises the camera as per the steps from: http://wiibrew.org/wiki/Wiimote#IR_Camera */
308 * IR object 1 x-position read from the Wii IR camera.
309 * @return The x-position of the object in the range 0-1023.
316 * IR object 1 y-position read from the Wii IR camera.
317 * @return The y-position of the object in the range 0-767.
324 * IR object 1 size read from the Wii IR camera.
325 * @return The size of the object in the range 0-15.
332 * IR object 2 x-position read from the Wii IR camera.
333 * @return The x-position of the object in the range 0-1023.
340 * IR object 2 y-position read from the Wii IR camera.
341 * @return The y-position of the object in the range 0-767.
348 * IR object 2 size read from the Wii IR camera.
349 * @return The size of the object in the range 0-15.
356 * IR object 3 x-position read from the Wii IR camera.
357 * @return The x-position of the object in the range 0-1023.
364 * IR object 3 y-position read from the Wii IR camera.
365 * @return The y-position of the object in the range 0-767.
372 * IR object 3 size read from the Wii IR camera.
373 * @return The size of the object in the range 0-15.
380 * IR object 4 x-position read from the Wii IR camera.
381 * @return The x-position of the object in the range 0-1023.
388 * IR object 4 y-position read from the Wii IR camera.
389 * @return The y-position of the object in the range 0-767.
396 * IR object 4 size read from the Wii IR camera.
397 * @return The size of the object in the range 0-15.
404 * Use this to check if the camera is enabled or not.
405 * If not call WII#IRinitialize to initialize the IR camera.
406 * @return True if it's enabled, false if not.
408 bool isIRCameraEnabled() {
409 return (wiiState & 0x08);
415 /** @name BluetoothService implementation */
417 * Used to pass acldata to the services.
418 * @param ACLData Incoming acldata.
420 void ACLData(uint8_t* ACLData);
421 /** Used to run part of the state machine. */
423 /** Use this to reset the service. */
426 * Called when the controller is successfully initialized.
427 * Use attachOnInit(void (*funcOnInit)(void)) to call your own function.
428 * This is useful for instance if you want to set the LEDs in a specific way.
435 void L2CAP_task(); // L2CAP state machine
437 /* Variables filled from HCI event management */
438 bool activeConnection; // Used to indicate if it's already has established a connection
440 /* Variables used by high level L2CAP task */
442 uint8_t wii_event_flag; // Used for Wii flags
444 uint32_t ButtonState;
445 uint32_t OldButtonState;
446 uint32_t ButtonClickState;
447 uint16_t hatValues[4];
449 uint8_t HIDBuffer[3]; // Used to store HID commands
451 uint16_t stateCounter;
452 bool unknownExtensionConnected;
453 bool extensionConnected;
454 bool checkBatteryLevel; // Set to true when getBatteryLevel() is called otherwise if should be false
455 bool motionPlusInside; // True if it's a new Wiimote with the Motion Plus extension build into it
458 uint8_t control_scid[2]; // L2CAP source CID for HID_Control
459 uint8_t control_dcid[2]; // 0x0060
460 uint8_t interrupt_scid[2]; // L2CAP source CID for HID_Interrupt
461 uint8_t interrupt_dcid[2]; // 0x0061
464 void HID_Command(uint8_t* data, uint8_t nbytes);
465 void setReportMode(bool continuous, uint8_t mode);
467 void writeData(uint32_t offset, uint8_t size, uint8_t* data);
468 void initExtension1();
469 void initExtension2();
471 void statusRequest(); // Used to update the Wiimote state and battery level
473 void readData(uint32_t offset, uint16_t size, bool EEPROM);
474 void readExtensionType();
476 void readWiiBalanceBoardCalibration(); // Used by the library to read the Wii Balance Board calibration values
478 void checkMotionPresent(); // Used to see if a Motion Plus is connected to the Wiimote
479 void initMotionPlus();
480 void activateMotionPlus();
482 uint16_t wiiBalanceBoardRaw[4]; // Wii Balance Board raw values
483 uint16_t wiiBalanceBoardCal[3][4]; // Wii Balance Board calibration values
485 double compPitch; // Fusioned angle using a complimentary filter if the Motion Plus is connected
486 double compRoll; // Fusioned angle using a complimentary filter if the Motion Plus is connected
488 bool activateNunchuck;
489 bool motionValuesReset; // This bool is true when the gyro values has been reset
492 uint8_t wiiState; // Stores the value in l2capinbuf[12] - (0x01: Battery is nearly empty), (0x02: An Extension Controller is connected), (0x04: Speaker enabled), (0x08: IR enabled), (0x10: LED1, 0x20: LED2, 0x40: LED3, 0x80: LED4)
493 uint8_t batteryLevel;
496 /* Private function and variables for the readings from the IR Camera */
497 void enableIRCamera1(); // Sets bit 2 of output report 13
498 void enableIRCamera2(); // Sets bit 2 of output report 1A
499 void writeSensitivityBlock1();
500 void writeSensitivityBlock2();
501 void write0x08Value();
502 void setWiiModeNumber(uint8_t mode_number);
504 uint16_t IR_object_x1; // IR x position 10 bits
505 uint16_t IR_object_y1; // IR y position 10 bits
506 uint8_t IR_object_s1; // IR size value
507 uint16_t IR_object_x2;
508 uint16_t IR_object_y2;
509 uint8_t IR_object_s2;
510 uint16_t IR_object_x3; // IR x position 10 bits
511 uint16_t IR_object_y3; // IR y position 10 bits
512 uint8_t IR_object_s3; // IR size value
513 uint16_t IR_object_x4;
514 uint16_t IR_object_y4;
515 uint8_t IR_object_s4;