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@56:85a26f839af2, 2016-01-31 (annotated)

Committer:
bwang
Date:
Sun Jan 31 06:44:58 2016 +0000
Revision:
56:85a26f839af2
Parent:
55:f102d271e808 
latest rev

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;
bwang 56:85a26f839af2 12 float I_b, I_c, angle, d_filtered, q_filtered, vd, vq, speed;
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 54:e8d9bc885723 16 float speed_raw = c->motor->GetSpeed();
bwang 56:85a26f839af2 17 dbg_speed = speed = c->filter_sp->Update(speed_raw);
bwang 56:85a26f839af2 18 //float delta = speed / 5.0f;
bwang 56:85a26f839af2 19 float delta = 0.0f;
bwang 56:85a26f839af2 20 if (delta > 70.0f) delta = 70.0f;
bwang 56:85a26f839af2 21 angle += delta;
bwang 56:85a26f839af2 22 if (angle >= 360.0f) angle -= 360.0f;
bwang 56:85a26f839af2 23 if (angle < 0.0f) angle += 360.0f;
bwang 24:f1ff9c7256b5 24
bwang 24:f1ff9c7256b5 25 Clarke(-(I_b + I_c), I_b, &alpha, &beta);
bwang 11:dccbaa9274c5 26 Parke(alpha, beta, angle, &d, &q);
bwang 53:ef62d7a958f2 27 dbg_q_raw = q;
bwang 54:e8d9bc885723 28 dbg_d_raw = d;
bwang 11:dccbaa9274c5 29
nki 34:bfe180de813a 30 dbg_d_filtered = d_filtered = c->filter_q->Update(q); //swapped
nki 34:bfe180de813a 31 dbg_q_filtered = q_filtered = -c->filter_d->Update(d); //these and added a negative on this line.
nki 34:bfe180de813a 32
nki 34:bfe180de813a 33 //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 34 //and that a positive vq command produced a reading of positive d axis torque.
nki 34:bfe180de813a 35 //This may be the final step in aligning the handedness of the reference and the output.
nki 36:11766b5da6ed 36
bwang 53:ef62d7a958f2 37 c->user->UpdateThrottle();
bwang 53:ef62d7a958f2 38 c->user->throttle = c->filter_th->Update(c->user->throttle);
nki 33:e7b132029bae 39 c->reference->GetReference(angle, c->user->throttle, &ref_d, &ref_q);
bwang 24:f1ff9c7256b5 40
bwang 24:f1ff9c7256b5 41 dbg_ref_d = ref_d;
bwang 24:f1ff9c7256b5 42 dbg_ref_q = ref_q;
bwang 24:f1ff9c7256b5 43 dbg_loop_d = vd = c->pid_d->Update(ref_d, d_filtered);
bwang 24:f1ff9c7256b5 44 dbg_loop_q = vq = c->pid_q->Update(ref_q, q_filtered);
bwang 53:ef62d7a958f2 45
bwang 53:ef62d7a958f2 46 dbg_throttle = c->user->throttle;
nki 49:0603121a0538 47
nki 50:16b43e8fe04f 48 if(c->user->throttle <= 0.05f){ //disable inverter - prevent whining, allow for safe precharge.
nki 50:16b43e8fe04f 49 c->inverter->Disable();
nki 50:16b43e8fe04f 50 c->pid_d->ZeroIntegrator();
nki 50:16b43e8fe04f 51 c->pid_q->ZeroIntegrator();
nki 49:0603121a0538 52 }
nki 50:16b43e8fe04f 53 else {
nki 50:16b43e8fe04f 54 c->inverter->Enable();
nki 50:16b43e8fe04f 55 }
nki 49:0603121a0538 56
nki 36:11766b5da6ed 57
nki 36:11766b5da6ed 58 dbg_loop_d = vd;
nki 36:11766b5da6ed 59 dbg_loop_q = vq;
nki 36:11766b5da6ed 60
nki 34:bfe180de813a 61 InverseParke(vd, vq, angle, &valpha, &vbeta);
nki 27:846c08fb3697 62
nki 34:bfe180de813a 63 dbg_valpha = valpha;
nki 34:bfe180de813a 64 dbg_vbeta = vbeta;
bwang 11:dccbaa9274c5 65
bwang 11:dccbaa9274c5 66 c->modulator->Update(valpha, vbeta);
bwang 11:dccbaa9274c5 67 }
bwang 11:dccbaa9274c5 68
bwang 11:dccbaa9274c5 69 void slow(Context *c) {
bwang 53:ef62d7a958f2 70 //c->user->UpdateThrottle();
nki 36:11766b5da6ed 71 //c->user->throttle = c->filter_th->Update(c->user->throttle);
bwang 11:dccbaa9274c5 72 }
bwang 11:dccbaa9274c5 73
bwang 11:dccbaa9274c5 74 void debug(Context *c) {
nki 37:09373294ff22 75 //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 76 //c->serial->printf("%f\n\r", dbg_angle);
nki 49:0603121a0538 77 //c->serial->printf("%f\n\r", c->user->throttle);
nki 50:16b43e8fe04f 78 //c->serial->printf("%f %f\n\r", dbg_ib, dbg_ic);
bwang 24:f1ff9c7256b5 79 }
bwang 24:f1ff9c7256b5 80
bwang 24:f1ff9c7256b5 81 void log(Context *c) {
bwang 54:e8d9bc885723 82 //c->debugger->Write(0, dbg_speed);
bwang 56:85a26f839af2 83 //c->debugger->Write(1, dbg_ib);
bwang 56:85a26f839af2 84 //c->debugger->Write(2, dbg_ic);
bwang 56:85a26f839af2 85 //c->debugger->Write(3, dbg_q_filtered);
bwang 11:dccbaa9274c5 86 }