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;
}
