LAB

Dependencies:   mbed

Fork of LAB11_Oppgave2 by EL-POM1001

main.cpp

Committer:
knut_johan
Date:
2015-12-08
Revision:
2:132db1b8730d
Parent:
1:d4624c8ba9f5

File content as of revision 2:132db1b8730d:

#include "mbed.h"
//SDA , SCL
I2C i2cBus(PB_9, PB_8);

Serial pc(SERIAL_TX, SERIAL_RX);
DigitalOut LED(PA_5);
PwmOut      servoMotor(PA_7);
DigitalOut  direction(PA_6);

#define NUNCHUK_8BIT_ADDR 0x52<<1  //   8-bit I2C slave address  = (  ( 7bit i2c address) <<  1  |  (read/write) bit 

typedef struct {
    int xStick;
    int yStick;
    int xAcc;
    int yAcc;
    int zAcc;
    bool bButtonC;
    bool bButtonZ;
} NunchukData;

void initNunchuk()
{
    i2cBus.frequency(100000);

    wait(0.5);

    char cmd[2];
    cmd[0] = 0x40;
    cmd[1] = 0x00;
    i2cBus.write(NUNCHUK_8BIT_ADDR, cmd, 2) ;

    cmd[0] = 0xF0;
    cmd[1] = 0x55;
    i2cBus.write(NUNCHUK_8BIT_ADDR, cmd, 2) ;

    cmd[0] = 0xFB;
    cmd[1] = 0x00;
    i2cBus.write(NUNCHUK_8BIT_ADDR, cmd, 2) ;
}

void GetNunchukData(NunchukData &data)
{
    char buffer[6];

    char cmd[1];
    cmd[0] = 0x00;
    i2cBus.write(NUNCHUK_8BIT_ADDR, cmd, 1) ;

    wait(.01);
    i2cBus.read(NUNCHUK_8BIT_ADDR, buffer, 6) ;

    data.xStick   = buffer[0];
    data.yStick   = buffer[1];
    data.xAcc     = (buffer[2] << 2) + ((buffer[5] & 0x0C) >> 2);
    data.yAcc     = (buffer[3] << 2) + ((buffer[5] & 0x30) >> 4);
    data.zAcc     = (buffer[4] << 2) + ((buffer[5] & 0xC0) >> 6);
    data.bButtonZ = (buffer[5] & 1) == 0;
    data.bButtonC = (buffer[5] & 2) == 0;
}

int main()
{
    NunchukData nunchukData;

    initNunchuk();
    direction=0;
    while(true) {
        LED = 0;
        if(nunchukData.bButtonC == 1) {
            LED = 1;
        }
        GetNunchukData(nunchukData);
        printf("x-yoy %3d,y-yoy %3d ",nunchukData.xStick,nunchukData.yStick );
        printf("X-acc: %3d, Y-acc: %3d, Z-acc: %3d, knapp Z: %d, knapp C: %d",nunchukData.xAcc,nunchukData.yAcc, nunchukData.zAcc,nunchukData.bButtonZ, nunchukData.bButtonC );
        printf("\r\n");

        if(nunchukData.xStick<139) {
            servoMotor=nunchukData.xStick/139.0f;
            direction=1;
        } else if(nunchukData.xStick>139) {
            servoMotor=nunchukData.xStick/139.0f;
            direction=0;
        }

        if(nunchukData.bButtonZ==1) 
        {
            if(nunchukData.xAcc<600) 
            {
                servoMotor=nunchukData.xAcc/600.0f;
                direction=0;
            } 
            if(nunchukData.xAcc>600); 
            {
                servoMotor=nunchukData.xAcc/600.0f;
                direction=!direction;
        }
     }

    }
}