void setConsignePIDs(float vg, float vd) { PID1.controlReference = Q15(vg) ; /*Set the Reference Input for your controller */ PID2.controlReference = Q15(vd) ; /*Set the Reference Input for your controller */ }
void ColourEngine::SetMode(mode_t mode, q15 param) { current_mode = mode; // Reset colour in manual mode if (mode == mManual) { current_rgbw = RGBWColour(Q15(1.0), Q15(1.0), Q15(1.0), Q15(1.0)); } mode_param = param; }
void pidHWSetFloatCoeffs(tPID* controller, float Kp, float Ki, float Kd) { fractional kCoeffs[3]; kCoeffs[0] = Q15(Kp); kCoeffs[1] = Q15(Ki); kCoeffs[2] = Q15(Kd); PIDCoeffCalc(&kCoeffs[0], controller); }
void runPIDs(){ PID1.measuredOutput = Q15(v1) ; PID2.measuredOutput = Q15(v2) ; PID(&PID1); /*Call the PID controller using the new measured input */ PID(&PID2); /*Call the PID controller using the new measured input */ /*The user may place a breakpoint on "PID(&fooPID)", halt the debugger,*/ /*tweak the measuredOutput variable within the watch window */ /*and then run the debugger again */ }
void ColourEngine::Tick() { // Animation if (brightness_fader.is_fading) { brightness_fader.update(); } switch (current_mode) { case mManual: break; // Do nothing, let SetRGBW control the colour case mHsvFade: { current_hsv.hue += mode_param; current_rgbw = current_hsv.to_rgbw(); break; } case mStrobe: { current_hsv.hue += Q15(0.01); current_rgbw = current_hsv.to_rgbw(); break; } } // Update the colour every tick. // We don't need to update it any faster than this Update(); }
int play_cgi(struct httpctl * ctl) { strcpy(rtsp_host, http_query_lookup(ctl, "host")); strcpy(rtsp_media, http_query_lookup(ctl, "mrl")); rtsp_port = atoi(http_query_lookup(ctl, "port")); audio_level = atoi(http_query_lookup(ctl, "vol")); if (audio_level > 800) audio_level = 800; if (rtsp_connect(&rtsp, rtsp_host, rtsp_port, rtsp_media) < 0) { int code; WARN("RTSP connection failed!"); send_play_form(ctl); send_error(ctl, "RTSP playback failed"); if ((code = rtsp_response_code(&rtsp)) > 200) { http_send(ctl, rtsp_iframe_html, sizeof(rtsp_iframe_html) - 1); } } else { audio_gain_set(audio_level * Q15(1.0) / 100); send_stop_form(ctl); } return http_send(ctl, footer_html, sizeof(footer_html) - 1); }
void __ISR(_TIMER_4_VECTOR, IPL2SOFT) tick_timer_isr() { INTClearFlag(INT_T4); ColourEngine::Tick(); //toggle(PIO_LED2); // Power toggle if (_PORT(PIO_BTN1) == HIGH) { last_btn1 = HIGH; } else if (last_btn1 == HIGH) { last_btn1 = LOW; power = (power == OFF) ? ON : OFF; ColourEngine::SetPower(power, 1000); } // Switch mode if (_PORT(PIO_BTN2) == HIGH) { last_btn2 = HIGH; } else if (last_btn2 == HIGH) { last_btn2 = LOW; if (mode++ == (int)ColourEngine::NUM_MODES-1) mode = 0; ColourEngine::SetMode(static_cast<ColourEngine::mode_t>(mode), Q15(0.0001)); } }
void ColourEngine::PowerOn(uint fade) { if (fade > 0) { brightness_fader.speed = q15(max(Q15_MAXINT / (fade/4), 1)); // Equivalent to (1.0f / fade) brightness_fader.on_finished = NULL; brightness_fader.start(Q15(1.0)); } Update(); PWMEnable(); }
void ColourEngine::PowerOff(uint fade) { if (fade == 0) { PWMDisable(); } else { brightness_fader.speed = q15(max(Q15_MAXINT / (fade/4), 1)); // Equivalent to (1.0f / fade) brightness_fader.on_finished = [](void) { brightness_fader.on_finished = NULL; PWMDisable(); }; brightness_fader.start(Q15(0.0)); } }
/* Main function demonstrating the use of PID(), PIDInit() and PIDCoeffCalc() functions from DSP library in MPLAB C30 v3.00 and higher */ int initPIDs(void) { /* Step 1: Initialize the PID data structure, PID */ PID1.abcCoefficients = &abcCoefficient1[0]; /*Set up pointer to derived coefficients */ PID1.controlHistory = &controlHistory1[0]; /*Set up pointer to controller history samples */ PIDInit(&PID1); /*Clear the controler history and the controller output */ kCoeffs1[0] = Q15(1.0); kCoeffs1[1] = Q15(0.0); kCoeffs1[2] = Q15(0.0); PIDCoeffCalc(&kCoeffs1[0], &PID); /*Derive the a,b, & c coefficients from the Kp, Ki & Kd */ PID2.abcCoefficients = &abcCoefficient2[0]; /*Set up pointer to derived coefficients */ PID2.controlHistory = &controlHistory2[0]; /*Set up pointer to controller history samples */ PIDInit(&PID2); /*Clear the controler history and the controller output */ kCoeffs2[0] = Q15(1.0); kCoeffs2[1] = Q15(0.0); kCoeffs2[2] = Q15(0.0); PIDCoeffCalc(&kCoeffs2[0], &PID2); /*Derive the a,b, & c coefficients from the Kp, Ki & Kd */ }
#include <p32xxxx.h> #include <cstdlib> #include "common.h" #include "fixedpoint.hpp" #include "math.hpp" #include "pwm.h" #include "hardware.h" #include "colourengine.hpp" #include "fader.hpp" ///// Static Member Initialization ///// q15 ColourEngine::mode_param = Q15(0.0); power_t ColourEngine::current_power = OFF; ColourEngine::mode_t ColourEngine::current_mode = mManual; ColourEngine::colourspace_t ColourEngine::current_colourspace = clSRGBW; q15 ColourEngine::current_brightness = Q15(0.0); RGBWColour ColourEngine::current_rgbw = RGBWColour(Q15(1.0), Q15(1.0), Q15(1.0), Q15(1.0)); HSVColour ColourEngine::current_hsv = HSVColour(Q15(0.0), Q15(1.0), Q15(1.0)); Fader ColourEngine::brightness_fader(¤t_brightness, 1, NULL); ///// Public Methods ///// void ColourEngine::Initialize() { // Nothing to initialize }
void SMC_Position_Estimation (SMC *s) { PUSHCORCON(); CORCONbits.SATA = 1; CORCONbits.SATB = 1; CORCONbits.ACCSAT = 1; CalcEstI(); CalcIError(); // Sliding control calculator if (_Q15abs(s->IalphaError) < s->MaxSMCError) { // s->Zalpha = (s->Kslide * s->IalphaError) / s->MaxSMCError // If we are in the linear range of the slide mode controller, // then correction factor Zalpha will be proportional to the // error (Ialpha - EstIalpha) and slide mode gain, Kslide. CalcZalpha(); } else if (s->IalphaError > 0) s->Zalpha = s->Kslide; else s->Zalpha = -s->Kslide; if (_Q15abs(s->IbetaError) < s->MaxSMCError) { // s->Zbeta = (s->Kslide * s->IbetaError) / s->MaxSMCError // If we are in the linear range of the slide mode controller, // then correction factor Zbeta will be proportional to the // error (Ibeta - EstIbeta) and slide mode gain, Kslide. CalcZbeta(); } else if (s->IbetaError > 0) s->Zbeta = s->Kslide; else s->Zbeta = -s->Kslide; // Sliding control filter -> back EMF calculator // s->Ealpha = s->Ealpha + s->Kslf * s->Zalpha - // s->Kslf * s->Ealpha // s->Ebeta = s->Ebeta + s->Kslf * s->Zbeta - // s->Kslf * s->Ebeta // s->EalphaFinal = s->EalphaFinal + s->KslfFinal * s->Ealpha // - s->KslfFinal * s->EalphaFinal // s->EbetaFinal = s->EbetaFinal + s->KslfFinal * s->Ebeta // - s->KslfFinal * s->EbetaFinal CalcBEMF(); // Rotor angle calculator -> Theta = atan(-EalphaFinal,EbetaFinal) s->Theta = atan2CORDIC(-s->EalphaFinal,s->EbetaFinal); AccumTheta += s->Theta - PrevTheta; PrevTheta = s->Theta; AccumThetaCnt++; if (AccumThetaCnt == IRP_PERCALC) { s->Omega = AccumTheta; AccumThetaCnt = 0; AccumTheta = 0; } // Q15(Omega) * 60 // Speed RPMs = ----------------------------- // SpeedLoopTime * Motor Poles // For example: // Omega = 0.5 // SpeedLoopTime = 0.001 // Motor Poles (pole pairs * 2) = 10 // Then: // Speed in RPMs is 3,000 RPMs // s->OmegaFltred = s->OmegaFltred + FilterCoeff * s->Omega // - FilterCoeff * s->OmegaFltred CalcOmegaFltred(); // Adaptive filter coefficients calculation // Cutoff frequency is defined as 2*_PI*electrical RPS // // Wc = 2*_PI*Fc. // Kslf = Tpwm*2*_PI*Fc // // Fc is the cutoff frequency of our filter. We want the cutoff frequency // be the frequency of the drive currents and voltages of the motor, which // is the electrical revolutions per second, or eRPS. // // Fc = eRPS = RPM * Pole Pairs / 60 // // Kslf is then calculated based on user parameters as follows: // First of all, we have the following equation for RPMS: // // RPM = (Q15(Omega) * 60) / (SpeedLoopTime * Motor Poles) // Let us use: Motor Poles = Pole Pairs * 2 // eRPS = RPM * Pole Pairs / 60), or // eRPS = (Q15(Omega) * 60 * Pole Pairs) / (SpeedLoopTime * Pole Pairs * 2 * 60) // Simplifying eRPS // eRPS = Q15(Omega) / (SpeedLoopTime * 2) // Using this equation to calculate Kslf // Kslf = Tpwm*2*_PI*Q15(Omega) / (SpeedLoopTime * 2) // Using diIrpPerCalc = SpeedLoopTime / Tpwm // Kslf = Tpwm*2*Q15(Omega)*_PI / (diIrpPerCalc * Tpwm * 2) // Simplifying: // Kslf = Q15(Omega)*_PI/diIrpPerCalc // // We use a second filter to get a cleaner signal, with the same coefficient // // Kslf = KslfFinal = Q15(Omega)*_PI/diIrpPerCalc // // What this allows us at the end is a fixed phase delay for theta compensation // in all speed range, since cutoff frequency is changing as the motor speeds up. // // Phase delay: Since cutoff frequency is the same as the input frequency, we can // define phase delay as being constant of -45 DEG per filter. This is because // the equation to calculate phase shift of this low pass filter is // arctan(Fin/Fc), and Fin/Fc = 1 since they are equal, hence arctan(1) = 45 DEG. // A total of -90 DEG after the two filters implemented (Kslf and KslfFinal). s->Kslf = s->KslfFinal = FracMpy(s->OmegaFltred,Q15(_PI / IRP_PERCALC)); // Since filter coefficients are dynamic, we need to make sure we have a minimum // so we define the lowest operation speed as the lowest filter coefficient if (s->Kslf < Q15(OMEGA0 * _PI / IRP_PERCALC)) { s->Kslf = Q15(OMEGA0 * _PI / IRP_PERCALC); s->KslfFinal = Q15(OMEGA0 * _PI / IRP_PERCALC); } // Theta compensation for different speed ranges. These are defined // based on Minimum and Maximum operating speed defined in UserParms.h if (s->OmegaFltred < Q15(OMEGA0)) { s->ThetaOffset = FracMpy(s->OmegaFltred,SLOPEFRAC0); s->ThetaOffset += s->OmegaFltred * SLOPEINT0; s->ThetaOffset += CONSTANT0; } else if (s->OmegaFltred < Q15(OMEGA1)) { s->ThetaOffset = FracMpy(s->OmegaFltred,SLOPEFRAC1); s->ThetaOffset += s->OmegaFltred * SLOPEINT1; s->ThetaOffset += CONSTANT1; } else if (s->OmegaFltred < Q15(OMEGA2)) { s->ThetaOffset = FracMpy(s->OmegaFltred,SLOPEFRAC2); s->ThetaOffset += s->OmegaFltred * SLOPEINT2; s->ThetaOffset += CONSTANT2; } else if (s->OmegaFltred < Q15(OMEGA3)) { s->ThetaOffset = FracMpy(s->OmegaFltred,SLOPEFRAC3); s->ThetaOffset += s->OmegaFltred * SLOPEINT3; s->ThetaOffset += CONSTANT3; } else if (s->OmegaFltred < Q15(OMEGA4)) { s->ThetaOffset = FracMpy(s->OmegaFltred,SLOPEFRAC4); s->ThetaOffset += s->OmegaFltred * SLOPEINT4; s->ThetaOffset += CONSTANT4; } else if (s->OmegaFltred < Q15(OMEGA5)) { s->ThetaOffset = FracMpy(s->OmegaFltred,SLOPEFRAC5); s->ThetaOffset += s->OmegaFltred * SLOPEINT5; s->ThetaOffset += CONSTANT5; } else s->ThetaOffset = DEFAULTCONSTANT; s->Theta = s->Theta + s->ThetaOffset; POPCORCON(); return; }
void SMCInit(SMC *s) { // R * Ts // Fsmopos = 1 - -------- // L // Ts // Gsmopos = ---- // L // Ts = Sampling Period. If sampling at PWM, Ts = 50 us // R = Phase Resistance. If not provided by motor datasheet, // measure phase to phase resistance with multimeter, and // divide over two to get phase resistance. If 4 Ohms are // measured from phase to phase, then R = 2 Ohms // L = Phase inductance. If not provided by motor datasheet, // measure phase to phase inductance with multimeter, and // divide over two to get phase inductance. If 2 mH are // measured from phase to phase, then L = 1 mH if (Q15(PHASERES * LOOPTIMEINSEC) > Q15(PHASEIND)) s->Fsmopos = Q15(0.0); else s->Fsmopos = Q15(1 - PHASERES * LOOPTIMEINSEC / PHASEIND); if (Q15(LOOPTIMEINSEC) > Q15(PHASEIND)) s->Gsmopos = Q15(0.99999); else s->Gsmopos = Q15(LOOPTIMEINSEC / PHASEIND); s->Kslide = Q15(SMCGAIN); s->MaxSMCError = Q15(MAXLINEARSMC); s->FiltOmCoef = Q15(OMEGA0 * _PI / IRP_PERCALC); // Cutoff frequency for omega filter // is minimum omega, or OMEGA0 return; }