Accelerometer.cpp

00001 #include "Accelerometer.h"
00002 
00003 Accelerometer::Accelerometer(int nI2cAddress, LCD_ST7735* pDisp) : i2cAddress(nI2cAddress), i2c(P0_5, P0_4)
00004 {   // constructor
00005     this->i2c.frequency(400000);        // fast I2C is 400 KHz, not 400 Hz. Default frequency is 100 KHz
00006     this->writeRegister(0x2A, 0x01); // initialize accelerometer (set CTRL_REG1 bit ACTIVE)
00007     this->pDisp=pDisp;
00008 
00009     this->colors[0] = Color565::Red;
00010     this->colors[1] = Color565::Green;
00011     this->colors[2] = Color565::Blue;
00012 }
00013 
00014 void Accelerometer::readRegisters(char address, char* buffer, int len)
00015 {
00016     this->i2c.write(i2cAddress, &address, 1, true);
00017     this->i2c.read(i2cAddress | 1, buffer, len);
00018 }
00019 
00020 int Accelerometer::writeRegister(char address, char value) {    
00021     char buffer[2] = { address, value };
00022     
00023     return this->i2c.write(i2cAddress, buffer, 2);
00024 }
00025 
00026 double Accelerometer::convert(char* buffer)
00027 {
00028     double val = ((buffer[0] << 2) | (buffer[1] >> 6));
00029             
00030     if (val > 511.0) 
00031         val -= 1024.0;
00032     
00033     return val / 512.0;
00034 }
00035 
00036 void Accelerometer::getXYZ(double& x, double& y, double& z) {
00037     char buffer[6];
00038     
00039     this->readRegisters(0x01, buffer, 6);
00040     
00041     x = this->convert(buffer);
00042     y = this->convert(buffer + 2);
00043     z = this->convert(buffer + 4);
00044 }
00045 
00046 //
00047 // Accellerator graph for debug purposes
00048 //
00049 
00050 void Accelerometer::drawAxes()
00051 {
00052     for (int i = 0; i < 3; i++) {
00053         this->pDisp->fillRect(0, i * (Accelerometer::GRAPH_HEIGHT + Accelerometer::GRAPH_SPACING), this->pDisp->getWidth(), i * (Accelerometer::GRAPH_HEIGHT + Accelerometer::GRAPH_SPACING) + Accelerometer::GRAPH_HEIGHT, Color565::fromRGB(i==0?0x22:0, i==1?0x22:0, i==2?0x22:0));
00054         this->pDisp->drawLine(0, i * (Accelerometer::GRAPH_HEIGHT + Accelerometer::GRAPH_SPACING), 0, i * (Accelerometer::GRAPH_HEIGHT + Accelerometer::GRAPH_SPACING) + Accelerometer::GRAPH_HEIGHT, Color565::White);
00055         this->pDisp->drawLine(0, i * (Accelerometer::GRAPH_HEIGHT + Accelerometer::GRAPH_SPACING) + Accelerometer::GRAPH_HEIGHT / 2, this->pDisp->getWidth(), i * (Accelerometer::GRAPH_HEIGHT + Accelerometer::GRAPH_SPACING) + Accelerometer::GRAPH_HEIGHT / 2, Color565::White);
00056     }
00057 }
00058 
00059 void Accelerometer::drawPoint(int axis, double value)
00060 {
00061     if (value < -1.0)
00062         value = -1.0;
00063 
00064     if (value > 1.0)
00065         value = 1.0;
00066 
00067     value += 1.0;
00068     value /= 2.0;
00069     value = 1.0 - value;
00070     value *= Accelerometer::GRAPH_HEIGHT;
00071 
00072     this->pDisp->setPixel(this->graphX, axis * (Accelerometer::GRAPH_HEIGHT + Accelerometer::GRAPH_SPACING) + (int)value, this->colors[axis]);
00073 }
00074 
00075 void Accelerometer::resetGraph()
00076 {
00077     this->graphX = 0;
00078     this->pDisp->clearScreen();
00079     this->drawAxes();
00080 }
00081 
00082 void Accelerometer::checkGraphReset()
00083 {
00084     if (this->graphX > this->pDisp->getWidth())
00085     {
00086         this->resetGraph();
00087     }
00088 }
00089 
00090 void Accelerometer::updateGraph()
00091 {
00092     double x, y, z;
00093     this->getXYZ(x, y, z);
00094     
00095     this->checkGraphReset();
00096     this->drawPoint(0, x);
00097     this->drawPoint(1, y);
00098     this->drawPoint(2, z);
00099     this->graphX++;
00100 }