Diff: gyroReader.cpp

Revision:

8:c23cd84befa2

Parent:

7:3b2cf7efe5d1

Child:

12:925f52da3ba9

--- a/gyroReader.cpp	Fri Nov 22 18:00:18 2013 +0000
+++ b/gyroReader.cpp	Fri Nov 22 21:28:52 2013 +0000
@@ -1,22 +1,19 @@
  #include "gyroReader.h"
- //#include "math.h"
  
  //keep track of our heading
  void gyroReader::gyroUpkeep(){
-    //testPin=1;
     int xdiff,ydiff,zdiff;
     checkGyro(xdiff,ydiff,zdiff);
     //checkCompass();
     gyroZcount+=zdiff+zcal;
     gyroXcount+=xdiff+xcal;
     polls++;
-    //testPin=0;
 }
     
 //starts up gyro, accel and magnetometer
 int gyroReader::startAccel(){
     char cmd[4];
-    char addr=0x32;//0b0011000;
+    //char addr=0x32;//0b0011000;
     char gyroaddr=0xD6;
     char magaddr=0x3C;
     cmd[0]=0x20;
@@ -72,7 +69,7 @@
     //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 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];
@@ -86,7 +83,7 @@
     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]);
+    //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]);
@@ -103,27 +100,9 @@
     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;
 }
@@ -137,8 +116,6 @@
     polls=0;
     startAccel();
     xmag=ymag=zmag=0;
-    //testPin=0;
-    //upkeepTimer.start(10);
     gyroUpkeepTicker.attach_us(this,&gyroReader::gyroUpkeep,10000);
 }
 void gyroReader::setLevel(){