Diff: sensors/positionsensors.cpp

Revision:

11:dccbaa9274c5

Child:

28:af9d43bcb371

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sensors/positionsensors.cpp	Sun Mar 08 08:37:38 2015 +0000
@@ -0,0 +1,45 @@
+#include "includes.h"
+#include "sensors.h"
+#include "lut.h"
+
+AnalogHallPositionSensor::AnalogHallPositionSensor(PinName pin_a, PinName pin_b, float cal1_a, float cal2_a, 
+                             float cal1_b, float cal2_b, float offset) {
+    _in_a = new AnalogIn(pin_a);
+    _in_b = new AnalogIn(pin_b);
+    _cal1_a = cal1_a;
+    _cal2_a = cal2_a;
+    _cal1_b = cal1_b;
+    _cal2_b = cal2_b;
+    _offset = offset;
+}
+
+float AnalogHallPositionSensor::GetPosition() {
+    float angle = 0.0f;
+    float ascaled = 2 * (((float) *_in_a - _cal1_a) / (_cal2_a - _cal1_a) - 0.5f);
+    float bscaled = 2 * (((float) *_in_b - _cal1_b)/ (_cal2_b - _cal1_b) - 0.5f);
+    
+    float x = bscaled / ascaled;
+    
+    unsigned int index = (abs(x) / ATAN_UPPER_BOUND) * ATAN_TABLE_SIZE;
+    
+    if(index >= ATAN_TABLE_SIZE) index = ATAN_TABLE_SIZE - 1;
+    
+    if(ascaled < 0){
+        if(bscaled < 0) angle = 90 - arctan[index];
+        if(bscaled > 0) angle = 90 + arctan[index];
+    } 
+       
+    if(ascaled>0){
+        if(bscaled > 0) angle = 270 - arctan[index];
+        if(bscaled < 0) angle = 270 + arctan[index];
+    }
+    
+    if(angle > 360.0f) angle = 360.0f;
+    if(angle < 0) angle = 0;
+    
+    angle -= _offset;
+    if (angle < 0.0f) angle += 360.0f;
+    if (angle >= 360.0f) angle -= 360.0f;
+    
+    return angle;
+}
\ No newline at end of file