N K
/
analoghalls6
motor spins
Fork of analoghalls5 by
positionsensors.cpp@0:54cf32d35f4d, 2015-03-01 (annotated)
- Committer:
- bwang
- Date:
- Sun Mar 01 06:16:01 2015 +0000
- Revision:
- 0:54cf32d35f4d
- Child:
- 1:1f58bdcf2956
first publish
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
bwang | 0:54cf32d35f4d | 1 | #include "includes.h" |
bwang | 0:54cf32d35f4d | 2 | #include "classes.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; |
bwang | 0:54cf32d35f4d | 43 | |
bwang | 0:54cf32d35f4d | 44 | return angle; |
bwang | 0:54cf32d35f4d | 45 | } |