Sensors.h

00001 
00002 //In here be functions related to Sensors
00003 
00004 //
00005 void setupaccel() {
00006     i2c.start();
00007     i2c.write(ACCADDRESS << 1 | 0);
00008     i2c.write(7);
00009     i2c.write(1);
00010     i2c.stop();
00011 }
00012 
00013 //
00014 void getaccel(signed char returnbuff[3]) {
00015     i2c.start();
00016     i2c.write(ACCADDRESS << 1 | 0);
00017     i2c.write(0);
00018     i2c.stop();
00019 
00020     //Read the axis data
00021     i2c.read(ACCADDRESS << 1 | 1, (char*)returnbuff, 3);
00022 
00023     for (int i = 0; i < 3; i++) {
00024         returnbuff[i] = returnbuff[i] << 2;
00025         returnbuff[i] /= 4;
00026     }
00027 
00028 }
00029 
00030 void calibgyro() {
00031 
00032     //activating the wiimotion +
00033     i2c.start();
00034     //device address | write
00035     i2c.write(0x53 << 1 | 0);
00036     //register address
00037     i2c.write(0xfe);
00038     //data
00039     i2c.write(0x04);
00040     i2c.stop();
00041 
00042     for (int i = 0; i < 100; i++) {
00043         //sending a 0x00 to flag that we want data
00044         i2c.start();
00045         //device address | write (note new address)
00046         i2c.write(0x52 << 1 | 0);
00047         //send 0x00
00048         i2c.write(0x00);
00049         i2c.stop();
00050 
00051         //reading the data
00052         char wmpdata[6];
00053         i2c.read(0x52 << 1 | 1, wmpdata, 6);
00054         //pc.printf("%x %x %x %x %x %x\r\n",  wmpdata[0], wmpdata[1], wmpdata[2], wmpdata[3], wmpdata[4], wmpdata[5]);
00055 
00056         gyrcalib[0] += (((wmpdata[3] >> 2) << 8) + wmpdata[0]) / 100;
00057         gyrcalib[1] += (((wmpdata[4] >> 2) << 8) + wmpdata[1]) / 100;
00058         gyrcalib[2] += (((wmpdata[5] >> 2) << 8) + wmpdata[2]) / 100;
00059 
00060         wait(0.05);
00061 
00062     }
00063 
00064 }
00065 
00066 void getgyros(float gyrodata[3]) {
00067     //sending a 0x00 to flag that we want data
00068     i2c.start();
00069     //device address | write (note new address)
00070     i2c.write(0x52 << 1 | 0);
00071     //send 0x00
00072     i2c.write(0x00);
00073     i2c.stop();
00074 
00075     //reading the data
00076     char wmpdata[6];
00077     i2c.read(0x52 << 1 | 1, wmpdata, 6);
00078     
00079     //detect if we ever went into fast mode
00080     bool fastdiscard = !(wmpdata[3] & 0x02 && wmpdata[4] & 0x02 && wmpdata[3] & 0x01);
00081     
00082     if (fastdiscard)
00083         myled = 1;
00084     else {
00085         gyrodata[0] = RPSPUNIT * -(((wmpdata[3] >> 2) << 8) + wmpdata[0] - gyrcalib[0]);
00086         gyrodata[1] = RPSPUNIT * (((wmpdata[4] >> 2) << 8) + wmpdata[1] - gyrcalib[1]);
00087         gyrodata[2] = RPSPUNIT * (((wmpdata[5] >> 2) << 8) + wmpdata[2] - gyrcalib[2]);
00088     }
00089 
00090     //pc.printf("yaw: %f, pitch: %f, roll: %f\r\n", gyrodata[0], gyrodata[1], gyrodata[2]);
00091 
00092     //wait(0.05);
00093 
00094 }
00095 
00096 #ifdef ALTSENSOR
00097 float getaltsensor() {
00098     return altsensor * 
00099 }
00100 #endif