N K
/
analoghalls6
motor spins
Fork of analoghalls5 by
positionsensors.cpp@5:ee1e6c84c302, 2015-03-03 (annotated)
- Committer:
- nki
- Date:
- Tue Mar 03 06:28:10 2015 +0000
- Revision:
- 5:ee1e6c84c302
- Parent:
- 1:1f58bdcf2956
hi there;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
bwang | 0:54cf32d35f4d | 1 | #include "includes.h" |
bwang | 1:1f58bdcf2956 | 2 | #include "sensors.h" |
bwang | 0:54cf32d35f4d | 3 | #include "lut.h" |
bwang | 0:54cf32d35f4d | 4 | |
bwang | 0:54cf32d35f4d | 5 | AnalogHallPositionSensor::AnalogHallPositionSensor(PinName pin_a, PinName pin_b, float cal1_a, float cal2_a, |
bwang | 0:54cf32d35f4d | 6 | float cal1_b, float cal2_b, float offset) { |
bwang | 0:54cf32d35f4d | 7 | _in_a = new AnalogIn(pin_a); |
bwang | 0:54cf32d35f4d | 8 | _in_b = new AnalogIn(pin_b); |
bwang | 0:54cf32d35f4d | 9 | _cal1_a = cal1_a; |
bwang | 0:54cf32d35f4d | 10 | _cal2_a = cal2_a; |
bwang | 0:54cf32d35f4d | 11 | _cal1_b = cal1_b; |
bwang | 0:54cf32d35f4d | 12 | _cal2_b = cal2_b; |
bwang | 0:54cf32d35f4d | 13 | _offset = offset; |
bwang | 0:54cf32d35f4d | 14 | } |
bwang | 0:54cf32d35f4d | 15 | |
bwang | 0:54cf32d35f4d | 16 | float AnalogHallPositionSensor::GetPosition() { |
bwang | 0:54cf32d35f4d | 17 | float angle = 0.0f; |
bwang | 0:54cf32d35f4d | 18 | float ascaled = 2 * (((float) *_in_a - _cal1_a) / (_cal2_a - _cal1_a) - 0.5f); |
bwang | 0:54cf32d35f4d | 19 | float bscaled = 2 * (((float) *_in_b - _cal1_b)/ (_cal2_b - _cal1_b) - 0.5f); |
bwang | 0:54cf32d35f4d | 20 | |
bwang | 0:54cf32d35f4d | 21 | float x = bscaled / ascaled; |
bwang | 0:54cf32d35f4d | 22 | |
bwang | 0:54cf32d35f4d | 23 | unsigned int index = (abs(x) / ATAN_UPPER_BOUND) * ATAN_TABLE_SIZE; |
bwang | 0:54cf32d35f4d | 24 | |
bwang | 0:54cf32d35f4d | 25 | if(index >= ATAN_TABLE_SIZE) index = ATAN_TABLE_SIZE - 1; |
bwang | 0:54cf32d35f4d | 26 | |
bwang | 0:54cf32d35f4d | 27 | if(ascaled < 0){ |
bwang | 0:54cf32d35f4d | 28 | if(bscaled < 0) angle = 90 - arctan[index]; |
bwang | 0:54cf32d35f4d | 29 | if(bscaled > 0) angle = 90 + arctan[index]; |
bwang | 0:54cf32d35f4d | 30 | } |
bwang | 0:54cf32d35f4d | 31 | |
bwang | 0:54cf32d35f4d | 32 | if(ascaled>0){ |
bwang | 0:54cf32d35f4d | 33 | if(bscaled > 0) angle = 270 - arctan[index]; |
bwang | 0:54cf32d35f4d | 34 | if(bscaled < 0) angle = 270 + arctan[index]; |
bwang | 0:54cf32d35f4d | 35 | } |
bwang | 0:54cf32d35f4d | 36 | |
bwang | 0:54cf32d35f4d | 37 | if(angle > 360.0f) angle = 360.0f; |
bwang | 0:54cf32d35f4d | 38 | if(angle < 0) angle = 0; |
bwang | 0:54cf32d35f4d | 39 | |
bwang | 0:54cf32d35f4d | 40 | angle -= _offset; |
bwang | 0:54cf32d35f4d | 41 | if (angle < 0.0f) angle += 360.0f; |
bwang | 0:54cf32d35f4d | 42 | if (angle >= 360.0f) angle -= 360.0f; |
nki | 5:ee1e6c84c302 | 43 | /* |
nki | 5:ee1e6c84c302 | 44 | #ifdef __DEBUG |
nki | 5:ee1e6c84c302 | 45 | pc->printf("%f",angle); |
nki | 5:ee1e6c84c302 | 46 | pc->printf("\t"); |
nki | 5:ee1e6c84c302 | 47 | pc->printf("%f",ascaled); |
nki | 5:ee1e6c84c302 | 48 | pc->printf("\t"); |
nki | 5:ee1e6c84c302 | 49 | pc->printf("%f",bscaled); |
nki | 5:ee1e6c84c302 | 50 | pc->printf("\t"); |
nki | 5:ee1e6c84c302 | 51 | pc->printf("\n\r"); |
nki | 5:ee1e6c84c302 | 52 | #endif |
nki | 5:ee1e6c84c302 | 53 | */ |
bwang | 0:54cf32d35f4d | 54 | |
bwang | 0:54cf32d35f4d | 55 | return angle; |
bwang | 0:54cf32d35f4d | 56 | } |