Dependents:   HoverboardTest RobotArmControl

main.cpp

Committer:
knaka
Date:
2010-11-23
Revision:
1:2fde37da31b9
Parent:
0:bc63bc0629f8

File content as of revision 1:2fde37da31b9:

//
// WiiNunchuck
//  

#include "mbed.h"
#include "TextLCD.h"

// I2C
#define NUNCHUCK_ADDR     0xA4  // 0x52 << 1
#define NUNCHUCK_REGADDR  0x40  // 
#define NUNCHUCK_READLEN  0x06  //
#define I2C_ACK 0
#define I2C_READ_DELAY  0.01

#define Joy_X   0
#define Joy_Y   1
#define Acc_X   2
#define Acc_Y   3
#define Acc_Z   4
#define Button  5

bool WiiNunInitFlag = false;
int joyX = 0, joyY = 0, accX = 0, accY = 0, accZ = 0, buttonC = 0, buttonZ = 0;
char readBuf[NUNCHUCK_READLEN];
I2C WiiNun(p9, p10);

TextLCD lcd(p24, p26, p27, p28, p29, p30);

bool WiiNunInit()
{
    bool result = false; 
    unsigned char cmd[] = {NUNCHUCK_REGADDR, 0x00};    
    if (WiiNun.write(NUNCHUCK_ADDR, (const char*)cmd, sizeof(cmd)) == I2C_ACK) {
        result = true;
    }
    return result;
}

void WiiNunRead()
{
    int i;
    
    if(!WiiNunInitFlag) {
        WiiNunInitFlag = WiiNunInit();
    }
    
    if(WiiNunInitFlag) {
        const unsigned char cmd[] = {0x00};
        if (WiiNun.write(NUNCHUCK_ADDR, (const char*)cmd, sizeof(cmd)) == I2C_ACK) {
            wait(I2C_READ_DELAY);
            if (WiiNun.read(NUNCHUCK_ADDR, readBuf, sizeof(readBuf)) == I2C_ACK) {
                for(i = 0; i < NUNCHUCK_READLEN; ++i) {
                    readBuf[i] = (readBuf[i] ^ 0x17) + 0x17;
                }
                joyX = readBuf[Joy_X];
                joyY = readBuf[Joy_Y];
                accX = readBuf[Acc_X] << 2;
                accY = readBuf[Acc_Y] << 2;
                accZ = readBuf[Acc_Z] << 2;
                
                if(readBuf[Button] & 0x01) {
                    buttonZ = 0;
                } else {
                    buttonZ = 1;
                }
                if(readBuf[Button] & 0x02) {
                    buttonC = 0;
                } else {
                    buttonC = 1;
                }
                if(readBuf[Button] & 0x04) accX += 1;
                if(readBuf[Button] & 0x08) accX += 2;
                if(readBuf[Button] & 0x10) accY += 1;
                if(readBuf[Button] & 0x20) accY += 2;
                if(readBuf[Button] & 0x40) accZ += 1;
                if(readBuf[Button] & 0x80) accZ += 2;
           }
        }
    }
}


int main() {
    while(1) {
        WiiNunRead();
        lcd.cls();
        lcd.printf("x%3d y%3d c%1d z%1d", joyX, joyY, buttonC, buttonZ);
        lcd.locate(0, 1);
        lcd.printf("x%d y%d z%d", accX, accY, accZ); 
        wait(0.1);
    }
}