Diff: Accelerometer.cpp

Revision:

0:3d0db4e183ee

Child:

1:71185a0aadfc

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Accelerometer.cpp	Fri Feb 06 09:51:06 2015 +0000
@@ -0,0 +1,102 @@
+#include "Accelerometer.h"
+
+Accelerometer::Accelerometer(int nI2cAddress, LCD_ST7735* pDisp) : i2cAddress(nI2cAddress), i2c(P0_5, P0_4)
+{   // constructor
+    this->i2c.frequency(400000);        // fast I2C is 400 KHz, not 400 Hz. Default frequency is 100 KHz
+    this->writeRegister(0x2A, 0x01); // initialize accelerometer (set CTRL_REG1 bit ACTIVE)
+    this->pDisp=pDisp;
+
+    this->colors[0] = Color565::Red;
+    this->colors[1] = Color565::Green;
+    this->colors[2] = Color565::Blue;
+}
+
+void Accelerometer::readRegisters(char address, char* buffer, int len) {
+//    int nStart=this->tWait.read_ms();
+    this->i2c.write(i2cAddress, &address, 1, true);
+    this->i2c.read(i2cAddress | 1, buffer, len);
+//    printDouble((double)this->tWait.read_ms()-nStart, 10, 10);
+}
+
+int Accelerometer::writeRegister(char address, char value) {    
+    char buffer[2] = { address, value };
+    
+    return this->i2c.write(i2cAddress, buffer, 2);
+}
+
+double Accelerometer::convert(char* buffer) {
+    double val = ((buffer[0] << 2) | (buffer[1] >> 6));
+            
+    if (val > 511.0) 
+        val -= 1024.0;
+    
+    return val / 512.0;
+}
+
+void Accelerometer::getXYZ(double& x, double& y, double& z) {
+    char buffer[6];
+    
+    this->readRegisters(0x01, buffer, 6);
+    
+    x = this->convert(buffer);
+    y = this->convert(buffer + 2);
+    z = this->convert(buffer + 4);
+}
+
+//
+// Accellerator graph for debug purposes
+//
+
+void Accelerometer::drawAxes()
+{
+    for (int i = 0; i < 3; i++) {
+        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));
+        this->pDisp->drawLine(0, i * (Accelerometer::GRAPH_HEIGHT + Accelerometer::GRAPH_SPACING), 0, i * (Accelerometer::GRAPH_HEIGHT + Accelerometer::GRAPH_SPACING) + Accelerometer::GRAPH_HEIGHT, Color565::White);
+        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);
+    }
+}
+
+void Accelerometer::drawPoint(int axis, double value)
+{
+    if (value < -1.0)
+        value = -1.0;
+
+    if (value > 1.0)
+        value = 1.0;
+
+    value += 1.0;
+    value /= 2.0;
+    value = 1.0 - value;
+    value *= Accelerometer::GRAPH_HEIGHT;
+
+    this->pDisp->setPixel(this->graphX, axis * (Accelerometer::GRAPH_HEIGHT + Accelerometer::GRAPH_SPACING) + (int)value, this->colors[axis]);
+}
+
+void Accelerometer::resetGraph()
+{
+    this->graphX = 0;
+    this->pDisp->clearScreen();
+    //this->pDisp->fillRect(0, 0, this->pDisp->getWidth(), this->pDisp->getHeight(), Color565::fromRGB(0x11, 0x33, 0x22));
+    this->drawAxes();
+}
+
+void Accelerometer::checkGraphReset()
+{
+    if (this->graphX > this->pDisp->getWidth())
+    {
+        this->resetGraph();
+    }
+}
+
+void Accelerometer::updateGraph()
+{
+    double x, y, z;
+    this->getXYZ(x, y, z);
+    
+    this->checkGraphReset();
+    this->drawPoint(0, x);
+    this->drawPoint(1, y);
+    this->drawPoint(2, z);
+    this->graphX++;
+}
+