positionsensors.cpp

00001 #include "includes.h"
00002 #include "sensors.h"
00003 #include "lut.h"
00004 
00005 AnalogHallPositionSensor::AnalogHallPositionSensor(PinName pin_a, PinName pin_b, float cal1_a, float cal2_a, 
00006                              float cal1_b, float cal2_b, float offset) {
00007     _in_a = new AnalogIn(pin_a);
00008     _in_b = new AnalogIn(pin_b);
00009     _cal1_a = cal1_a;
00010     _cal2_a = cal2_a;
00011     _cal1_b = cal1_b;
00012     _cal2_b = cal2_b;
00013     _offset = offset;
00014 }
00015 
00016 float AnalogHallPositionSensor::GetPosition() {
00017     float angle = 0.0f;
00018     float ascaled = 2 * (((float) *_in_a - _cal1_a) / (_cal2_a - _cal1_a) - 0.5f);
00019     float bscaled = 2 * (((float) *_in_b - _cal1_b)/ (_cal2_b - _cal1_b) - 0.5f);
00020     
00021     float x = bscaled / ascaled;
00022     
00023     unsigned int index = (abs(x) / ATAN_UPPER_BOUND) * ATAN_TABLE_SIZE;
00024     
00025     if(index >= ATAN_TABLE_SIZE) index = ATAN_TABLE_SIZE - 1;
00026     
00027     if(ascaled < 0){
00028         if(bscaled < 0) angle = 90 - arctan[index];
00029         if(bscaled > 0) angle = 90 + arctan[index];
00030     } 
00031        
00032     if(ascaled>0){
00033         if(bscaled > 0) angle = 270 - arctan[index];
00034         if(bscaled < 0) angle = 270 + arctan[index];
00035     }
00036     
00037     if(angle > 360.0f) angle = 360.0f;
00038     if(angle < 0) angle = 0;
00039     
00040     angle -= _offset;
00041     if (angle < 0.0f) angle += 360.0f;
00042     if (angle >= 360.0f) angle -= 360.0f;
00043     /*
00044     #ifdef __DEBUG
00045     pc->printf("%f",angle);
00046     pc->printf("\t");
00047     pc->printf("%f",ascaled);
00048     pc->printf("\t");
00049     pc->printf("%f",bscaled);
00050     pc->printf("\t");
00051     pc->printf("\n\r");
00052     #endif
00053     */
00054     
00055     return angle;
00056 }