accelerometer og akse
Dependencies: mbed
Fork of LAB11_Oppgave2 by
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); } } } }