N K / Mbed 2 deprecated analoghalls5_5

working-est copy with class-based code. still open loop

Dependencies:   mbed

Fork of analoghalls6 by N K

positionsensors.cpp@10:b4abecccec7a, 2015-03-08 (annotated)

Committer:
nki
Date:
Sun Mar 08 00:45:28 2015 +0000
Revision:
10:b4abecccec7a
Parent:
5:ee1e6c84c302
Child:
7:76d6ceb23e0d
uguu;

Who changed what in which revision?

UserRevisionLine numberNew 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 }