callbacks.cpp

00001 #include "includes.h"
00002 #include "transforms.h"
00003 #include "filters.h"
00004 #include "context.h"
00005 #include "core.h"
00006 #include "meta.h"
00007 #include "sensors.h"
00008 #include "debug.h"
00009 
00010 void fast(Context *c) {
00011     float alpha, beta, d, q, ref_d, ref_q, valpha, vbeta;
00012     float I_b, I_c, angle, d_filtered, q_filtered, vd, vq;
00013     dbg_ib = I_b = c->motor->GetCurrentB();
00014     dbg_ic = I_c = c->motor->GetCurrentC();
00015     dbg_angle = angle = c->motor->GetPosition();
00016     
00017     Clarke(-(I_b + I_c), I_b, &alpha, &beta);
00018     Parke(alpha, beta, angle, &d, &q);
00019     
00020     dbg_d_filtered = d_filtered = c->filter_q->Update(q); //swapped  
00021     dbg_q_filtered = q_filtered = -c->filter_d->Update(d); //these and added a negative on this line. 
00022     
00023     //The necessary changes were analytically determined by observing that a postive vd command corresponded to a reading of negative q axis torque
00024     //and that a positive vq command produced a reading of positive d axis torque.  
00025     //This may be the final step in aligning the handedness of the reference and the output.
00026     
00027     c->reference->GetReference(angle, c->user->throttle, &ref_d, &ref_q);
00028     
00029     dbg_ref_d = ref_d;
00030     dbg_ref_q = ref_q;
00031     dbg_loop_d = vd = c->pid_d->Update(ref_d, d_filtered);
00032     dbg_loop_q = vq = c->pid_q->Update(ref_q, q_filtered);
00033     
00034     /*
00035     
00036     if(c->user->throttle <= 0.1f){
00037         float ramp = (c->user->throttle*10.0f)*(c->user->throttle*10.0f);
00038         vd = ramp*vd;
00039         vq = ramp*vq;
00040         }
00041     
00042     */
00043         
00044     dbg_loop_d = vd;
00045     dbg_loop_q = vq;
00046         
00047     InverseParke(vd, vq, angle, &valpha, &vbeta);
00048     
00049     dbg_valpha = valpha;
00050     dbg_vbeta = vbeta;
00051     
00052     c->modulator->Update(valpha, vbeta); 
00053 }
00054 
00055 void slow(Context *c) {
00056     c->user->UpdateThrottle();
00057     //c->user->throttle = c->filter_th->Update(c->user->throttle);
00058 }
00059 
00060 void debug(Context *c) {
00061     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);
00062     //c->serial->printf("%f\n\r", dbg_angle);
00063     //c->serial->printf("%f\n\r", c->user->throttle);
00064 }
00065 
00066 void log(Context *c) {
00067     //c->debugger->Write(0, dbg_angle);
00068     //c->debugger->Write(1, dbg_q_filtered);
00069 }