Bayley Wang / Mbed 2 deprecated priustroller

Prius IPM controller

Dependencies:   mbed

Fork of analoghalls5_5 by N K

sensors/positionsensors.cpp@35:83cf9564bd0c, 2015-03-16 (annotated)

Committer:
bwang
Date:
Mon Mar 16 15:40:07 2015 +0000
Revision:
35:83cf9564bd0c
Parent:
33:e7b132029bae 
Set for 3A Iq, 0A Id, Ki = 1e-3, Kp = Kd = 0, loop still oscillates but is passable

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 11:dccbaa9274c5 3 #include "lut.h"
bwang 11:dccbaa9274c5 4
bwang 11:dccbaa9274c5 5 AnalogHallPositionSensor::AnalogHallPositionSensor(PinName pin_a, PinName pin_b, float cal1_a, float cal2_a,
bwang 11:dccbaa9274c5 6 float cal1_b, float cal2_b, float offset) {
nki 33:e7b132029bae 7 _in_a = new AnalogIn(pin_a);
nki 33:e7b132029bae 8 _in_b = new AnalogIn(pin_b);
bwang 11:dccbaa9274c5 9 _cal1_a = cal1_a;
bwang 11:dccbaa9274c5 10 _cal2_a = cal2_a;
bwang 11:dccbaa9274c5 11 _cal1_b = cal1_b;
bwang 11:dccbaa9274c5 12 _cal2_b = cal2_b;
bwang 11:dccbaa9274c5 13 _offset = offset;
bwang 11:dccbaa9274c5 14 }
bwang 11:dccbaa9274c5 15
bwang 11:dccbaa9274c5 16 float AnalogHallPositionSensor::GetPosition() {
bwang 11:dccbaa9274c5 17 float angle = 0.0f;
bwang 11:dccbaa9274c5 18 float ascaled = 2 * (((float) *_in_a - _cal1_a) / (_cal2_a - _cal1_a) - 0.5f);
bwang 11:dccbaa9274c5 19 float bscaled = 2 * (((float) *_in_b - _cal1_b)/ (_cal2_b - _cal1_b) - 0.5f);
bwang 11:dccbaa9274c5 20
bwang 11:dccbaa9274c5 21 float x = bscaled / ascaled;
bwang 11:dccbaa9274c5 22
bwang 11:dccbaa9274c5 23 unsigned int index = (abs(x) / ATAN_UPPER_BOUND) * ATAN_TABLE_SIZE;
bwang 11:dccbaa9274c5 24
bwang 11:dccbaa9274c5 25 if(index >= ATAN_TABLE_SIZE) index = ATAN_TABLE_SIZE - 1;
bwang 11:dccbaa9274c5 26
bwang 11:dccbaa9274c5 27 if(ascaled < 0){
bwang 11:dccbaa9274c5 28 if(bscaled < 0) angle = 90 - arctan[index];
bwang 11:dccbaa9274c5 29 if(bscaled > 0) angle = 90 + arctan[index];
bwang 11:dccbaa9274c5 30 }
bwang 11:dccbaa9274c5 31
bwang 11:dccbaa9274c5 32 if(ascaled>0){
bwang 11:dccbaa9274c5 33 if(bscaled > 0) angle = 270 - arctan[index];
bwang 11:dccbaa9274c5 34 if(bscaled < 0) angle = 270 + arctan[index];
bwang 11:dccbaa9274c5 35 }
bwang 11:dccbaa9274c5 36
bwang 11:dccbaa9274c5 37 if(angle > 360.0f) angle = 360.0f;
bwang 11:dccbaa9274c5 38 if(angle < 0) angle = 0;
bwang 11:dccbaa9274c5 39
bwang 11:dccbaa9274c5 40 angle -= _offset;
bwang 11:dccbaa9274c5 41 if (angle < 0.0f) angle += 360.0f;
bwang 11:dccbaa9274c5 42 if (angle >= 360.0f) angle -= 360.0f;
bwang 11:dccbaa9274c5 43
bwang 11:dccbaa9274c5 44 return angle;
bwang 11:dccbaa9274c5 45 }