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, speed;
00013     dbg_ib = I_b = c->motor->GetCurrentB();
00014     dbg_ic = I_c = c->motor->GetCurrentC();
00015     dbg_angle = angle = c->motor->GetPosition();
00016     float speed_raw = c->motor->GetSpeed();
00017     dbg_speed = speed = c->filter_sp->Update(speed_raw);
00018     //float delta = speed / 5.0f;
00019     float delta = 0.0f;
00020     if (delta > 70.0f) delta = 70.0f;
00021     angle += delta;
00022     if (angle >= 360.0f) angle -= 360.0f;
00023     if (angle < 0.0f) angle += 360.0f;
00024     
00025     Clarke(-(I_b + I_c), I_b, &alpha, &beta);
00026     Parke(alpha, beta, angle, &d, &q);
00027     dbg_q_raw = q;
00028     dbg_d_raw = d;
00029     
00030     dbg_d_filtered = d_filtered = c->filter_q->Update(q); //swapped  
00031     dbg_q_filtered = q_filtered = -c->filter_d->Update(d); //these and added a negative on this line. 
00032     
00033     //The necessary changes were analytically determined by observing that a postive vd command corresponded to a reading of negative q axis torque
00034     //and that a positive vq command produced a reading of positive d axis torque.  
00035     //This may be the final step in aligning the handedness of the reference and the output.
00036     
00037     c->user->UpdateThrottle();
00038     c->user->throttle = c->filter_th->Update(c->user->throttle);
00039     c->reference->GetReference(angle, c->user->throttle, &ref_d, &ref_q);
00040     
00041     dbg_ref_d = ref_d;
00042     dbg_ref_q = ref_q;
00043     dbg_loop_d = vd = c->pid_d->Update(ref_d, d_filtered);
00044     dbg_loop_q = vq = c->pid_q->Update(ref_q, q_filtered);
00045 
00046     dbg_throttle = c->user->throttle;
00047     
00048     if(c->user->throttle <= 0.05f){  //disable inverter - prevent whining, allow for safe precharge.
00049         c->inverter->Disable();
00050         c->pid_d->ZeroIntegrator();
00051         c->pid_q->ZeroIntegrator();
00052         }
00053     else {
00054         c->inverter->Enable();
00055     }
00056     
00057         
00058     dbg_loop_d = vd;
00059     dbg_loop_q = vq;
00060         
00061     InverseParke(vd, vq, angle, &valpha, &vbeta);
00062     
00063     dbg_valpha = valpha;
00064     dbg_vbeta = vbeta;
00065     
00066     c->modulator->Update(valpha, vbeta); 
00067 }
00068 
00069 void slow(Context *c) {
00070     //c->user->UpdateThrottle();
00071     //c->user->throttle = c->filter_th->Update(c->user->throttle);
00072 }
00073 
00074 void debug(Context *c) {
00075     //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);
00076     //c->serial->printf("%f\n\r", dbg_angle);
00077     //c->serial->printf("%f\n\r", c->user->throttle);
00078     //c->serial->printf("%f %f\n\r", dbg_ib, dbg_ic);
00079 }
00080 
00081 void log(Context *c) {
00082     //c->debugger->Write(0, dbg_speed);
00083     //c->debugger->Write(1, dbg_ib);
00084     //c->debugger->Write(2, dbg_ic);
00085     //c->debugger->Write(3, dbg_q_filtered);
00086 }