N K
/
analoghalls6
motor spins
Fork of analoghalls5 by
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Fri Jul 15 2022 12:15:05 by 1.7.2