Wii.h

Go to the documentation of this file.

/* 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"

#define WIICAMERA

/* Bluetooth L2CAP states for L2CAP_task() */
#define L2CAP_WAIT                      0

// These states are used if the Wiimote is the host
#define L2CAP_CONTROL_SUCCESS           1
#define L2CAP_INTERRUPT_SETUP           2

// These states are used if the Arduino is the host
#define L2CAP_CONTROL_CONNECT_REQUEST   3
#define L2CAP_CONTROL_CONFIG_REQUEST    4
#define L2CAP_INTERRUPT_CONNECT_REQUEST 5

#define L2CAP_INTERRUPT_CONFIG_REQUEST  6

#define L2CAP_CHECK_MOTION_PLUS_STATE   7
#define L2CAP_CHECK_EXTENSION_STATE     8
#define L2CAP_INIT_MOTION_PLUS_STATE    9
#define L2CAP_LED_STATE                 10
#define L2CAP_DONE                      11

#define L2CAP_INTERRUPT_DISCONNECT      12
#define L2CAP_CONTROL_DISCONNECT        13

/* L2CAP event flags */
#define L2CAP_FLAG_CONTROL_CONNECTED                0x001
#define L2CAP_FLAG_INTERRUPT_CONNECTED              0x002
#define L2CAP_FLAG_CONFIG_CONTROL_SUCCESS           0x004
#define L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS         0x008
#define L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE      0x040
#define L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE    0x080
#define L2CAP_FLAG_CONNECTION_CONTROL_REQUEST       0x100
#define L2CAP_FLAG_CONNECTION_INTERRUPT_REQUEST     0x200

/* Macros for L2CAP event flag tests */
#define l2cap_connected_control_flag (l2cap_event_flag & L2CAP_FLAG_CONTROL_CONNECTED)
#define l2cap_connected_interrupt_flag (l2cap_event_flag & L2CAP_FLAG_INTERRUPT_CONNECTED)
#define l2cap_config_success_control_flag (l2cap_event_flag & L2CAP_FLAG_CONFIG_CONTROL_SUCCESS)
#define l2cap_config_success_interrupt_flag (l2cap_event_flag & L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS)
#define l2cap_disconnect_response_control_flag (l2cap_event_flag & L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE)
#define l2cap_disconnect_response_interrupt_flag (l2cap_event_flag & L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE)
#define l2cap_connection_request_control_flag (l2cap_event_flag & L2CAP_FLAG_CONNECTION_CONTROL_REQUEST)
#define l2cap_connection_request_interrupt_flag (l2cap_event_flag & L2CAP_FLAG_CONNECTION_INTERRUPT_REQUEST)

/* Wii event flags */
#define WII_FLAG_MOTION_PLUS_CONNECTED              0x400
#define WII_FLAG_NUNCHUCK_CONNECTED                 0x800

#define motion_plus_connected_flag (l2cap_event_flag & WII_FLAG_MOTION_PLUS_CONNECTED)
#define nunchuck_connected_flag (l2cap_event_flag & WII_FLAG_NUNCHUCK_CONNECTED)

#define PAIR    1

enum Hat {
        HatX = 0,
        HatY = 1,
};

class WII : public BluetoothService {
public:
        WII(BTD *p, bool pair = false);

        virtual void ACLData(uint8_t* ACLData);
        virtual void Run();
        virtual void Reset();
        virtual void disconnect();
        bool getButtonPress(Button b);
        bool getButtonClick(Button b);
        void pair(void) {
                if(pBtd)
                        pBtd->pairWithWiimote();
        }
        uint8_t getAnalogHat(Hat a);
        uint16_t getAnalogHat(AnalogHat a);

        double getPitch() {
                if (motionPlusConnected)
                        return compPitch;
                return getWiimotePitch();
        };

        double getRoll() {
                if (motionPlusConnected)
                        return compRoll;
                return getWiimoteRoll();
        };

        double getYaw() {
                return gyroYaw;
        };

        void setAllOff();
        void setRumbleOff();
        void setRumbleOn();
        void setRumbleToggle();

        void setLedRaw(uint8_t value);
        void setLedOff(LED a);
        void setLedOn(LED a);
        void setLedToggle(LED a);
        void setLedStatus();

        uint8_t getBatteryLevel();
        uint8_t getWiiState() {
                return wiiState;
        };

        void attachOnInit(void (*funcOnInit)(void)) {
                pFuncOnInit = funcOnInit;
        };
        bool wiimoteConnected;
        bool nunchuckConnected;
        bool motionPlusConnected;
        bool wiiUProControllerConnected;
        /* IMU Data, might be usefull if you need to do something more advanced than just calculating the angle */

        double getWiimotePitch() {
                return (atan2(accYwiimote, accZwiimote) + PI) * RAD_TO_DEG;
        };
        double getWiimoteRoll() {
                return (atan2(accXwiimote, accZwiimote) + PI) * RAD_TO_DEG;
        };
        double getNunchuckPitch() {
                return (atan2(accYnunchuck, accZnunchuck) + PI) * RAD_TO_DEG;
        };
        double getNunchuckRoll() {
                return (atan2(accXnunchuck, accZnunchuck) + PI) * RAD_TO_DEG;
        };
        int16_t accXwiimote, accYwiimote, accZwiimote;
        int16_t accXnunchuck, accYnunchuck, accZnunchuck;
        /* Variables for the gyro inside the Motion Plus */
        double gyroPitch;
        double gyroRoll;
        double gyroYaw;

        double pitchGyroSpeed;
        double rollGyroSpeed;
        double yawGyroSpeed;
        uint16_t pitchGyroScale;
        uint16_t rollGyroScale;
        uint16_t yawGyroScale;
        int16_t gyroYawRaw;
        int16_t gyroRollRaw;
        int16_t gyroPitchRaw;
        int16_t gyroYawZero;
        int16_t gyroRollZero;
        int16_t gyroPitchZero;
#ifdef WIICAMERA

        void IRinitialize();

        uint16_t getIRx1() {
                return IR_object_x1;
        };

        uint16_t getIRy1() {
                return IR_object_y1;
        };

        uint8_t getIRs1() {
                return IR_object_s1;
        };

        uint16_t getIRx2() {
                return IR_object_x2;
        };

        uint16_t getIRy2() {
                return IR_object_y2;
        };

        uint8_t getIRs2() {
                return IR_object_s2;
        };

        uint16_t getIRx3() {
                return IR_object_x3;
        };

        uint16_t getIRy3() {
                return IR_object_y3;
        };

        uint8_t getIRs3() {
                return IR_object_s3;
        };

        uint16_t getIRx4() {
                return IR_object_x4;
        };

        uint16_t getIRy4() {
                return IR_object_y4;
        };

        uint8_t getIRs4() {
                return IR_object_s4;
        };

        bool isIRCameraEnabled() {
                return(wiiState & 0x08);
        };
#endif

private:
        BTD *pBtd; // Pointer to BTD instance

        void onInit();
        void (*pFuncOnInit)(void); // Pointer to function called in onInit()

        void L2CAP_task(); // L2CAP state machine

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

        /* Variables used by high level L2CAP task */
        uint8_t l2cap_state;
        uint16_t l2cap_event_flag; // l2cap flags of received Bluetooth events

        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 checkExtension; // Set to false when getBatteryLevel() is called otherwise if should be true
        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
        uint8_t identifier; // Identifier for connection

        /* 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 checkMotionPresent(); // Used to see if a Motion Plus is connected to the Wiimote
        void initMotionPlus();
        void activateMotionPlus();

        double compPitch; // Fusioned angle using a complimentary filter if the Motion Plus is connected
        double 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
        unsigned long 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