USB_Host_Shield_2.0/Wii.h

/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.

 This software may be distributed and modified under the terms of the GNU
 General Public License version 2 (GPL2) as published by the Free Software
 Foundation and appearing in the file GPL2.TXT included in the packaging of
 this file. Please note that GPL2 Section 2[b] requires that all works based
 on this software must also be made publicly available under the terms of
 the GPL2 ("Copyleft").

 Contact information
 -------------------

 Kristian Lauszus, TKJ Electronics
 Web      :  http://www.tkjelectronics.com
 e-mail   :  kristianl@tkjelectronics.com

 IR camera support added by Allan Glover (adglover9.81@gmail.com) and Kristian Lauszus
 */

#ifndef _wii_h_
#define _wii_h_

#include "BTD.h"
#include "controllerEnums.h"

/* Wii event flags */
#define WII_FLAG_MOTION_PLUS_CONNECTED          (1 << 0)
#define WII_FLAG_NUNCHUCK_CONNECTED             (1 << 1)
#define WII_FLAG_CALIBRATE_BALANCE_BOARD        (1 << 2)

#define wii_check_flag(flag)  (wii_event_flag & (flag))
#define wii_set_flag(flag)  (wii_event_flag |= (flag))
#define wii_clear_flag(flag)  (wii_event_flag &= ~(flag))

/** Enum used to read the joystick on the Nunchuck. */
enum HatEnum {
        /** Read the x-axis on the Nunchuck joystick. */
        HatX = 0,
        /** Read the y-axis on the Nunchuck joystick. */
        HatY = 1,
};

/** Enum used to read the weight on Wii Balance Board. */
enum BalanceBoardEnum {
        TopRight = 0,
        BotRight = 1,
        TopLeft = 2,
        BotLeft = 3,
};

/**
 * This BluetoothService class implements support for the Wiimote including the Nunchuck and Motion Plus extension.
 *
 * It also support the Wii U Pro Controller.
 */
class WII : public BluetoothService {
public:
        /**
         * Constructor for the WII class.
         * @param  p   Pointer to BTD class instance.
         * @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.
         * One can use ::PAIR to set it to true.
         */
        WII(BTD *p, bool pair = false);

        /** @name BluetoothService implementation */
        /** Used this to disconnect any of the controllers. */
        void disconnect();
        /**@}*/

        /** @name Wii Controller functions */
        /**
         * getButtonPress(Button b) will return true as long as the button is held down.
         *
         * While getButtonClick(Button b) will only return it once.
         *
         * So you instance if you need to increase a variable once you would use getButtonClick(Button b),
         * but if you need to drive a robot forward you would use getButtonPress(Button b).
         * @param  b          ::ButtonEnum to read.
         * @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.
         */
        bool getButtonPress(ButtonEnum b);
        bool getButtonClick(ButtonEnum b);
        /**@}*/

        /** @name Wii Controller functions */

        /** Call this to start the pairing sequence with a controller */
        void pair(void) {
                if(pBtd)
                        pBtd->pairWithWiimote();
        };
        /**
         * Used to read the joystick of the Nunchuck.
         * @param  a Either ::HatX or ::HatY.
         * @return   Return the analog value in the range from approximately 25-230.
         */
        uint8_t getAnalogHat(HatEnum a);
        /**
         * Used to read the joystick of the Wii U Pro Controller.
         * @param  a Either ::LeftHatX, ::LeftHatY, ::RightHatX or ::RightHatY.
         * @return   Return the analog value in the range from approximately 800-3200.
         */
        uint16_t getAnalogHat(AnalogHatEnum a);

        /**
         * Pitch calculated from the Wiimote. A complimentary filter is used if the Motion Plus is connected.
         * @return Pitch in the range from 0-360.
         */
        float getPitch() {
                if(motionPlusConnected)
                        return compPitch;
                return getWiimotePitch();
        };

        /**
         * Roll calculated from the Wiimote. A complimentary filter is used if the Motion Plus is connected.
         * @return Roll in the range from 0-360.
         */
        float getRoll() {
                if(motionPlusConnected)
                        return compRoll;
                return getWiimoteRoll();
        };

        /**
         * This is the yaw calculated by the gyro.
         *
         * <B>NOTE:</B> This angle will drift a lot and is only available if the Motion Plus extension is connected.
         * @return The angle calculated using the gyro.
         */
        float getYaw() {
                return gyroYaw;
        };

        /** Used to set all LEDs and rumble off. */
        void setAllOff();
        /** Turn off rumble. */
        void setRumbleOff();
        /** Turn on rumble. */
        void setRumbleOn();
        /** Toggle rumble. */
        void setRumbleToggle();

        /**
         * Set LED value without using the ::LEDEnum.
         * @param value See: ::LEDEnum.
         */
        void setLedRaw(uint8_t value);

        /** Turn all LEDs off. */
        void setLedOff() {
                setLedRaw(0);
        };
        /**
         * Turn the specific ::LEDEnum off.
         * @param a The ::LEDEnum to turn off.
         */
        void setLedOff(LEDEnum a);
        /**
         * Turn the specific ::LEDEnum on.
         * @param a The ::LEDEnum to turn on.
         */
        void setLedOn(LEDEnum a);
        /**
         * Toggle the specific ::LEDEnum.
         * @param a The ::LEDEnum to toggle.
         */
        void setLedToggle(LEDEnum a);
        /**
         * This will set the LEDs, so the user can see which connections are active.
         *
         * The first ::LEDEnum indicate that the Wiimote is connected,
         * the second ::LEDEnum indicate indicate that a Motion Plus is also connected
         * the third ::LEDEnum will indicate that a Nunchuck controller is also connected.
         */
        void setLedStatus();

        /**
         * Return the battery level of the Wiimote.
         * @return The battery level in the range 0-255.
         */
        uint8_t getBatteryLevel();

        /**
         * Return the Wiimote state.
         * @return See: http://wiibrew.org/wiki/Wiimote#0x20:_Status.
         */
        uint8_t getWiiState() {
                return wiiState;
        };
        /**@}*/

        /**@{*/
        /** Variable used to indicate if a Wiimote is connected. */
        bool wiimoteConnected;
        /** Variable used to indicate if a Nunchuck controller is connected. */
        bool nunchuckConnected;
        /** Variable used to indicate if a Nunchuck controller is connected. */
        bool motionPlusConnected;
        /** Variable used to indicate if a Wii U Pro controller is connected. */
        bool wiiUProControllerConnected;
        /** Variable used to indicate if a Wii Balance Board is connected. */
        bool wiiBalanceBoardConnected;
        /**@}*/

        /* IMU Data, might be usefull if you need to do something more advanced than just calculating the angle */

        /**@{*/

        /** Pitch and roll calculated from the accelerometer inside the Wiimote. */
        float getWiimotePitch() {
                return (atan2f(accYwiimote, accZwiimote) + PI) * RAD_TO_DEG;
        };

        float getWiimoteRoll() {
                return (atan2f(accXwiimote, accZwiimote) + PI) * RAD_TO_DEG;
        };
        /**@}*/

        /**@{*/

        /** Pitch and roll calculated from the accelerometer inside the Nunchuck. */
        float getNunchuckPitch() {
                return (atan2f(accYnunchuck, accZnunchuck) + PI) * RAD_TO_DEG;
        };

        float getNunchuckRoll() {
                return (atan2f(accXnunchuck, accZnunchuck) + PI) * RAD_TO_DEG;
        };
        /**@}*/

        /**@{*/
        /** Accelerometer values used to calculate pitch and roll. */
        int16_t accXwiimote, accYwiimote, accZwiimote;
        int16_t accXnunchuck, accYnunchuck, accZnunchuck;
        /**@}*/

        /* Variables for the gyro inside the Motion Plus */
        /** This is the pitch calculated by the gyro - use this to tune WII#pitchGyroScale. */
        float gyroPitch;
        /** This is the roll calculated by the gyro - use this to tune WII#rollGyroScale. */
        float gyroRoll;
        /** This is the yaw calculated by the gyro - use this to tune WII#yawGyroScale. */
        float gyroYaw;

        /**@{*/
        /** The speed in deg/s from the gyro. */
        float pitchGyroSpeed;
        float rollGyroSpeed;
        float yawGyroSpeed;
        /**@}*/

        /**@{*/
        /** You might need to fine-tune these values. */
        uint16_t pitchGyroScale;
        uint16_t rollGyroScale;
        uint16_t yawGyroScale;
        /**@}*/

        /**@{*/
        /** Raw value read directly from the Motion Plus. */
        int16_t gyroYawRaw;
        int16_t gyroRollRaw;
        int16_t gyroPitchRaw;
        /**@}*/

        /**@{*/
        /** These values are set when the controller is first initialized. */
        int16_t gyroYawZero;
        int16_t gyroRollZero;
        int16_t gyroPitchZero;
        /**@}*/

        /** @name Wii Balance Board functions */

        /**
         * Used to get the weight at the specific position on the Wii Balance Board.
         * @param  pos ::BalanceBoardEnum to read from.
         * @return     Returns the weight in kg.
         */
        float getWeight(BalanceBoardEnum pos);

        /**
         * Used to get total weight on the Wii Balance Board.
         * @return Returns the weight in kg.
         */
        float getTotalWeight();

        /**
         * Used to get the raw reading at the specific position on the Wii Balance Board.
         * @param  pos ::BalanceBoardEnum to read from.
         * @return     Returns the raw reading.
         */
        uint16_t getWeightRaw(BalanceBoardEnum pos) {
                return wiiBalanceBoardRaw[pos];
        };
        /**@}*/

#ifdef WIICAMERA
        /** @name Wiimote IR camera functions
         * You will have to set ::ENABLE_WII_IR_CAMERA in settings.h to 1 in order use the IR camera.
         */
        /** Initialises the camera as per the steps from: http://wiibrew.org/wiki/Wiimote#IR_Camera */
        void IRinitialize();

        /**
         * IR object 1 x-position read from the Wii IR camera.
         * @return The x-position of the object in the range 0-1023.
         */
        uint16_t getIRx1() {
                return IR_object_x1;
        };

        /**
         * IR object 1 y-position read from the Wii IR camera.
         * @return The y-position of the object in the range 0-767.
         */
        uint16_t getIRy1() {
                return IR_object_y1;
        };

        /**
         * IR object 1 size read from the Wii IR camera.
         * @return The size of the object in the range 0-15.
         */
        uint8_t getIRs1() {
                return IR_object_s1;
        };

        /**
         * IR object 2 x-position read from the Wii IR camera.
         * @return The x-position of the object in the range 0-1023.
         */
        uint16_t getIRx2() {
                return IR_object_x2;
        };

        /**
         * IR object 2 y-position read from the Wii IR camera.
         * @return The y-position of the object in the range 0-767.
         */
        uint16_t getIRy2() {
                return IR_object_y2;
        };

        /**
         * IR object 2 size read from the Wii IR camera.
         * @return The size of the object in the range 0-15.
         */
        uint8_t getIRs2() {
                return IR_object_s2;
        };

        /**
         * IR object 3 x-position read from the Wii IR camera.
         * @return The x-position of the object in the range 0-1023.
         */
        uint16_t getIRx3() {
                return IR_object_x3;
        };

        /**
         * IR object 3 y-position read from the Wii IR camera.
         * @return The y-position of the object in the range 0-767.
         */
        uint16_t getIRy3() {
                return IR_object_y3;
        };

        /**
         * IR object 3 size read from the Wii IR camera.
         * @return The size of the object in the range 0-15.
         */
        uint8_t getIRs3() {
                return IR_object_s3;
        };

        /**
         * IR object 4 x-position read from the Wii IR camera.
         * @return The x-position of the object in the range 0-1023.
         */
        uint16_t getIRx4() {
                return IR_object_x4;
        };

        /**
         * IR object 4 y-position read from the Wii IR camera.
         * @return The y-position of the object in the range 0-767.
         */
        uint16_t getIRy4() {
                return IR_object_y4;
        };

        /**
         * IR object 4 size read from the Wii IR camera.
         * @return The size of the object in the range 0-15.
         */
        uint8_t getIRs4() {
                return IR_object_s4;
        };

        /**
         * Use this to check if the camera is enabled or not.
         * If not call WII#IRinitialize to initialize the IR camera.
         * @return     True if it's enabled, false if not.
         */
        bool isIRCameraEnabled() {
                return (wiiState & 0x08);
        };
        /**@}*/
#endif

protected:
        /** @name BluetoothService implementation */
        /**
         * Used to pass acldata to the services.
         * @param ACLData Incoming acldata.
         */
        void ACLData(uint8_t* ACLData);
        /** Used to run part of the state machine. */
        void Run();
        /** Use this to reset the service. */
        void Reset();
        /**
         * Called when the controller is successfully initialized.
         * Use attachOnInit(void (*funcOnInit)(void)) to call your own function.
         * This is useful for instance if you want to set the LEDs in a specific way.
         */
        void onInit();
        /**@}*/

private:

        void L2CAP_task(); // L2CAP state machine

        /* Variables filled from HCI event management */
        bool activeConnection; // Used to indicate if it's already has established a connection

        /* Variables used by high level L2CAP task */
        uint8_t l2cap_state;
        uint8_t wii_event_flag; // Used for Wii flags

        uint32_t ButtonState;
        uint32_t OldButtonState;
        uint32_t ButtonClickState;
        uint16_t hatValues[4];

        uint8_t HIDBuffer[3]; // Used to store HID commands

        uint16_t stateCounter;
        bool unknownExtensionConnected;
        bool extensionConnected;
        bool checkBatteryLevel; // Set to true when getBatteryLevel() is called otherwise if should be false
        bool motionPlusInside; // True if it's a new Wiimote with the Motion Plus extension build into it

        /* L2CAP Channels */
        uint8_t control_scid[2]; // L2CAP source CID for HID_Control
        uint8_t control_dcid[2]; // 0x0060
        uint8_t interrupt_scid[2]; // L2CAP source CID for HID_Interrupt
        uint8_t interrupt_dcid[2]; // 0x0061

        /* HID Commands */
        void HID_Command(uint8_t* data, uint8_t nbytes);
        void setReportMode(bool continuous, uint8_t mode);

        void writeData(uint32_t offset, uint8_t size, uint8_t* data);
        void initExtension1();
        void initExtension2();

        void statusRequest(); // Used to update the Wiimote state and battery level

        void readData(uint32_t offset, uint16_t size, bool EEPROM);
        void readExtensionType();
        void readCalData();
        void readWiiBalanceBoardCalibration(); // Used by the library to read the Wii Balance Board calibration values

        void checkMotionPresent(); // Used to see if a Motion Plus is connected to the Wiimote
        void initMotionPlus();
        void activateMotionPlus();

        uint16_t wiiBalanceBoardRaw[4]; // Wii Balance Board raw values
        uint16_t wiiBalanceBoardCal[3][4]; // Wii Balance Board calibration values

        float compPitch; // Fusioned angle using a complimentary filter if the Motion Plus is connected
        float compRoll; // Fusioned angle using a complimentary filter if the Motion Plus is connected

        bool activateNunchuck;
        bool motionValuesReset; // This bool is true when the gyro values has been reset
        uint32_t timer;

        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)
        uint8_t batteryLevel;

#ifdef WIICAMERA
        /* Private function and variables for the readings from the IR Camera */
        void enableIRCamera1(); // Sets bit 2 of output report 13
        void enableIRCamera2(); // Sets bit 2 of output report 1A
        void writeSensitivityBlock1();
        void writeSensitivityBlock2();
        void write0x08Value();
        void setWiiModeNumber(uint8_t mode_number);

        uint16_t IR_object_x1; // IR x position 10 bits
        uint16_t IR_object_y1; // IR y position 10 bits
        uint8_t IR_object_s1; // IR size value
        uint16_t IR_object_x2;
        uint16_t IR_object_y2;
        uint8_t IR_object_s2;
        uint16_t IR_object_x3; // IR x position 10 bits
        uint16_t IR_object_y3; // IR y position 10 bits
        uint8_t IR_object_s3; // IR size value
        uint16_t IR_object_x4;
        uint16_t IR_object_y4;
        uint8_t IR_object_s4;
#endif
};
#endif