N K / Mbed 2 deprecated priustroller_current

stock mbed AnalogReads current loop closed and working

Dependencies:   mbed

Fork of priustroller_2 by N K

callbacks.cpp@40:b2bd9a097e3f, 2015-04-16 (annotated)

Committer:
nki
Date:
Thu Apr 16 22:45:38 2015 +0000
Revision:
40:b2bd9a097e3f
Parent:
39:6f3d08745cd9
Child:
41:0a573cf3e9b9
spins (backwards)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bwang 11:dccbaa9274c5 1 #include "includes.h"
bwang 11:dccbaa9274c5 2 #include "transforms.h"
bwang 11:dccbaa9274c5 3 #include "filters.h"
bwang 11:dccbaa9274c5 4 #include "context.h"
bwang 11:dccbaa9274c5 5 #include "core.h"
bwang 11:dccbaa9274c5 6 #include "meta.h"
bwang 11:dccbaa9274c5 7 #include "sensors.h"
bwang 24:f1ff9c7256b5 8 #include "debug.h"
bwang 11:dccbaa9274c5 9
bwang 11:dccbaa9274c5 10 void fast(Context *c) {
bwang 11:dccbaa9274c5 11 float alpha, beta, d, q, ref_d, ref_q, valpha, vbeta;
nki 21:da41c08e51bc 12 float I_b, I_c, angle, d_filtered, q_filtered, vd, vq;
bwang 24:f1ff9c7256b5 13 dbg_ib = I_b = c->motor->GetCurrentB();
bwang 24:f1ff9c7256b5 14 dbg_ic = I_c = c->motor->GetCurrentC();
bwang 24:f1ff9c7256b5 15 dbg_angle = angle = c->motor->GetPosition();
bwang 24:f1ff9c7256b5 16
bwang 24:f1ff9c7256b5 17 Clarke(-(I_b + I_c), I_b, &alpha, &beta);
bwang 11:dccbaa9274c5 18 Parke(alpha, beta, angle, &d, &q);
bwang 11:dccbaa9274c5 19
nki 34:bfe180de813a 20 dbg_d_filtered = d_filtered = c->filter_q->Update(q); //swapped
nki 34:bfe180de813a 21 dbg_q_filtered = q_filtered = -c->filter_d->Update(d); //these and added a negative on this line.
nki 34:bfe180de813a 22
nki 34:bfe180de813a 23 //The necessary changes were analytically determined by observing that a postive vd command corresponded to a reading of negative q axis torque
nki 34:bfe180de813a 24 //and that a positive vq command produced a reading of positive d axis torque.
nki 34:bfe180de813a 25 //This may be the final step in aligning the handedness of the reference and the output.
nki 36:11766b5da6ed 26
nki 33:e7b132029bae 27 c->reference->GetReference(angle, c->user->throttle, &ref_d, &ref_q);
bwang 24:f1ff9c7256b5 28
bwang 24:f1ff9c7256b5 29 dbg_ref_d = ref_d;
bwang 24:f1ff9c7256b5 30 dbg_ref_q = ref_q;
bwang 24:f1ff9c7256b5 31 dbg_loop_d = vd = c->pid_d->Update(ref_d, d_filtered);
bwang 24:f1ff9c7256b5 32 dbg_loop_q = vq = c->pid_q->Update(ref_q, q_filtered);
bwang 11:dccbaa9274c5 33
nki 37:09373294ff22 34 dbg_loop_d = vd = 0.0f;
bwang 38:232c51b6853f 35 dbg_loop_q = vq = c->user->throttle / 2.0f;
bwang 38:232c51b6853f 36
nki 36:11766b5da6ed 37 /*
nki 36:11766b5da6ed 38 if(c->user->throttle <= 0.1f){
nki 36:11766b5da6ed 39 float ramp = (c->user->throttle*10.0f)*(c->user->throttle*10.0f);
nki 36:11766b5da6ed 40 vd = ramp*vd;
nki 36:11766b5da6ed 41 vq = ramp*vq;
bwang 38:232c51b6853f 42 }
nki 36:11766b5da6ed 43
nki 36:11766b5da6ed 44 dbg_loop_d = vd;
nki 36:11766b5da6ed 45 dbg_loop_q = vq;
bwang 38:232c51b6853f 46 */
nki 36:11766b5da6ed 47
nki 34:bfe180de813a 48 InverseParke(vd, vq, angle, &valpha, &vbeta);
nki 27:846c08fb3697 49
nki 34:bfe180de813a 50 dbg_valpha = valpha;
nki 34:bfe180de813a 51 dbg_vbeta = vbeta;
bwang 11:dccbaa9274c5 52
bwang 11:dccbaa9274c5 53 c->modulator->Update(valpha, vbeta);
bwang 11:dccbaa9274c5 54 }
bwang 11:dccbaa9274c5 55
bwang 38:232c51b6853f 56 void fast_test(Context *c) {
bwang 38:232c51b6853f 57 float angle, angle120, angle240;
bwang 38:232c51b6853f 58 angle = c->motor->GetPosition();
bwang 38:232c51b6853f 59 angle120 = angle + 120.0f;
bwang 38:232c51b6853f 60 if (angle120 >= 360.0f) angle120 -= 360.0f;
bwang 38:232c51b6853f 61 if (angle120 < 0.0f) angle120 += 360.0f;
bwang 38:232c51b6853f 62 angle240 = angle -120.0f;
bwang 38:232c51b6853f 63 if (angle120 >= 360.0f) angle240 -= 360.0f;
bwang 38:232c51b6853f 64 if (angle120 < 0.0f) angle240 += 360.0f;
nki 40:b2bd9a097e3f 65
nki 40:b2bd9a097e3f 66 dbg_dtcB = 0.5f + 0.5f * FastSin(angle) * c->user->throttle;
nki 40:b2bd9a097e3f 67 dbg_dtcA = 0.5f + 0.5f * FastSin(angle120) * c->user->throttle;
nki 40:b2bd9a097e3f 68 dbg_dtcC = 0.5f + 0.5f * FastSin(angle240) * c->user->throttle;
nki 40:b2bd9a097e3f 69 c->inverter->SetDtcA(dbg_dtcA);
nki 40:b2bd9a097e3f 70 c->inverter->SetDtcB(dbg_dtcB);
nki 40:b2bd9a097e3f 71 c->inverter->SetDtcC(dbg_dtcC);
bwang 38:232c51b6853f 72 }
bwang 38:232c51b6853f 73
bwang 11:dccbaa9274c5 74 void slow(Context *c) {
bwang 11:dccbaa9274c5 75 c->user->UpdateThrottle();
nki 36:11766b5da6ed 76 //c->user->throttle = c->filter_th->Update(c->user->throttle);
bwang 11:dccbaa9274c5 77 }
bwang 11:dccbaa9274c5 78
bwang 11:dccbaa9274c5 79 void debug(Context *c) {
nki 37:09373294ff22 80 //c->serial->printf("%f %f %f %f %f %f\n\r", dbg_d_filtered, dbg_q_filtered, dbg_ref_d, dbg_ref_q, dbg_loop_d, dbg_loop_q);
nki 36:11766b5da6ed 81 //c->serial->printf("%f\n\r", dbg_angle);
nki 40:b2bd9a097e3f 82 //c->serial->printf("%f %f %f %f\n\r", c->user->throttle, dbg_dtcA, dbg_dtcB, dbg_dtcC);
bwang 24:f1ff9c7256b5 83 }
bwang 24:f1ff9c7256b5 84
bwang 24:f1ff9c7256b5 85 void log(Context *c) {
nki 37:09373294ff22 86 //c->debugger->Write(0, dbg_dtc_b);
bwang 38:232c51b6853f 87 //c->debugger->Write(1, dbg_q_filtered);
nki 37:09373294ff22 88 //c->debugger->Write(1, dbg_dtc_b);
nki 37:09373294ff22 89 //c->debugger->Write(2, dbg_dtc_c);
bwang 11:dccbaa9274c5 90 }