diff --git a/speeduino/auxiliaries.h b/speeduino/auxiliaries.h index 8a67541e..8d535704 100644 --- a/speeduino/auxiliaries.h +++ b/speeduino/auxiliaries.h @@ -17,10 +17,15 @@ volatile unsigned int boost_pwm_cur_value; long boost_pwm_target_value; long boost_cl_target_boost; byte boostCounter; +//Boost control uses a scaling factor of 100 on the MAP reading and MAP target in order to have a reasonable response time +//These are the values that are passed to the PID controller +long MAPx100; +long boostTargetx100; volatile bool vvt_pwm_state; unsigned int vvt_pwm_max_count; //Used for variable PWM frequency volatile unsigned int vvt_pwm_cur_value; long vvt_pwm_target_value; + #endif diff --git a/speeduino/auxiliaries.ino b/speeduino/auxiliaries.ino index ec71dcf9..69f24d25 100644 --- a/speeduino/auxiliaries.ino +++ b/speeduino/auxiliaries.ino @@ -3,7 +3,8 @@ Speeduino - Simple engine management for the Arduino Mega 2560 platform Copyright (C) Josh Stewart A full copy of the license may be found in the projects root directory */ -integerPID boostPID(¤tStatus.MAP, &boost_pwm_target_value, &boost_cl_target_boost, configPage3.boostKP, configPage3.boostKI, configPage3.boostKD, DIRECT); //This is the PID object if that algorithm is used. Needs to be global as it maintains state outside of each function call +//integerPID boostPID(¤tStatus.MAP, &boost_pwm_target_value, &boost_cl_target_boost, configPage3.boostKP, configPage3.boostKI, configPage3.boostKD, DIRECT); //This is the PID object if that algorithm is used. Needs to be global as it maintains state outside of each function call +integerPID boostPID(&MAPx100, &boost_pwm_target_value, &boostTargetx100, configPage3.boostKP, configPage3.boostKI, configPage3.boostKD, DIRECT); //This is the PID object if that algorithm is used. Needs to be global as it maintains state outside of each function call /* Fan control @@ -48,6 +49,7 @@ void initialiseAuxPWM() TIMSK1 |= (1 << OCIE1B); //Turn on the B compare unit (ie turn on the interrupt) boostPID.SetOutputLimits(0, boost_pwm_max_count); + boostPID.SetTunings(configPage3.boostKP, configPage3.boostKI, configPage3.boostKD); boostPID.SetMode(AUTOMATIC); //Turn PID on boostCounter = 0; @@ -58,10 +60,17 @@ void boostControl() if(configPage3.boostEnabled) { if(currentStatus.MAP < 100) { TIMSK1 &= ~(1 << OCIE1A); digitalWrite(pinBoost, LOW); return; } //Set duty to 0 and turn off timer compare + MAPx100 = currentStatus.MAP * 100; + boost_cl_target_boost = get3DTableValue(&boostTable, currentStatus.TPS, currentStatus.RPM) * 2; //Boost target table is in kpa and divided by 2 + boostTargetx100 = boost_cl_target_boost * 100; currentStatus.boostTarget = boost_cl_target_boost >> 1; //Boost target is sent as a byte value to TS and so is divided by 2 + if(currentStatus.boostTarget == 0) { TIMSK1 &= ~(1 << OCIE1A); digitalWrite(pinBoost, LOW); return; } //Set duty to 0 and turn off timer compare if the target is 0 + if( (boostCounter & 31) == 1) { boostPID.SetTunings(configPage3.boostKP, configPage3.boostKI, configPage3.boostKD); } //This only needs to be run very infrequently, once every 32 calls to boostControl(). This is approx. once per second + boostPID.Compute(); + TIMSK1 |= (1 << OCIE1A); //Turn on the compare unit (ie turn on the interrupt) } else { TIMSK1 &= ~(1 << OCIE1A); } // Disable timer channel diff --git a/speeduino/src/PID_v1/PID_v1.cpp b/speeduino/src/PID_v1/PID_v1.cpp index 4d30a2bc..2bc90449 100755 --- a/speeduino/src/PID_v1/PID_v1.cpp +++ b/speeduino/src/PID_v1/PID_v1.cpp @@ -14,19 +14,19 @@ #include "PID_v1.h" /*Constructor (...)********************************************************* - * The parameters specified here are those for for which we can't set up + * The parameters specified here are those for for which we can't set up * reliable defaults, so we need to have the user set them. ***************************************************************************/ PID::PID(long* Input, long* Output, long* Setpoint, byte Kp, byte Ki, byte Kd, byte ControllerDirection) { - + myOutput = Output; myInput = Input; mySetpoint = Setpoint; inAuto = false; - - PID::SetOutputLimits(0, 255); //default output limit corresponds to + + PID::SetOutputLimits(0, 255); //default output limit corresponds to //the arduino pwm limits //SampleTime = 100; //default Controller Sample Time is 0.1 seconds @@ -34,16 +34,16 @@ PID::PID(long* Input, long* Output, long* Setpoint, PID::SetControllerDirection(ControllerDirection); PID::SetTunings(Kp, Ki, Kd); - //lastTime = millis()-SampleTime; + //lastTime = millis()-SampleTime; } - - + + /* Compute() ********************************************************************** * This, as they say, is where the magic happens. this function should be called * every time "void loop()" executes. the function will decide for itself whether a new * pid Output needs to be computed. returns true when the output is computed, * false when nothing has been done. - **********************************************************************************/ + **********************************************************************************/ bool PID::Compute() { if(!inAuto) return false; @@ -58,14 +58,14 @@ bool PID::Compute() if(ITerm > outMax) ITerm= outMax; else if(ITerm < outMin) ITerm= outMin; long dInput = (input - lastInput); - + /*Compute PID Output*/ long output = (kp * error)/100 + ITerm- (kd * dInput)/100; - + if(output > outMax) output = outMax; else if(output < outMin) output = outMin; *myOutput = output; - + /*Remember some variables for next time*/ lastInput = input; //lastTime = now; @@ -76,18 +76,18 @@ bool PID::Compute() /* SetTunings(...)************************************************************* - * This function allows the controller's dynamic performance to be adjusted. + * This function allows the controller's dynamic performance to be adjusted. * it's called automatically from the constructor, but tunings can also * be adjusted on the fly during normal operation - ******************************************************************************/ + ******************************************************************************/ void PID::SetTunings(byte Kp, byte Ki, byte Kd) { if (Kp<0 || Ki<0 || Kd<0) return; - + dispKp = Kp; dispKi = Ki; dispKd = Kd; - + /* - double SampleTimeInSec = ((double)SampleTime)/1000; + double SampleTimeInSec = ((double)SampleTime)/1000; kp = Kp; ki = Ki * SampleTimeInSec; kd = Kd / SampleTimeInSec; @@ -96,7 +96,7 @@ void PID::SetTunings(byte Kp, byte Ki, byte Kd) kp = Kp; ki = (Ki * 100) / InverseSampleTimeInSec; kd = (Kd * InverseSampleTimeInSec) / 100; - + if(controllerDirection ==REVERSE) { kp = (0 - kp); @@ -104,9 +104,9 @@ void PID::SetTunings(byte Kp, byte Ki, byte Kd) kd = (0 - kd); } } - + /* SetSampleTime(...) ********************************************************* - * sets the period, in Milliseconds, at which the calculation is performed + * sets the period, in Milliseconds, at which the calculation is performed ******************************************************************************/ void PID::SetSampleTime(int NewSampleTime) { @@ -119,7 +119,7 @@ void PID::SetSampleTime(int NewSampleTime) SampleTime = (unsigned long)NewSampleTime; } } - + /* SetOutputLimits(...)**************************************************** * This function will be used far more often than SetInputLimits. while * the input to the controller will generally be in the 0-1023 range (which is @@ -133,12 +133,12 @@ void PID::SetOutputLimits(long Min, long Max) if(Min >= Max) return; outMin = Min; outMax = Max; - + if(inAuto) { if(*myOutput > outMax) *myOutput = outMax; else if(*myOutput < outMin) *myOutput = outMin; - + if(ITerm > outMax) ITerm= outMax; else if(ITerm < outMin) ITerm= outMin; } @@ -148,7 +148,7 @@ void PID::SetOutputLimits(long Min, long Max) * Allows the controller Mode to be set to manual (0) or Automatic (non-zero) * when the transition from manual to auto occurs, the controller is * automatically initialized - ******************************************************************************/ + ******************************************************************************/ void PID::SetMode(int Mode) { bool newAuto = (Mode == AUTOMATIC); @@ -158,11 +158,11 @@ void PID::SetMode(int Mode) } inAuto = newAuto; } - + /* Initialize()**************************************************************** * does all the things that need to happen to ensure a bumpless transfer * from manual to automatic mode. - ******************************************************************************/ + ******************************************************************************/ void PID::Initialize() { ITerm = *myOutput; @@ -172,7 +172,7 @@ void PID::Initialize() } /* SetControllerDirection(...)************************************************* - * The PID will either be connected to a DIRECT acting process (+Output leads + * The PID will either be connected to a DIRECT acting process (+Output leads * to +Input) or a REVERSE acting process(+Output leads to -Input.) we need to * know which one, because otherwise we may increase the output when we should * be decreasing. This is called from the constructor. @@ -184,13 +184,13 @@ void PID::SetControllerDirection(byte Direction) kp = (0 - kp); ki = (0 - ki); kd = (0 - kd); - } + } controllerDirection = Direction; } /* Status Funcions************************************************************* * Just because you set the Kp=-1 doesn't mean it actually happened. these - * functions query the internal state of the PID. they're here for display + * functions query the internal state of the PID. they're here for display * purposes. this are the functions the PID Front-end uses for example ******************************************************************************/ byte PID::GetKp(){ return dispKp; } @@ -200,19 +200,19 @@ int PID::GetMode(){ return inAuto ? AUTOMATIC : MANUAL;} int PID::GetDirection(){ return controllerDirection;} /*Constructor (...)********************************************************* - * The parameters specified here are those for for which we can't set up + * The parameters specified here are those for for which we can't set up * reliable defaults, so we need to have the user set them. ***************************************************************************/ integerPID::integerPID(long* Input, long* Output, long* Setpoint, byte Kp, byte Ki, byte Kd, byte ControllerDirection) { - + myOutput = Output; myInput = (long*)Input; mySetpoint = Setpoint; inAuto = false; - - integerPID::SetOutputLimits(0, 255); //default output limit corresponds to + + integerPID::SetOutputLimits(0, 255); //default output limit corresponds to //the arduino pwm limits SampleTime = 100; //default Controller Sample Time is 0.1 seconds @@ -220,61 +220,62 @@ integerPID::integerPID(long* Input, long* Output, long* Setpoint, integerPID::SetControllerDirection(ControllerDirection); integerPID::SetTunings(Kp, Ki, Kd); - lastTime = millis()-SampleTime; + lastTime = millis()-SampleTime; } - - + + /* Compute() ********************************************************************** * This, as they say, is where the magic happens. this function should be called * every time "void loop()" executes. the function will decide for itself whether a new * pid Output needs to be computed. returns true when the output is computed, * false when nothing has been done. - **********************************************************************************/ + **********************************************************************************/ bool integerPID::Compute() { if(!inAuto) return false; unsigned long now = millis(); //SampleTime = (now - lastTime); unsigned long timeChange = (now - lastTime); - if(timeChange>=SampleTime) + if(timeChange >= SampleTime) { /*Compute all the working error variables*/ - long input = *myInput; + long input = *myInput; long error = *mySetpoint - input; - ITerm+= (ki * error)/1000; //Note that ki is multiplied by 1000 rather than 100, so it must be divided by 1000 here - if(ITerm > outMax) ITerm= outMax; - else if(ITerm < outMin) ITerm= outMin; + ITerm += (ki * error)/1000; //Note that ki is multiplied by 1000 rather than 100, so it must be divided by 1000 here + if(ITerm > outMax) { ITerm = outMax; } + else if(ITerm < outMin) { ITerm = outMin; } long dInput = (input - lastInput); - + /*Compute PID Output*/ long output = (kp * error)/100 + ITerm - (kd * dInput)/100; - - if(output > outMax) output = outMax; + + if(output > outMax) output = outMax; else if(output < outMin) output = outMin; - *myOutput = output; - + *myOutput = output; + /*Remember some variables for next time*/ lastInput = input; lastTime = now; - return true; + + return true; } else return false; } /* SetTunings(...)************************************************************* - * This function allows the controller's dynamic performance to be adjusted. + * This function allows the controller's dynamic performance to be adjusted. * it's called automatically from the constructor, but tunings can also * be adjusted on the fly during normal operation - ******************************************************************************/ + ******************************************************************************/ void integerPID::SetTunings(byte Kp, byte Ki, byte Kd) { if (Kp<0 || Ki<0 || Kd<0) return; if ( dispKp == Kp && dispKi == Ki && dispKd == Kd ) return; //Only do anything if one of the values has changed dispKp = Kp; dispKi = Ki; dispKd = Kd; - + /* - double SampleTimeInSec = ((double)SampleTime)/1000; + double SampleTimeInSec = ((double)SampleTime)/1000; kp = Kp; ki = Ki * SampleTimeInSec; kd = Kd / SampleTimeInSec; @@ -283,7 +284,7 @@ void integerPID::SetTunings(byte Kp, byte Ki, byte Kd) kp = Kp; ki = (long)((long)Ki * 1000) / InverseSampleTimeInSec; kd = ((long)Kd * InverseSampleTimeInSec) / 100; - + if(controllerDirection == REVERSE) { kp = (0 - kp); @@ -291,13 +292,13 @@ void integerPID::SetTunings(byte Kp, byte Ki, byte Kd) kd = (0 - kd); } } - + /* SetSampleTime(...) ********************************************************* - * sets the period, in Milliseconds, at which the calculation is performed + * sets the period, in Milliseconds, at which the calculation is performed ******************************************************************************/ void integerPID::SetSampleTime(int NewSampleTime) { - if (SampleTime == (unsigned long)NewSampleTime) return; //If new value = old value, no action required. + if (SampleTime == (unsigned long)NewSampleTime) return; //If new value = old value, no action required. if (NewSampleTime > 0) { unsigned long ratioX1000 = (unsigned long)(NewSampleTime * 1000) / (unsigned long)SampleTime; @@ -307,7 +308,7 @@ void integerPID::SetSampleTime(int NewSampleTime) SampleTime = (unsigned long)NewSampleTime; } } - + /* SetOutputLimits(...)**************************************************** * This function will be used far more often than SetInputLimits. while * the input to the controller will generally be in the 0-1023 range (which is @@ -321,12 +322,12 @@ void integerPID::SetOutputLimits(long Min, long Max) if(Min >= Max) return; outMin = Min; outMax = Max; - + if(inAuto) { if(*myOutput > outMax) *myOutput = outMax; else if(*myOutput < outMin) *myOutput = outMin; - + if(ITerm > outMax) ITerm= outMax; else if(ITerm < outMin) ITerm= outMin; } @@ -336,7 +337,7 @@ void integerPID::SetOutputLimits(long Min, long Max) * Allows the controller Mode to be set to manual (0) or Automatic (non-zero) * when the transition from manual to auto occurs, the controller is * automatically initialized - ******************************************************************************/ + ******************************************************************************/ void integerPID::SetMode(int Mode) { bool newAuto = (Mode == AUTOMATIC); @@ -346,11 +347,11 @@ void integerPID::SetMode(int Mode) } inAuto = newAuto; } - + /* Initialize()**************************************************************** * does all the things that need to happen to ensure a bumpless transfer * from manual to automatic mode. - ******************************************************************************/ + ******************************************************************************/ void integerPID::Initialize() { ITerm = *myOutput; @@ -360,7 +361,7 @@ void integerPID::Initialize() } /* SetControllerDirection(...)************************************************* - * The PID will either be connected to a DIRECT acting process (+Output leads + * The PID will either be connected to a DIRECT acting process (+Output leads * to +Input) or a REVERSE acting process(+Output leads to -Input.) we need to * know which one, because otherwise we may increase the output when we should * be decreasing. This is called from the constructor. @@ -372,13 +373,13 @@ void integerPID::SetControllerDirection(byte Direction) kp = (0 - kp); ki = (0 - ki); kd = (0 - kd); - } + } controllerDirection = Direction; } /* Status Funcions************************************************************* * Just because you set the Kp=-1 doesn't mean it actually happened. these - * functions query the internal state of the PID. they're here for display + * functions query the internal state of the PID. they're here for display * purposes. this are the functions the PID Front-end uses for example ******************************************************************************/ byte integerPID::GetKp(){ return dispKp; }