N K / Mbed 2 deprecated GaNtroller

a fork of priustroller

Dependencies:   mbed

Fork of priustroller_current by N K

sensors/positionsensors.cpp@55:f102d271e808, 2015-05-21 (annotated)

Committer:
nki
Date:
Thu May 21 02:19:25 2015 +0000
Revision:
55:f102d271e808
Parent:
54:e8d9bc885723 
still testing;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bwang 11:dccbaa9274c5 1 #include "includes.h"
bwang 11:dccbaa9274c5 2 #include "sensors.h"
bwang 54:e8d9bc885723 3 #include "filters.h"
bwang 11:dccbaa9274c5 4 #include "lut.h"
bwang 11:dccbaa9274c5 5
bwang 11:dccbaa9274c5 6 AnalogHallPositionSensor::AnalogHallPositionSensor(PinName pin_a, PinName pin_b, float cal1_a, float cal2_a,
bwang 11:dccbaa9274c5 7 float cal1_b, float cal2_b, float offset) {
nki 33:e7b132029bae 8 _in_a = new AnalogIn(pin_a);
nki 33:e7b132029bae 9 _in_b = new AnalogIn(pin_b);
bwang 11:dccbaa9274c5 10 _cal1_a = cal1_a;
bwang 11:dccbaa9274c5 11 _cal2_a = cal2_a;
bwang 11:dccbaa9274c5 12 _cal1_b = cal1_b;
bwang 11:dccbaa9274c5 13 _cal2_b = cal2_b;
bwang 11:dccbaa9274c5 14 _offset = offset;
bwang 54:e8d9bc885723 15 _time_upd_ticker = new Ticker();
bwang 54:e8d9bc885723 16 _time_upd_ticker->attach_us(this, &AnalogHallPositionSensor::upd_function, 50);
bwang 54:e8d9bc885723 17 _last_time = 0.0f;
bwang 54:e8d9bc885723 18 _last_position = 0.0f;
bwang 54:e8d9bc885723 19 _speed = 0.0f;
bwang 54:e8d9bc885723 20 GetPosition(); //first speed result is wrong, need to do this once
bwang 11:dccbaa9274c5 21 }
bwang 11:dccbaa9274c5 22
bwang 11:dccbaa9274c5 23 float AnalogHallPositionSensor::GetPosition() {
bwang 11:dccbaa9274c5 24 float angle = 0.0f;
bwang 11:dccbaa9274c5 25 float ascaled = 2 * (((float) *_in_a - _cal1_a) / (_cal2_a - _cal1_a) - 0.5f);
bwang 11:dccbaa9274c5 26 float bscaled = 2 * (((float) *_in_b - _cal1_b)/ (_cal2_b - _cal1_b) - 0.5f);
bwang 11:dccbaa9274c5 27
bwang 11:dccbaa9274c5 28 float x = bscaled / ascaled;
bwang 11:dccbaa9274c5 29
bwang 11:dccbaa9274c5 30 unsigned int index = (abs(x) / ATAN_UPPER_BOUND) * ATAN_TABLE_SIZE;
bwang 11:dccbaa9274c5 31
bwang 11:dccbaa9274c5 32 if(index >= ATAN_TABLE_SIZE) index = ATAN_TABLE_SIZE - 1;
bwang 11:dccbaa9274c5 33
bwang 11:dccbaa9274c5 34 if(ascaled < 0){
bwang 11:dccbaa9274c5 35 if(bscaled < 0) angle = 90 - arctan[index];
bwang 11:dccbaa9274c5 36 if(bscaled > 0) angle = 90 + arctan[index];
bwang 11:dccbaa9274c5 37 }
bwang 11:dccbaa9274c5 38
bwang 11:dccbaa9274c5 39 if(ascaled>0){
bwang 11:dccbaa9274c5 40 if(bscaled > 0) angle = 270 - arctan[index];
bwang 11:dccbaa9274c5 41 if(bscaled < 0) angle = 270 + arctan[index];
bwang 11:dccbaa9274c5 42 }
bwang 11:dccbaa9274c5 43
bwang 11:dccbaa9274c5 44 if(angle > 360.0f) angle = 360.0f;
bwang 11:dccbaa9274c5 45 if(angle < 0) angle = 0;
bwang 11:dccbaa9274c5 46
bwang 11:dccbaa9274c5 47 angle -= _offset;
bwang 11:dccbaa9274c5 48 if (angle < 0.0f) angle += 360.0f;
bwang 11:dccbaa9274c5 49 if (angle >= 360.0f) angle -= 360.0f;
bwang 11:dccbaa9274c5 50
bwang 54:e8d9bc885723 51 float delta_t = _time - _last_time;
bwang 54:e8d9bc885723 52 float delta_angle = -(angle - _last_position); //backwards, for now
bwang 54:e8d9bc885723 53 _last_time = _time;
bwang 54:e8d9bc885723 54 _last_position = angle;
bwang 54:e8d9bc885723 55 if (delta_angle < 0.0f) return angle;
bwang 54:e8d9bc885723 56 _speed = delta_angle / delta_t;
bwang 11:dccbaa9274c5 57 return angle;
bwang 11:dccbaa9274c5 58 }