Diff: gyroReader.cpp

Revision:

1:c28fac16a109

Child:

3:a223b0bf8256

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/gyroReader.cpp	Sun Nov 17 03:05:35 2013 +0000
@@ -0,0 +1,142 @@
+ #include "gyroReader.h"
+ 
+ void gyroReader::gyroUpkeep(){
+    //testPin=1;
+    int xdiff,ydiff,zdiff;
+    checkGyro(xdiff,ydiff,zdiff);
+    gyroZcount+=zdiff+zcal;
+    gyroXcount+=xdiff+xcal;
+    polls++;
+    //testPin=0;
+}
+    
+int gyroReader::startAccel(){
+    char cmd[4];
+    char addr=0x30;//0b0011000;
+    char gyroaddr=0xD2;
+    char magaddr=0x3C;
+    cmd[0]=0x20;
+    cmd[1]=0x2f;//0b00101111;
+    cmd[2]=0;
+    cmd[3]=0;
+    printf("%d\r\n",accel.write(addr,cmd,2));
+    cmd[1]=0x0f;
+    printf("%d\r\n",accel.write(gyroaddr,cmd,2));
+    printf("%X,%X,%X,%X\r\n",cmd[0],cmd[1],cmd[2],cmd[3]);
+    cmd[0]=0x02;
+    cmd[1]=0x00;//0b00000000
+    //pc.printf("%d\r\n",accel.write(magaddr,cmd,2));
+    DBGPRINT("%d\r\n",accel.write(magaddr,cmd,2));
+    return 1;
+}
+
+float gyroReader::checkAccel(){
+    char com[1];
+    char cmd[6]={0,0,0,0,0,0};
+    char addr=0x30;//0b0011000;
+    com[0]=0x28|0x80;
+    accel.write(addr, com, 1,true);
+    accel.read(addr, cmd, 6);
+    //int x=cmd
+    /*accel.start();
+    accel.write(addr);
+    accel.write(0x28|0x80);
+    accel.start();
+    accel.write(addr|1);
+    cmd[0]=accel.read(1);
+    cmd[1]=accel.read(1);
+    cmd[2]=accel.read(1);
+    cmd[3]=accel.read(0);
+    accel.stop();*/
+    //int yacc=(signed char)cmd[1];
+    //int xacc=(signed char)cmd[3];
+    //int zacc=-(signed char)cmd[5];
+    int xacc=(signed short)((((unsigned char)cmd[1])<<8)+(unsigned char)cmd[0]);
+    int yacc=(signed short)((((unsigned char)cmd[3])<<8)+(unsigned char)cmd[2]);
+    int zacc=(signed short)((((unsigned char)cmd[5])<<8)+(unsigned char)cmd[4]);
+    //int yacc=(signed char)cmd[3];
+    //int zacc=(signed char)cmd[5];
+    float angle=atan(zacc==0?yacc*1000000.0:(float)yacc/(float)zacc)-(zacc>=0?0.0:3.141592);
+    //pc.printf("%X\t%X\t%X\t%d\t%X\t%d\t%f\t%f\r\n",cmd[0],cmd[1],cmd[2],yacc,cmd[4],zacc,angle);
+    //pc.printf("%X\t%X\t%X\t%X\t%X\t%X\r\n",cmd[0],cmd[1],cmd[2],cmd[3],cmd[4],cmd[5]);
+    //pc.printf("%d\t\t%d\t\t%d\r\n",xacc,yacc,zacc);
+    
+    addr=0x3D;//0b0011000;
+    com[0]=0x03|0x80;
+    accel.write(addr, com, 1,true);
+    accel.read(addr, cmd, 6);
+    xacc=(signed short)((((unsigned char)cmd[0])<<8)+(unsigned char)cmd[1]);
+    yacc=(signed short)((((unsigned char)cmd[2])<<8)+(unsigned char)cmd[3]);
+    zacc=(signed short)((((unsigned char)cmd[4])<<8)+(unsigned char)cmd[5]);
+    //pc.printf("%X\t%X\t%X\t%X\t%X\t%X\r\n",cmd[0],cmd[1],cmd[2],cmd[3],cmd[4],cmd[5]);
+    //pc.printf("%d\t\t%d\t\t%d\r\n",xacc,yacc,zacc);
+    
+    return angle;
+}
+
+int gyroReader::checkGyro(int& xacc, int& yacc, int& zacc){
+    char com[1];
+    char cmd[6]={0,0,0,0,0,0};
+    char addr=0xD2;//0b0011000;
+    com[0]=0x28|0x80;
+    accel.write(addr, com, 1,true);
+    accel.read(addr, cmd, 6);
+    //int x=cmd
+    /*accel.start();
+    accel.write(addr);
+    accel.write(0x28|0x80);
+    accel.start();
+    accel.write(addr|1);
+    cmd[0]=accel.read(1);
+    cmd[1]=accel.read(1);
+    cmd[2]=accel.read(1);
+    cmd[3]=accel.read(0);
+    accel.stop();*/
+    //int xacc=((int)cmd[3]<<8)|cmd[2];
+    //int xacc=((int)cmd[1]<<8)|cmd[0];
+    //xacc=-((cmd[1]&0x80)==0)?xacc:-((xacc-1)^0xFFFF);
+    //xacc=-((cmd[3]&0x80)==0)?xacc:-((xacc-1)^0xFFFF);
+    //int yacc=(signed char)cmd[3];
+    //int zacc=(signed char)cmd[5];
+    xacc=(signed short)((((unsigned char)cmd[1])<<8)+(unsigned char)cmd[0]);
+    yacc=(signed short)((((unsigned char)cmd[3])<<8)+(unsigned char)cmd[2]);
+    zacc=(signed short)((((unsigned char)cmd[5])<<8)+(unsigned char)cmd[4]);
+    //float angle=atan(zacc==0?yacc*1000000.0:(float)yacc/(float)zacc)-(zacc>=0?0.1:3.141592);
+    //pc.printf("%d\t%d\t%d\t%d\t%d\t%d\t%f\r\n",cmd[1],xacc,cmd[3],yacc,cmd[5],zacc);
+    return xacc;
+}
+
+gyroReader::gyroReader(PinName pinA, PinName pinB) : accel(pinA,pinB)/*, upkeepTimer(gyroReader::gyroUpkeep, osTimerPeriodic, (void *)0), testPin(test)*/{
+    gyroZcount=0;
+    gyroXcount=0;
+    zcal=42;
+    xcal=16;
+    polls=0;
+    startAccel();
+    //testPin=0;
+    //upkeepTimer.start(10);
+    gyroUpkeepTicker.attach_us(this,&gyroReader::gyroUpkeep,10000);
+}
+void gyroReader::setLevel(){
+    gyroXcount=0;
+}
+
+void gyroReader::resetZ(){
+    gyroZcount=0;
+}
+
+int gyroReader::getZ(){
+    return gyroZcount;
+}
+int gyroReader::getX(){
+    return gyroXcount;
+}
+void gyroReader::reset(){
+    gyroZcount=gyroXcount=polls=0;
+}
+
+void gyroReader::calibrate(){
+    zcal=-(gyroZcount/polls)+zcal;
+    xcal=-(gyroXcount/polls)+xcal;
+    DBGPRINT("cal: %d, %d\r\n",zcal,xcal);
+}
\ No newline at end of file