El-POM1001 / Mbed 2 deprecated LAB11_Oppgave2

accelerometer og akse

Dependencies:   mbed

Fork of LAB11_Oppgave2 by EL-POM1001

main.cpp@2:b3bd4f835893, 2015-11-12 (annotated)

Committer:
sinx
Date:
Thu Nov 12 11:10:45 2015 +0000
Revision:
2:b3bd4f835893
Parent:
1:d4624c8ba9f5 
ferdig

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rlanghbv 0:87a84e072ec7 1 #include "mbed.h"
rlanghbv 0:87a84e072ec7 2 //SDA , SCL
rlanghbv 0:87a84e072ec7 3 I2C i2cBus(PB_9, PB_8);
rlanghbv 0:87a84e072ec7 4
rlanghbv 0:87a84e072ec7 5 Serial pc(SERIAL_TX, SERIAL_RX);
sinx 2:b3bd4f835893 6 DigitalOut led(PA_5);
sinx 2:b3bd4f835893 7
sinx 2:b3bd4f835893 8 PwmOut servoMotor(PA_7);
sinx 2:b3bd4f835893 9 DigitalOut direction(PA_6);
rlanghbv 0:87a84e072ec7 10
rlanghbv 0:87a84e072ec7 11 #define NUNCHUK_8BIT_ADDR 0x52<<1 // 8-bit I2C slave address = ( ( 7bit i2c address) << 1 | (read/write) bit
rlanghbv 0:87a84e072ec7 12
rlanghbv 0:87a84e072ec7 13 typedef struct {
rlanghbv 0:87a84e072ec7 14 int xStick;
rlanghbv 0:87a84e072ec7 15 int yStick;
rlanghbv 0:87a84e072ec7 16 int xAcc;
rlanghbv 0:87a84e072ec7 17 int yAcc;
rlanghbv 0:87a84e072ec7 18 int zAcc;
rlanghbv 0:87a84e072ec7 19 bool bButtonC;
rlanghbv 0:87a84e072ec7 20 bool bButtonZ;
rlanghbv 0:87a84e072ec7 21 } NunchukData;
rlanghbv 0:87a84e072ec7 22
rlanghbv 0:87a84e072ec7 23 void initNunchuk()
rlanghbv 0:87a84e072ec7 24 {
rlanghbv 0:87a84e072ec7 25 i2cBus.frequency(100000);
rlanghbv 0:87a84e072ec7 26
rlanghbv 0:87a84e072ec7 27 wait(0.5);
rlanghbv 0:87a84e072ec7 28
rlanghbv 0:87a84e072ec7 29 char cmd[2];
rlanghbv 0:87a84e072ec7 30 cmd[0] = 0x40;
rlanghbv 0:87a84e072ec7 31 cmd[1] = 0x00;
rlanghbv 0:87a84e072ec7 32 i2cBus.write(NUNCHUK_8BIT_ADDR, cmd, 2) ;
rlanghbv 0:87a84e072ec7 33
rlanghbv 0:87a84e072ec7 34 cmd[0] = 0xF0;
rlanghbv 0:87a84e072ec7 35 cmd[1] = 0x55;
rlanghbv 0:87a84e072ec7 36 i2cBus.write(NUNCHUK_8BIT_ADDR, cmd, 2) ;
rlanghbv 0:87a84e072ec7 37
rlanghbv 0:87a84e072ec7 38 cmd[0] = 0xFB;
rlanghbv 0:87a84e072ec7 39 cmd[1] = 0x00;
rlanghbv 0:87a84e072ec7 40 i2cBus.write(NUNCHUK_8BIT_ADDR, cmd, 2) ;
rlanghbv 0:87a84e072ec7 41 }
rlanghbv 0:87a84e072ec7 42
rlanghbv 0:87a84e072ec7 43 void GetNunchukData(NunchukData &data)
rlanghbv 0:87a84e072ec7 44 {
rlanghbv 0:87a84e072ec7 45 char buffer[6];
rlanghbv 0:87a84e072ec7 46
rlanghbv 0:87a84e072ec7 47 char cmd[1];
rlanghbv 0:87a84e072ec7 48 cmd[0] = 0x00;
rlanghbv 0:87a84e072ec7 49 i2cBus.write(NUNCHUK_8BIT_ADDR, cmd, 1) ;
rlanghbv 0:87a84e072ec7 50
rlanghbv 0:87a84e072ec7 51 wait(.01);
rlanghbv 0:87a84e072ec7 52 i2cBus.read(NUNCHUK_8BIT_ADDR, buffer, 6) ;
rlanghbv 0:87a84e072ec7 53
rlanghbv 0:87a84e072ec7 54 data.xStick = buffer[0];
rlanghbv 0:87a84e072ec7 55 data.yStick = buffer[1];
rlanghbv 0:87a84e072ec7 56 data.xAcc = (buffer[2] << 2) + ((buffer[5] & 0x0C) >> 2);
rlanghbv 0:87a84e072ec7 57 data.yAcc = (buffer[3] << 2) + ((buffer[5] & 0x30) >> 4);
rlanghbv 0:87a84e072ec7 58 data.zAcc = (buffer[4] << 2) + ((buffer[5] & 0xC0) >> 6);
rlanghbv 0:87a84e072ec7 59 data.bButtonZ = (buffer[5] & 1) == 0;
rlanghbv 0:87a84e072ec7 60 data.bButtonC = (buffer[5] & 2) == 0;
rlanghbv 0:87a84e072ec7 61 }
rlanghbv 0:87a84e072ec7 62
rlanghbv 0:87a84e072ec7 63 int main()
rlanghbv 0:87a84e072ec7 64 {
sinx 2:b3bd4f835893 65 servoMotor.period(.01);
rlanghbv 0:87a84e072ec7 66 NunchukData nunchukData;
rlanghbv 0:87a84e072ec7 67
rlanghbv 0:87a84e072ec7 68 initNunchuk();
rlanghbv 0:87a84e072ec7 69 while(true) {
rlanghbv 0:87a84e072ec7 70 GetNunchukData(nunchukData);
rlanghbv 0:87a84e072ec7 71 printf("x-yoy %3d,y-yoy %3d ",nunchukData.xStick,nunchukData.yStick );
sinx 2:b3bd4f835893 72 printf("x-acc %3d,y-acc %3d,z-acc %3d ",nunchukData.xAcc,nunchukData.yAcc,nunchukData.zAcc);
sinx 2:b3bd4f835893 73 printf("ButtonZ %d,ButtonC %d, ",nunchukData.bButtonZ,nunchukData.bButtonC);
rlanghbv 1:d4624c8ba9f5 74 printf("\r\n");
sinx 2:b3bd4f835893 75 led=nunchukData.bButtonC;
sinx 2:b3bd4f835893 76
sinx 2:b3bd4f835893 77 if( !nunchukData.bButtonZ) {
sinx 2:b3bd4f835893 78 if(nunchukData.xStick<135) {
sinx 2:b3bd4f835893 79 float speed=(135.0f-(float)nunchukData.xStick)/100.0f;
sinx 2:b3bd4f835893 80 printf("%f",speed);
sinx 2:b3bd4f835893 81 direction=0;
sinx 2:b3bd4f835893 82 servoMotor.write(speed); //
sinx 2:b3bd4f835893 83 } else if(nunchukData.xStick>145) {
sinx 2:b3bd4f835893 84 float speed=((float)nunchukData.xStick-145.0f)/100.0f;
sinx 2:b3bd4f835893 85 printf("%f",speed);
sinx 2:b3bd4f835893 86 direction=1;
sinx 2:b3bd4f835893 87 servoMotor.write(1.0f-speed);
sinx 2:b3bd4f835893 88 } else {
sinx 2:b3bd4f835893 89 direction=1;
sinx 2:b3bd4f835893 90 servoMotor.write(1);
sinx 2:b3bd4f835893 91 }
sinx 2:b3bd4f835893 92 }
sinx 2:b3bd4f835893 93
sinx 2:b3bd4f835893 94
sinx 2:b3bd4f835893 95 if( nunchukData.bButtonZ) {
sinx 2:b3bd4f835893 96 if(nunchukData.xAcc<495) {
sinx 2:b3bd4f835893 97 float speed=(495.0f-(float)nunchukData.xAcc)/100.0f;
sinx 2:b3bd4f835893 98 printf("%f",speed);
sinx 2:b3bd4f835893 99 direction=0;
sinx 2:b3bd4f835893 100 servoMotor.write(speed); //
sinx 2:b3bd4f835893 101 } else if(nunchukData.xAcc>505) {
sinx 2:b3bd4f835893 102 float speed=((float)nunchukData.xAcc-505.0f)/100.0f;
sinx 2:b3bd4f835893 103 printf("%f",speed);
sinx 2:b3bd4f835893 104 direction=1;
sinx 2:b3bd4f835893 105 servoMotor.write(1.0f-speed);
sinx 2:b3bd4f835893 106 } else {
sinx 2:b3bd4f835893 107 direction=1;
sinx 2:b3bd4f835893 108 servoMotor.write(1);
sinx 2:b3bd4f835893 109 }
sinx 2:b3bd4f835893 110 }
rlanghbv 0:87a84e072ec7 111 }
rlanghbv 0:87a84e072ec7 112 }
rlanghbv 0:87a84e072ec7 113