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

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

Committer:
nki
Date:
Sun Mar 08 00:45:28 2015 +0000
Revision:
10:b4abecccec7a
Parent:
9:d3b70c15baa9 
uguu;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bwang 1:1f58bdcf2956 1 #include "includes.h"
bwang 1:1f58bdcf2956 2 #include "core.h"
bwang 1:1f58bdcf2956 3 #include "sensors.h"
bwang 1:1f58bdcf2956 4 #include "meta.h"
bwang 3:0a2396597e0d 5 #include "lut.h"
bwang 1:1f58bdcf2956 6
bwang 3:0a2396597e0d 7 Inverter* LoopDriver::_inverter;
bwang 3:0a2396597e0d 8 Motor* LoopDriver::_motor;
bwang 3:0a2396597e0d 9 User* LoopDriver::_user;
bwang 3:0a2396597e0d 10 PidController* LoopDriver::_pid_d;
bwang 3:0a2396597e0d 11 PidController* LoopDriver::_pid_q;
bwang 3:0a2396597e0d 12 ReferenceSynthesizer* LoopDriver::_reference;
bwang 3:0a2396597e0d 13 Modulator* LoopDriver::_modulator;
nki 4:fdadf4a3577a 14 int LoopDriver::_blinky_toggle;
bwang 3:0a2396597e0d 15
bwang 3:0a2396597e0d 16 LoopDriver::LoopDriver(Inverter *inverter, Motor *motor, User *user, PidController *pid_d,
bwang 3:0a2396597e0d 17 PidController *pid_q, Modulator *modulator, float max_phase_current, int update_frequency) {
bwang 3:0a2396597e0d 18 _inverter = inverter;
bwang 3:0a2396597e0d 19 _motor = motor;
bwang 3:0a2396597e0d 20 _user = user;
bwang 3:0a2396597e0d 21 _pid_d = pid_d;
bwang 3:0a2396597e0d 22 _pid_q = pid_q;
nki 9:d3b70c15baa9 23 _reference = new SynchronousReferenceSynthesizer(max_phase_current);
bwang 3:0a2396597e0d 24 _modulator = modulator;
bwang 3:0a2396597e0d 25 _max_phase_current = max_phase_current;
bwang 3:0a2396597e0d 26 _update_frequency = update_frequency;
bwang 3:0a2396597e0d 27 }
bwang 3:0a2396597e0d 28
bwang 3:0a2396597e0d 29 void LoopDriver::Start() {
bwang 3:0a2396597e0d 30 _upd_ticker.attach_us(&update, 1000000 / _update_frequency);
bwang 3:0a2396597e0d 31 }
bwang 3:0a2396597e0d 32
nki 6:99ee0ce47fb2 33 void LoopDriver::Clarke(float c, float b, float *alpha, float *beta) {
nki 9:d3b70c15baa9 34 /*
nki 9:d3b70c15baa9 35 //ignoring current for now. These are motor position vectors (A,B,C) but no C because it's not necessary for the calculation)
nki 9:d3b70c15baa9 36 //these numbers range from -1:1, but when we use current instead of angle, the units will be amps. The amps will be unbounded ;)
nki 9:d3b70c15baa9 37 float A, B;
nki 9:d3b70c15baa9 38 A = FastSin(_motor->angle);
nki 9:d3b70c15baa9 39 B = FastSin(_motor->angle - 120.0f);
nki 9:d3b70c15baa9 40
nki 9:d3b70c15baa9 41 //\alpha \beta (Clarke) transform
nki 9:d3b70c15baa9 42 *alpha = A;
nki 9:d3b70c15baa9 43 *beta = (A + 2.0f*B)/sqrt(3.0f);
nki 9:d3b70c15baa9 44 */
nki 9:d3b70c15baa9 45 //\alpha \beta (Clarke) transform
nki 9:d3b70c15baa9 46 float ia = -(b+c);
nki 9:d3b70c15baa9 47 float ib = b;
nki 9:d3b70c15baa9 48 float ic = c;
nki 9:d3b70c15baa9 49
nki 9:d3b70c15baa9 50 *alpha = ia;
nki 9:d3b70c15baa9 51 *beta = (ia + 2.0f*ib)/sqrt(3.0f);
bwang 3:0a2396597e0d 52 }
bwang 3:0a2396597e0d 53
bwang 3:0a2396597e0d 54 void LoopDriver::Parke(float alpha, float beta, float theta, float *d, float *q) {
nki 9:d3b70c15baa9 55 float cos = FastCos(theta);
nki 9:d3b70c15baa9 56 float sin = FastSin(theta);
nki 9:d3b70c15baa9 57 *d = alpha * cos + beta * sin;
nki 9:d3b70c15baa9 58 *q = -alpha * sin + beta * cos;
bwang 3:0a2396597e0d 59 }
bwang 3:0a2396597e0d 60
bwang 3:0a2396597e0d 61 void LoopDriver::InverseParke(float d, float q, float theta, float *alpha, float *beta) {
nki 9:d3b70c15baa9 62 float cos = FastCos(theta);
nki 9:d3b70c15baa9 63 float sin = FastSin(theta);
nki 9:d3b70c15baa9 64 *alpha = cos * d - sin * q;
nki 9:d3b70c15baa9 65 *beta = sin * d + cos * q;
nki 6:99ee0ce47fb2 66
nki 9:d3b70c15baa9 67 /*
nki 6:99ee0ce47fb2 68 *alpha = cos * 1.0f;
nki 9:d3b70c15baa9 69 *beta = -sin * 1.0f;
nki 9:d3b70c15baa9 70 */
bwang 3:0a2396597e0d 71 }
bwang 3:0a2396597e0d 72
bwang 3:0a2396597e0d 73 float LoopDriver::LutSin(float theta) {
bwang 3:0a2396597e0d 74 if (theta < 0.0f) theta += 360.0f;
bwang 3:0a2396597e0d 75 if (theta >= 360.0f) theta -= 360.0f;
nki 6:99ee0ce47fb2 76 return sinetab[(int) theta] * 2.0f - 1.0f;
bwang 3:0a2396597e0d 77 }
bwang 3:0a2396597e0d 78
bwang 3:0a2396597e0d 79 float LoopDriver::LutCos(float theta) {
bwang 3:0a2396597e0d 80 return LutSin(90.0f - theta);
bwang 3:0a2396597e0d 81 }
bwang 3:0a2396597e0d 82
nki 6:99ee0ce47fb2 83 void LoopDriver::update() {
nki 9:d3b70c15baa9 84
bwang 3:0a2396597e0d 85 float alpha, beta, d, q, ref_d, ref_q, valpha, vbeta;
nki 6:99ee0ce47fb2 86 Clarke(_motor->I_c, _motor->I_b, &alpha, &beta);
bwang 3:0a2396597e0d 87 Parke(alpha, beta, _motor->angle, &d, &q);
nki 9:d3b70c15baa9 88
nki 9:d3b70c15baa9 89 d_mean = 0.01f*d + 0.99f*d_mean;
nki 9:d3b70c15baa9 90 q_mean = 0.01f*q + 0.99f*q_mean;
nki 9:d3b70c15baa9 91
nki 10:b4abecccec7a 92 _reference->GetReference(_motor->angle, _user->throttle, &ref_d, &ref_q); //this just returns d = 0 and q = 1 right now. doesn't care about angle
nki 9:d3b70c15baa9 93 float vd = _pid_d->Update(ref_d, d_mean);
nki 9:d3b70c15baa9 94 float vq = _pid_q->Update(ref_q, q_mean);
nki 9:d3b70c15baa9 95
nki 10:b4abecccec7a 96 vd = 0.0f;
nki 10:b4abecccec7a 97 vq = _user->throttle;
nki 9:d3b70c15baa9 98
nki 9:d3b70c15baa9 99
bwang 3:0a2396597e0d 100 InverseParke(vd, vq, _motor->angle, &valpha, &vbeta);
nki 10:b4abecccec7a 101
nki 10:b4abecccec7a 102 if(acquire==1){
nki 10:b4abecccec7a 103 if(bufidx < bufsize){
nki 10:b4abecccec7a 104 fbuffer[bufidx] = _motor->angle;
nki 10:b4abecccec7a 105 }
nki 10:b4abecccec7a 106 else{
nki 10:b4abecccec7a 107 acquire = 0;
nki 10:b4abecccec7a 108 }
nki 10:b4abecccec7a 109 }
nki 9:d3b70c15baa9 110 _modulator->Update(valpha, vbeta);
nki 9:d3b70c15baa9 111
nki 9:d3b70c15baa9 112 /*
nki 10:b4abecccec7a 113 test_alpha = valpha;
nki 10:b4abecccec7a 114 test_beta = vbeta;
nki 10:b4abecccec7a 115 */
bwang 3:0a2396597e0d 116 }