accelerometer og akse

Dependencies:   mbed

Fork of LAB11_Oppgave2 by EL-POM1001

main.cpp

Committer:
sinx
Date:
2015-11-12
Revision:
2:b3bd4f835893
Parent:
1:d4624c8ba9f5

File content as of revision 2:b3bd4f835893:

#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()
{
    servoMotor.period(.01);
    NunchukData nunchukData;

    initNunchuk();
    while(true) {
        GetNunchukData(nunchukData);
        printf("x-yoy %3d,y-yoy %3d ",nunchukData.xStick,nunchukData.yStick );
        printf("x-acc %3d,y-acc %3d,z-acc %3d ",nunchukData.xAcc,nunchukData.yAcc,nunchukData.zAcc);
        printf("ButtonZ %d,ButtonC %d, ",nunchukData.bButtonZ,nunchukData.bButtonC);
        printf("\r\n");
        led=nunchukData.bButtonC;

        if( !nunchukData.bButtonZ) {
            if(nunchukData.xStick<135) {
                float speed=(135.0f-(float)nunchukData.xStick)/100.0f;
                printf("%f",speed);
                direction=0;
                servoMotor.write(speed); //
            } else if(nunchukData.xStick>145) {
                float speed=((float)nunchukData.xStick-145.0f)/100.0f;
                printf("%f",speed);
                direction=1;
                servoMotor.write(1.0f-speed);
            } else {
                direction=1;
                servoMotor.write(1);
            }
        }


        if( nunchukData.bButtonZ) {
            if(nunchukData.xAcc<495) {
                float speed=(495.0f-(float)nunchukData.xAcc)/100.0f;
                printf("%f",speed);
                direction=0;
                servoMotor.write(speed); //
            } else if(nunchukData.xAcc>505) {
                float speed=((float)nunchukData.xAcc-505.0f)/100.0f;
                printf("%f",speed);
                direction=1;
                servoMotor.write(1.0f-speed);
            } else {
                direction=1;
                servoMotor.write(1);
            }
        }
    }
}