Diff: ReadCommand.cpp

Revision:

0:e188325211af

Child:

1:fd4c0e2decb8

diff -r 000000000000 -r e188325211af ReadCommand.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ReadCommand.cpp	Thu Jun 04 10:56:18 2015 +0000
@@ -0,0 +1,66 @@
+#include "ReadCommand.h"
+
+ReadCommand::ReadCommand()
+{
+    Storage* storage = Storage::getInstance();
+    Sensors* sensors = Sensors::getInstance();
+    this->running = 0;
+    this->addr = 0x00;
+}
+
+void ReadCommand::initialize()
+{
+    printf("ReadCommand\n");
+    setLed(1);
+}
+
+void ReadCommand::finish()
+{
+    setLed(0);
+}
+
+
+void ReadCommand::execute()
+{
+}
+
+void ReadCommand::button()
+{
+    printf("**** BUTTON: ReadCommand    *****\n");
+
+    __disable_irq();
+    readFromMemory();
+    __enable_irq();
+}
+
+
+void ReadCommand::readFromMemory()
+{
+    float angle;
+    int16_t gyro;
+
+    this->storage->read(addr);
+
+    angle_H= this->storage->readData();
+    angle_L = this->storage->readData();
+    imu_H = this->storage->readData();
+    imu_L = this->storage->readData();
+
+    //for(int i = 0; i <100; i++) {
+    do{
+        angle = ((angle_H << 8) + angle_L) * 0.087912087;
+        gyro = ((imu_H << 8) + imu_L)/ 200;
+
+        angle_H= this->storage->readData();
+        angle_L = this->storage->readData();
+        imu_H = this->storage->readData();
+        imu_L = this->storage->readData();
+        
+        
+        printf("Memory: angle = %.2f, gyroX =  %d\n", angle, gyro);
+    } while(angle_H != 0xFF);
+    
+    this->storage->endAction();
+    printf("DONE!\n");
+}
+