positionsensors.cpp

00001 #include "includes.h"
00002 #include "sensors.h"
00003 #include "filters.h"
00004 #include "lut.h"
00005 
00006 AnalogHallPositionSensor::AnalogHallPositionSensor(PinName pin_a, PinName pin_b, float cal1_a, float cal2_a, 
00007                              float cal1_b, float cal2_b, float offset) {
00008     _in_a = new AnalogIn(pin_a);
00009     _in_b = new AnalogIn(pin_b);
00010     _cal1_a = cal1_a;
00011     _cal2_a = cal2_a;
00012     _cal1_b = cal1_b;
00013     _cal2_b = cal2_b;
00014     _offset = offset;
00015     _time_upd_ticker = new Ticker();
00016     _time_upd_ticker->attach_us(this, &AnalogHallPositionSensor::upd_function, 50);
00017     _last_time = 0.0f;
00018     _last_position = 0.0f;
00019     _speed = 0.0f;
00020     GetPosition(); //first speed result is wrong, need to do this once
00021 }
00022 
00023 float AnalogHallPositionSensor::GetPosition() {
00024     float angle = 0.0f;
00025     float ascaled = 2 * (((float) *_in_a - _cal1_a) / (_cal2_a - _cal1_a) - 0.5f);
00026     float bscaled = 2 * (((float) *_in_b - _cal1_b)/ (_cal2_b - _cal1_b) - 0.5f);
00027     
00028     float x = bscaled / ascaled;
00029     
00030     unsigned int index = (abs(x) / ATAN_UPPER_BOUND) * ATAN_TABLE_SIZE;
00031     
00032     if(index >= ATAN_TABLE_SIZE) index = ATAN_TABLE_SIZE - 1;
00033     
00034     if(ascaled < 0){
00035         if(bscaled < 0) angle = 90 - arctan[index];
00036         if(bscaled > 0) angle = 90 + arctan[index];
00037     } 
00038        
00039     if(ascaled>0){
00040         if(bscaled > 0) angle = 270 - arctan[index];
00041         if(bscaled < 0) angle = 270 + arctan[index];
00042     }
00043     
00044     if(angle > 360.0f) angle = 360.0f;
00045     if(angle < 0) angle = 0;
00046     
00047     angle -= _offset;
00048     if (angle < 0.0f) angle += 360.0f;
00049     if (angle >= 360.0f) angle -= 360.0f;
00050     
00051     float delta_t = _time - _last_time;
00052     float delta_angle = -(angle - _last_position); //backwards, for now
00053     _last_time = _time;
00054     _last_position = angle;
00055     if (delta_angle < 0.0f) return angle;
00056     _speed = delta_angle / delta_t;
00057     return angle;
00058 }