3DP: RTOS and Heating

This post should finish the heating of a 3d printer bed and extruder within FreeRTOS. The code has been delayed to enable me to test to make sure it works. Finally! Over Temperature and Maintenance have been tested.

Free RTOS and PSOC

There have been several posts on the Cypress/Infineon community forum about FreeRTOS. I took a bloated port and slimmed it down for the PSOC 5. It also has a couple of communication tasks (which are used in the 3d Printer). This port will save a lot of time in your work, and allow you to see usable tasks implemented in conjunction with PSOC components.

So, if you need a minimal port of FreeRTOS, (which happens to be hosted on the CY8Ckit-059 board), see https://github.com/wmaxfield/FreeRTOS-Minimal. It handles several usual things, including USB and UART, so you are ready to go in terms of creating your own projects without spending hours trimming the FreeRTOS demo code base.

There are no dependencies on the 059 kit, it just happened to be what I was using at the time. Change the device for the project to host onto another PSOC 5LP device (or probably even a PSOC 6, but probably not on the M0 processor on that part).

There is a *lot* of code in this post. Basically, I have spent the last month or so getting the temperature control subsystem to work with FreeRTOS. I have it heating a left-over Mosaic 1.0 bed and extruder properly now. It turns out the PID control “defaults” that the other 3d printer folks say not to depend on is actually a *very good,* usable collection of settings (and may work no matter your platform).

PID Controller Task

In controlling the temperature, you need the PID algorithms. Fortunately there is a library out there that works, hosted onto Arduino. I down ported it to C, and am publishing the modified MIT class library code here. It will eventually be available on Github with the printer code once the 3d printer works in at least minimal form.

Down Port to C Notes

It is rather simple to down port an Object from C++ to C. What you have to first understand is that a C++ Class is simply a typedef (called Class) structure with pointers to the functions that are defined for that class. Fancy compiler work is done to make it objectified, but in a nutshell, that is it.

The choices in down porting are 1) put pointers to functions in the structure typedef and initialize them with code along with any variables, or 2) create a structure typedef with the variables only. Pass the structure to the functions in the Class and let the code use those structures to manipulate the variables.

I chose the second route in porting code from C++ to C, simply because pointers to functions can be overwritten since they live in RAM. I have enough problems getting my code to work without adding OOPS! layers on top of it.

In Arduino 3d printers, the classes are almost all Singleton classes, so using a structure pointer is not a big deal. Even so, the structure pointer allows for multiple “objects,” much like C++, if the code constrains its variables solely to the structures.

In the PID Controller system, I created a PID_TYPE structure, and passed it in as the first parameter. I usually call the function pointer variable “this,” which allows you to easily up port to C++ should the need arise.

PID Header File

The Class variables are now structure variables. PID_init() must be called to initialize the fake class.

#ifndef PID_v1_h
#define PID_v1_h
/****************************************************************
* Arduino PID Library - Version 1.2.1
* by Brett Beauregard <br3ttb at gmail> brettbeauregard.com
*
* This Library is licensed under the MIT License
***************************************************************/

    // downported to C from C++ Wade Maxfield 2020
    // modified to zero in on bed temperature faster with
    // less initial overshoot by resetting PID system close to the
    // desired temperature.
    //
    // * (P)roportional  
    // * (I)ntegral  
    // * (D)erivative  
    // controller for regulating bed temperature and Extruder temperature

#define LIBRARY_VERSION	1.2.1
#include <project.h>

#ifndef bool_type_defined
    #define bool_type_defined
    typedef int16_t bool;
#endif

  //Constants used in some of the functions below
  #define AUTOMATIC	1
  #define MANUAL	0
  #define DIRECT    0
  #define REVERSE   1
  #define P_ON_M    0              // proportional measurement specified
  #define P_ON_E    1              // proportional measurement on error == 1

typedef struct pid_struct {
	double  dispKp;				    // * we'll hold on to the tuning parameters in user-entered 
	double  dispKi;				    //   format for display purposes
	double  dispKd;				    //
    
	double  kp;                     // * (P)roportional Tuning Parameter
    double  ki;                     // * (I)ntegral Tuning Parameter
    double  kd;                     // * (D)erivative Tuning Parameter

	int16_t     controllerDirection;    // either DIRECT or REVERSE (input params are positive or negative)
	int16_t     pOn;                    // 

    double  Input;              //
    int16_t  Output;             //  
    double  Setpoint;           //  
			  
    unsigned long lastTime;
    double  outputSum, lastInput;
    int16_t  lastOutput;

    unsigned long SampleTime;
    double  outMin, outMax;
    bool inAuto, pOnE;
} PID_TYPE;    


    
  //commonly used functions **************************************************************************
    // * constructor.  links the PID to the Input, Output, and 
    //   Setpoint.  Initial tuning parameters are also set here.
    void  PID_init(PID_TYPE *pidPtr,
                    double Input, 
                    double Output, 
                    double Setpoint,
                    double Kp, 
                    double Ki, 
                    double Kd, 
                    int16_t MonitorRate,
                    int16_t POn, 
                    int16_t ControllerDirection); 

	
    void PID_SetMode(PID_TYPE *pidPtr,int Mode);               // * sets PID to either MANUAL (0) or AUTOMATIC (non-0)

    void PID_SetOutputLimits(PID_TYPE *pidPtr,double, double); // * clamps the output to a specific range. 0-255 by default, but									                      //   it's likely the user will want to change this depending on									                      //   the application


  //available but not commonly used functions ********************************************************
    // * While most users will set the tunings once in the 
    //   constructor, this function gives the user the option
    //   of changing tunings during runtime for Adaptive control
    void PID_SetTunings(PID_TYPE *this,double Kp, double Ki, double Kd, int ProportionalSetting);
	
    void PID_SetControllerDirection(PID_TYPE *pidPtr,int);	  // * Sets the Direction, or "Action" of the controller. DIRECT										  //   means the output will increase when error is positive. REVERSE									  //   means the opposite.  it's very unlikely that this will be needed									  //   once it is set in the constructor.
    
    void PID_SetSampleTime(PID_TYPE *pidPtr,int);              // * sets the frequency, in Milliseconds, with which 
//   the PID calculation is performed.  default is 100										  
//Display functions ****************************************************************
	double PID_GetKp(PID_TYPE *pidPtr);						  // These functions query the pid for interal values.
	double PID_GetKi(PID_TYPE *pidPtr);						  //  they were created mainly for the pid front-end,
	double PID_GetKd(PID_TYPE *pidPtr);						  // where it's important to know what is actually 
	int PID_GetMode(PID_TYPE *pidPtr);						  //  inside the PID.
	int PID_GetDirection(PID_TYPE *pidPtr);					  //
//  private:
	void PID_Initialize(PID_TYPE *pidPtr);
    
    bool PID_Compute(PID_TYPE *pidPtr);                 // * performs the PID calculation.  it should be
                                          //   called every time loop() cycles. ON/OFF and
                                          //   calculation frequency can be set using SetMode
                                          //   SetSampleTime respectively
#endif

PID Controller Functions

These functions are the backbone for the controller, called from the FreeRTOS task that controls either the Bed temperature or the Extruder Temperature.

/**********************************************************************************************
 * Arduino PID Library - Version 1.2.1
 * by Brett Beauregard <br3ttb at gmail> brettbeauregard.com
 *
 * This Library is licensed under the MIT License
 * Down-ported to C from C++ by Wade Maxfield, 2020
    // modified to zero in on bed temperature faster with
    // less overshoot.
    //
    // * (P)roportional  
    // * (I)ntegral  
    // * (D)erivative  
    // controller for regulating bed temperature and Extruder temperature

 **********************************************************************************************/

#include <project.h>
#include "FreeRTOS.h"
#include "task.h"
#include <includes.h>
#include <PID_v1.h>

#define  millis() xTaskGetTickCount() // FreeRTOS millisecond clock




/********************************************************
 * PID Adaptive Tuning Example
 * One of the benefits of the PID library is that you can
 * change the tuning parameters at any time.  this can be
 * helpful if we want the controller to be agressive at some
 * times, and conservative at others.   in the example below
 * we set the controller to use Conservative Tuning Parameters
 * when we're near setpoint and more agressive Tuning
 * Parameters when we're farther away.
 ********************************************************/

#include <PID_v1.h>

// the following is not used in FreeRTOS, left here for some examples
#if 0
//Define Variables we'll be connecting to
double Setpoint, Input, Output;

//Define the aggressive and conservative Tuning Parameters
double aggKp=4, aggKi=0.2, aggKd=1;
double consKp=1, consKi=0.05, consKd=0.25;

//Specify the links and initial tuning parameters
PID myPID(&Input, &Output, &Setpoint, consKp, consKi, consKd, DIRECT);

void setup()
{
  //initialize the variables we're linked to
  Input = analogRead(PIN_INPUT);
  Setpoint = 100;

  //turn the PID on
  myPID.SetMode(AUTOMATIC);
}

void loop()
{
  Input = analogRead(PIN_INPUT);

  double gap = abs(Setpoint-Input); //distance away from setpoint
  if (gap < 10)
  {  //we're close to setpoint, use conservative tuning parameters
    myPID.SetTunings(consKp, consKi, consKd);
  }
  else
  {
     //we're far from setpoint, use aggressive tuning parameters
     myPID.SetTunings(aggKp, aggKi, aggKd);
  }

  myPID.Compute();
  analogWrite(PIN_OUTPUT, Output);
}
#endif



/*Constructor (...)*********************************************************
 *    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.
 ***************************************************************************/
void PID_init(PID_TYPE *this,double Input, double Output, double Setpoint,
        double Kp, double Ki, double Kd,int16_t MonitorRate, int16_t POn, int16_t ControllerDirection)
{
    
    this->Output = Output;
    this->Input = Input;
    this->Setpoint = Setpoint;
    this->inAuto = false;

    PID_SetOutputLimits(this,0, 255);				//default output limit corresponds to
											//the current processor pwm limits for 8 bit pwm

    this->SampleTime = MonitorRate;					//default Controller Sample Time is 0.1 seconds

    this->controllerDirection = DIRECT;     // pre-initialize variable to default 
    PID_SetControllerDirection(this,ControllerDirection);

    PID_SetTunings(this,Kp, Ki, Kd, POn);   // this will also "reset" for controllerDirection
    

    this->lastTime = millis()-this->SampleTime; // check immediately at PID_Compute()
}


/* Compute() **********************************************************************
 *     This, as they say, is where the magic happens.  This function should be called
 *   every time your for(;;)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(PID_TYPE *this)
{
   if(!this->inAuto) 
     return false;

   unsigned long now = millis();
   unsigned long timeChange = (now - this->lastTime);

   if(timeChange>=this->SampleTime)  {
      /*Compute all the working error variables*/
      double input = this->Input;
      double error = this->Setpoint - input;
      double dInput = (input - this->lastInput);
    
      this->outputSum+= (this->ki * error);

      /*Add Proportional on Measurement, if P_ON_M is specified*/
      if(!this->pOnE) 
            this->outputSum-= this->kp * dInput;

      if(this->outputSum > this->outMax) 
            this->outputSum= this->outMax;
      else 
            if(this->outputSum < this->outMin) 
                this->outputSum= this->outMin;

      /*Add Proportional on Error, if P_ON_E is specified  (value != 0 )*/
	  double output=0;

     
      if(this->pOnE) 
        output = this->kp * error;
      //else 
      //  output = 0;

      /*Compute Rest of PID Output*/
      output += this->outputSum - this->kd * dInput;

        if(output > this->outMax) 
            output = this->outMax;
        else 
            if(output < this->outMin) 
                output = this->outMin;
            
        this->lastOutput = this->Output;
        this->Output = output;

      /*Remember some variables for next time*/
      this->lastInput = input;
      this->lastTime = now;
            
	    return true;
   } 


   return false;
}

/* SetTunings(...)*************************************************************
 * 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(PID_TYPE *this,double Kp, double Ki, double Kd, int ProportionalSetting)
{
   if (Kp<0 || Ki<0 || Kd<0) return;

   this->pOn = ProportionalSetting;  //pOn is either P_ON_E or P_ON_M
                    // if P_ON_M then proportional measurement specified always
                    // else it will switch to proportional measurement on error.
   this->pOnE = ProportionalSetting == P_ON_E;

   this->dispKp = Kp; 
    this->dispKi = Ki; 
    this->dispKd = Kd;

   double SampleTimeInSec = ((double)this->SampleTime)/1000;
   this->kp = Kp;
   this->ki = Ki * SampleTimeInSec;
   this->kd = Kd / SampleTimeInSec;

  if(this->controllerDirection ==REVERSE)
   {
      this->kp = (0 - this->kp);
      this->ki = (0 - this->ki);
      this->kd = (0 - this->kd);
   }
}


/* SetSampleTime(...) *********************************************************
 * sets the period, in Milliseconds, at which the calculation is performed
 ******************************************************************************/
void PID_SetSampleTime(PID_TYPE *this,int NewSampleTime)
{
   if (NewSampleTime > 0)
   {
      double ratio  = (double)NewSampleTime
                      / (double)this->SampleTime;
      this->ki *= ratio;
      this->kd /= ratio;
      this->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
 *  the default already)  the output will be a little different.  maybe they'll
 *  be doing a time window and will need 0-8000 or something.  or maybe they'll
 *  want to clamp it from 0-125.  who knows.  at any rate, that can all be done
 *  here.
 **************************************************************/
void PID_SetOutputLimits(PID_TYPE *this,double Min, double Max)
{
   if(Min >= Max) 
    return;

   this->outMin = Min;
   this->outMax = Max;

   if(this->inAuto)
   {
	   if(this->Output > this->outMax) 
            this->Output = this->outMax;
	   else 
        if(this->Output < this->outMin) 
            this->Output = this->outMin;

	   if(this->outputSum > this->outMax) 
            this->outputSum= this->outMax;
	   else 
            if(this->outputSum < this->outMin) 
                this->outputSum= this->outMin;
   }
}

/* SetMode(...)****************************************************************
 * 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(PID_TYPE *this,int Mode)
{
    bool newAuto = (Mode == AUTOMATIC);
    
    if(newAuto && !this->inAuto)
    {  /*we just went from manual to auto*/
        PID_Initialize(this);
    }
    this->inAuto = newAuto;
}

/* Initialize()****************************************************************
 *	does all the things that need to happen to ensure a bumpless transfer
 *  from manual to automatic mode.
 ******************************************************************************/
void PID_Initialize(PID_TYPE *this)
{
   this->outputSum = this->Output;
   this->lastInput = this->Input;
    
   if(this->outputSum > this->outMax) 
        this->outputSum = this->outMax;
   else 
        if(this->outputSum < this->outMin) 
            this->outputSum = this->outMin;
}

/* SetControllerDirection(...)*************************************************
 * 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.
 ***************************************************************/
void PID_SetControllerDirection(PID_TYPE *this,int Direction)
{
    //reverse sign
   if(this->inAuto && Direction !=this->controllerDirection)
   {
	  this->kp = (0 - this->kp);
      this->ki = (0 - this->ki);
      this->kd = (0 - this->kd);
   }
   this->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 this->  they're here for display
 * purposes.  this are the functions the PID Front-end uses for example
 ***************************************************************/
double PID_GetKp(PID_TYPE *this){ 
    return  this->dispKp; 
}
double PID_GetKi(PID_TYPE *this){ 
    return  this->dispKi;
}
double PID_GetKd(PID_TYPE *this){ 
    return  this->dispKd;
}
int PID_GetMode(PID_TYPE *this){ 
    return  this->inAuto ? AUTOMATIC : MANUAL;
}
int PID_GetDirection(PID_TYPE *this){ 
    return this->controllerDirection;
}

FreeRTOS Temperature Management Tasks

Here is the temperature management part of this system. There are two management tasks created in FreeRTOS, each controls one temperature. They share the PID library code. Note that each task is written as if nothing else in the system was happening. This is the strength of using a RTOS, you can write and think about your code as if nothing else was happening. (There are important exceptions to this paradigm, but that is a system design consideration.)

To do a “software PWM” under FreeRTOS, we do a vTaskDelay. If the system is properly designed, and the SOC is not fully occupied, the lower level heat control task will run often enough to control the temperature. If issues arise with not enough time being given to the control task, the system design either needs tweaking or you need to totally rethink the problem. In my 30+ years experience of using RTOS, I have never had to redo an entire design. In most cases, tweaking task priorities fixed the issues.

PID Temperature Control Tasks

Structure information
//---------------------------------------------
typedef struct myPidControls {
    double Input;
    double Output;

    //Define the aggressive and conservative Tuning Parameters
    double aggKp, aggKi, aggKd;
    double consKp, consKi, consKd;    
    
    int16_t *PWM;
    int16_t CurrentSlot;
    int16_t MaxSlotCount;
    #if !USE_HARDWARE_PWM_FOR_BED_HEATER
    int16_t PWM_Value;
    int16_t PWM_Sum;
    #endif
    int16_t offset;
    double  OffsetMultiplier; // amount to bend the knee when temperature has reached kneeTemperature
                                // Can be 1.0,  0.9,  0.7, etc.  Lower number bends knee more.
    double kneeTemperature; // temperature at which we should start the running sum over.
    
} MY_PID_CONTROLS;

//---------------------------------------------
typedef struct temperatureProtectionStruct {
    // immediate shutdown temperature
    uint32_t                FailHighTemperatureCausingShutdown;// equal to or above this, halt printer
    
        // when starting heater
    uint32_t                RampingTemperatureChangeMillisecondsWatchPeriod;       // 20000
    uint32_t                RampingTemperatureChangeDegreesC;        // 2
    uint32_t                RampingTemperatureBreachedAtThisMilliseconds;
        // maintaining temperature
    uint32_t                MaximumNumberOfMilliSecondsToWaitWhileMaintaining;       // 20000 
    uint32_t                TemperatureHysteresisDegreesCWhileMaintaining;      // 2   
    uint32_t                MaintainingTemperatureBreachedAtThisMilliseconds; // when breach occurs, record here.
    
    // temperature(s)
    double                  LastCurrentTemperature;
} TEMPERATURE_PROTECTION;

//---------------------------------------------
typedef struct temperatureControlStruct {
    double                  CurrentTemperature;

    double                  DesiredTemperature;
    enum TemperatureStatesEnum  TemperatureState;
    MY_PID_CONTROLS         PIDControls;
    PID_TYPE                PID;
    TEMPERATURE_PROTECTION  TemperatureProtection;
    char                    HeaterASCIIidString[32];// "0\0xa"
} TEMPERATURE_CONTROL;

//---------------------------------------------
extern TEMPERATURE_CONTROL gBedTempControl;
extern TEMPERATURE_CONTROL gE0TempControl;
Bed Task
/**
 * PSOCino 3d printer firmware, FreeRTOS version
 *
 * Copyright 2020, Wade Maxfield
 * Written by Wade Maxfield
 *
 * Commercial license Available.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 */
/* Scheduler include files. */
#include "main.h"
#include "printf.h"  //<---for temperature calculation prints
#include "PID_v1.h"
#include "gCodeVariables.h"
#include "mCodeVariables.h"
#include "MessageRouter.h"
#include "Messages.h"
// can't include stdlib.h, we are using our own printf
//#include <stdlib.h>  // abs function


MY_PID_CONTROLS ExtruderPIDControls;
MY_PID_CONTROLS BedPIDControls;

PID_TYPE BedPID;
PID_TYPE ExtruderPID;

#define BedTempSTACK_SIZE		(configMINIMAL_STACK_SIZE ) // uses printf
#define HeadTempSTACK_SIZE      (configMINIMAL_STACK_SIZE ) // 
#define TempCalcSTACK_SIZE      (configMINIMAL_STACK_SIZE *3) //floating point used

int16_t  BedTemperatureOnFlag;
double   BedTemperatureDesired; // to nearest degree
double   BedTemperature;
int16_t  BedPWM[BED_PWM_SLOTS]; 

int16_t  gExtruderTemperatureOnFlag;  // when true, Extruder should be heating 
double   gExtruderTemperatureDesired; // 
double   ExtruderTemperature;
int16_t  ExtruderPWM[EXTRUDER_PWM_SLOTS]; 


double LastBedTemperature;
/*-----------------------------------------------------------*/
//------------------------------------------------------------
// task that monitors the bed temperature a few times per second
// if the bed temperature is too low, it turns the bed heater on.
// if the bed temperature is too high, it turns the bed heater off.
//------------------------------------------------------------
static portTASK_FUNCTION( vBedTemperatureTask, pvParameters ){

 static   TaskMonitor_t *TaskMonitor = &TaskMonitorArray[BED_TEMPERATURE_TASK];
    
    (void) pvParameters;
    // PID VARIABLES
    // 24VDC heated bed.  (AC Heated Bed will have different PID values)    

        // initialize constants
    BedPIDControls.aggKp=20.22, BedPIDControls.aggKi=1.22, BedPIDControls.aggKd=84.07;
    BedPIDControls.consKp=BedPIDControls.aggKp/4, BedPIDControls.consKi=BedPIDControls.aggKi/4 BedPIDControls.consKd=BedPIDControls.aggKd/4;
    BedPIDControls.PWM = BedPWM;

#define BED_TEMPERATURE_KNEE_FOR_PID_CONTROL    1.0  // degrees away from target temp where PID slope 
                                                                 // calculations are forcibly reset
#define BED_TEMPERATURE_PID_KNEE_SLOPE_MULTIPLIER  0.70 // can be 1.0, 0.9, 0.7, etc

#define BED_PWM_SLOTS    10   // 10 ms * 10 == 0.1 second

    BedPIDControls.MaxSlotCount = BED_PWM_SLOTS;// value of 10
    
    BedPIDControls.kneeTemperature = BED_TEMPERATURE_KNEE_FOR_PID_CONTROL ;
    BedPIDControls.OffsetMultiplier = BED_TEMPERATURE_PID_KNEE_SLOPE_MULTIPLIER;
    
    PID_init(   &BedPID,
                BedPIDControls.Input, 
                BedPIDControls.Output, 
                BedTemperatureDesired,
                BedPIDControls.consKp, 
                BedPIDControls.consKi,
                BedPIDControls.consKd, 
                BED_TEMPERATURE_MONITOR_RATE_IN_MILLISECONDS,
                P_ON_M,  // specify proportional on input reading
                DIRECT);
    
    PID_SetMode(&BedPID,AUTOMATIC);

    for(;;) {
        
        vTaskDelay(pdMS_TO_TICKS(BED_TEMPERATURE_MONITOR_RATE_IN_MILLISECONDS));//Default: 20 times per second
        TaskMonitor->runCounter++;
        
        
        
        if (!BedTemperatureOnFlag) {// if bed temperature not on
            #if USE_HARDWARE_PWM_FOR_BED_HEATER
                PWM_BedHeater_WriteCompare(0);
            #else    
                PinWrite(BED_HEATER_PIN,LOW);// turn heater off
            #endif
            BedPIDControls.offset=0; // when turn off, reset the flag
            continue;
        }
        
        BedPID.Setpoint = BedTemperatureDesired;
        BedPIDControls.Input = BedTemperature;
                
        // we are "ON".  Call the PID function, which computes at 10 times per second (default)
        // and then controls the output
        handlePID(&BedPID, &BedPIDControls);
        #if USE_HARDWARE_PWM_FOR_BED_HEATER
            PWM_BedHeater_WriteCompare((int8_t)BedPID.Output);
        #else
            applySoftwarePWM(&BedPID, &BedPIDControls,BED_HEATER_PIN);
        #endif
    
    } 
}
Extruder Task
//-------------------------------------------------------------
// task that monitors the extruder temperature a few times per second
// Uses PID control 
//-------------------------------------------------------------
static portTASK_FUNCTION( vExtruderTemperatureTask, pvParameters ){

 static   TaskMonitor_t *TaskMonitor = &TaskMonitorArray[EXTRUDER_TEMPERATURE_TASK];
    
    (void) pvParameters;
   
    // initialize constants.  agg is Aggressive, cons is Conservative
    // these values are stored in EEPROM, in config structure, which is
    // initialized in main() before tasks run
    gE0TempControl.PIDControls.aggKp = 22.20;
    gE0TempControl.PIDControls.aggKi = 1.08;
    gE0TempControl.PIDControls.aggKd = 114.0;

//-----
//  the following is based on a temperature of around 200C  
//  50% lower temperatures
//  will experience overshoot of around 8 to 10 degrees C
#define EXTRUDER_TEMPERATURE_KNEE_FOR_PID_CONTROL           3.5 //-1==disable, degrees away from target temp where PID slope 
                                                                // calculations are forcibly reset
#define EXTRUDER_TEMPERATURE_PID_KNEE_SLOPE_MULTIPLIER    0.75 // can be 1.0, 0.9, 0.7, etc
#define EXTRUDER_TEMPERATURE_MONITOR_RATE_IN_MILLISECONDS   50
#define EXTRUDER_TEMPERATURE_PID_RATE_IN_MILLISECONDS       10   // 1==as fast as possible
// following is used to provide a "software" pwm for the PID (very sloppy)
#define EXTRUDER_PWM_SLOTS                                  10  // 10 ms * 10 == 0.1 second

    
    gE0TempControl.PIDControls.consKp=gE0TempControl.PIDControls.aggKp/4.0;
    gE0TempControl.PIDControls.consKi= gE0TempControl.PIDControls.aggKi/4.0;
    gE0TempControl.PIDControls.consKd= gE0TempControl.PIDControls.aggKd/4.0;
    
    gE0TempControl.PIDControls.PWM = ExtruderPWM;
    gE0TempControl.PIDControls.MaxSlotCount = EXTRUDER_PWM_SLOTS;
    gE0TempControl.PIDControls.kneeTemperature = EXTRUDER_TEMPERATURE_KNEE_FOR_PID_CONTROL;// 3.5 degrees
    gE0TempControl.PIDControls.OffsetMultiplier = EXTRUDER_TEMPERATURE_PID_KNEE_SLOPE_MULTIPLIER;//
    strncpy(gE0TempControl.HeaterASCIIidString ,"extruder 0",sizeof (gE0TempControl.HeaterASCIIidString));// 0.75
    
    #ifdef ENABLE_THERMAL_PROTECTION_HOTEND
        gE0TempControl.TemperatureProtection.MaximumNumberOfMilliSecondsToWaitWhileMaintaining = THERMAL_PROTECTION_HOTEND_TIME_IN_SECONDS*1000;       // 20 
        gE0TempControl.TemperatureProtection.TemperatureHysteresisDegreesCWhileMaintaining = THERMAL_PROTECTION_HOTEND_TEMPERATURE_HYSTERESIS_DEGREES_C;      // 2   
        // when starting heater
        gE0TempControl.TemperatureProtection.RampingTemperatureChangeMillisecondsWatchPeriod=THERMAL_HOTEND_START_HEAT_WATCH_PERIOD*1000;       // 20
        gE0TempControl.TemperatureProtection.RampingTemperatureChangeDegreesC= THERMAL_HOTEND_START_TEMPERATURE_INCREASE_DEGREES_C;        // 2
        gE0TempControl.TemperatureProtection.MaintainingTemperatureBreachedAtThisMilliseconds=0;
        gE0TempControl.TemperatureProtection.RampingTemperatureBreachedAtThisMilliseconds=0;
        gE0TempControl.TemperatureProtection.FailHighTemperatureCausingShutdown=THERMAL_PROTECTION_HOTEND_MAXIMUM_TEMPERATURE_DEGREES_C;
    #endif

    
    
    PID_init(   &gE0TempControl.PID,
                gE0TempControl.DesiredTemperature,
                gE0TempControl.PIDControls.aggKp, 
                gE0TempControl.PIDControls.aggKi,
                gE0TempControl.PIDControls.aggKd, 
                EXTRUDER_TEMPERATURE_PID_RATE_IN_MILLISECONDS,// calculation rate
                P_ON_E,  // specify proportional on error
                DIRECT);
    
    PID_SetMode(&gE0TempControl.PID,AUTOMATIC);
    
    for(;;) {
        
        TaskDelay(pdMS_TO_TICKS(EXTRUDER_TEMPERATURE_MONITOR_RATE_IN_MILLISECONDS),TaskMonitor);//Default: 10 times per second

        TaskMonitor->runCounter++;
        
        if (gE0TempControl.TemperatureState == tempOffState){
           #if USE_HARDWARE_PWM_FOR_BED_HEATER
                PWM_ExtruderHeater_WriteCompare(0);// turn off
            #else    
                PinWrite(EXTRUDER_HEATER_PIN,LOW);// turn heater off
            #endif
            gE0TempControl.PIDControls.offset=0; // when turn off, reset the knee flag
            gE0TempControl.PID.Output =0; // reset pwm
            //continue;
        } else {            
            // heater is "ON".  
            gE0TempControl.PIDControls.Input = gE0TempControl.CurrentTemperature;
            gE0TempControl.PID.Setpoint = gE0TempControl.DesiredTemperature;


            handlePID(&gE0TempControl.PID,&gE0TempControl.PIDControls);
            
            #ifdef ENABLE_THERMAL_PROTECTION_EXTRUDER
                handleThermalProtection(&gE0TempControl);
            #endif

            
            #if USE_HARDWARE_PWM_FOR_EXTRUDER_HEATER
                PWM_ExtruderHeater_WriteCompare((int8_t)gE0TempControl.PID.Output);
            #else
                applySoftwarePWM(&gE0TempControl.PID, &gE0TempControl.PIDControls,EXTRUDER_HEATER_PIN);
            #endif
        }
        
    } 
}

Handle PID Routine

We use this function to look for a “knee” temperature (where the temperature rise graph needs to take a turn to the right from a steep vertical, and reset the PID parameters to allow the system to “learn” from a minimum offset start. This keeps the initial overshoot to a minimum.

The code is here:


//-------------------------------------------------------------
//------------------------------------------------------------
//-------------------------------------------------------------
// change PID tuning to accommodate the knee point where we want to do less
// aggressive control.  Basically the theory is that the system has learned
// during the ramp up to go fast.  At a certain point (the "knee" point)
// the pid is reset to less aggressive and the learned information is wiped.
// This leads to less overshoot (and less undershoot during overshoot correction)
// and faster settling time.
//  Also an offsetMultiplier is used to force "teach" the system faster, if need.
//-------------------------------------------------------------
int16_t handlePID(PID_TYPE *PID, MY_PID_CONTROLS *PIDControls){        

   
    double gap =abs(PID->Setpoint-PIDControls->Input); //distance away from setpoint
    
    // If the temperature setpoint is 0, the heater is off
    if (PID->Setpoint==0){
        PID->Output=0;
        return 0;// return 0 pwm
    }
    
    // if we are past the knee temperature, change the sum.
    // this "teaching" flattens the overshoot
    if (!PIDControls->offset && PID->Input < PID->Setpoint) {
        if (PIDControls->kneeTemperature >=0 && gap < PIDControls->kneeTemperature){
                PIDControls->offset = 1;// reset the one-time flag
                PID->outputSum*=PIDControls->OffsetMultiplier; // reset the sum
        }    
    }
    
    // if kneeTemperature is 0 or greater, then reset PID to less aggressive values
    // if kneeTemperature is < 0, then do not reset the PID to less aggressive values.
    if (PIDControls->kneeTemperature >=0 && gap < PIDControls->kneeTemperature )    { 
        //we're close to setpoint, use conservative tuning parameters
        PID_SetTunings(PID,PIDControls->consKp, PIDControls->consKi, PIDControls->consKd,P_ON_E);
        
    }  else {
         //we're far from setpoint, use aggressive tuning parameters
         PID_SetTunings(PID,PIDControls->aggKp, PIDControls->aggKi, PIDControls->aggKd,P_ON_E);
    }
    
    PID->Input=PIDControls->Input;

    // if PID_Compute() returns true, output was recomputed
    PID_Compute(PID);

    return PID->Output;
}

PWM Hardware for PID Control of Heating

With the PSOC, we have easy access to hardware PWM. The PSOC 5LP only has 5 hardware PWM’s installed. This is not a problem, because there are plenty of UDB’s, and they are configured to be PWM’s. Here is the schematic entry for the PWM’s for the extruder and bed Heater:

Starting the FreeRTOS Tasks

Of Course, all of this has to be started. The start task function is called from main, and it controls all the temperature control pieces:

//-----------------------------------------
// set up the temperature calc tasks
//-----------------------------------------
void vAltStartTemperatureTasks( UBaseType_t uxPriorityBed, UBaseType_t uxPriorityExtruder, UBaseType_t uxPriorityCalculator )
{
    extern void initializeHeaterHardware();
    initializeHeaterHardware(); // HardwareInit.c
    
	xTaskCreate( vBedTemperatureTask, "BedTemp", BedTempSTACK_SIZE, NULL, uxPriorityBed, ( TaskHandle_t * ) &TaskMonitorArray[BED_TEMPERATURE_TASK].taskHandle );
	xTaskCreate( vExtruderTemperatureTask, "ExtruderTemp", HeadTempSTACK_SIZE, NULL, uxPriorityExtruder, ( TaskHandle_t * ) &TaskMonitorArray[EXTRUDER_TEMPERATURE_TASK].taskHandle );
	xTaskCreate( vTemperatureCalculateTask, "TempCalc", TempCalcSTACK_SIZE, NULL, uxPriorityCalculator, ( TaskHandle_t * ) &TaskMonitorArray[TEMPERATURE_CALC_TASK].taskHandle );
}
Calculate the State the Heater is in

//---------------------------------------------
enum TemperatureStatesEnum {
    tempOffState=0
    ,tempRampUpState        // turned on, ramp up
    ,tempReachedState       // reached
    ,tempMaintainedState    // keep here
    ,tempRampDownState      // turned off, ramp down
    ,tempFailState          // machine must be killed
    
};

char *TemperatureStatesString[] ={
     "tempOffState"
    ,"tempRampUpState"        // turned on, ramp up
    ,"tempReachedState"       // reached
    ,"tempMaintainedState"    // keep here
    ,"tempRampDownState"      // turned off, ramp down
    ,"tempFailState"          // machine must be killed

};
char *getTemperatureStatesString(enum TemperatureStatesEnum state){
    // protect from bad software
    if ((int)state <0 || state > (int)tempFailState)
        state=0;
    
    return TemperatureStatesString[state];
}
//-------------------------------------------------------------
// determine which state the heater is in
//-------------------------------------------------------------
void handleStateCalculation(TEMPERATURE_CONTROL *pTempControl){
                
    if (pTempControl->DesiredTemperature ) {
        // temp going up
        if (pTempControl->CurrentTemperature < pTempControl->DesiredTemperature ){
            pTempControl->TemperatureState = tempRampUpState;
        } else {
            // current temperature == to desired (with a small window)
            if (pTempControl->CurrentTemperature >=  pTempControl->DesiredTemperature -1.0
                && pTempControl->CurrentTemperature <=  pTempControl->DesiredTemperature+1.0 ){
                    pTempControl->TemperatureState = tempMaintainedState;
                } else {
                    // temp going down
                    pTempControl->TemperatureState = tempRampDownState;
                }                            
        }                
    } else {
        // desired temperature is 0, turn off
        pTempControl->TemperatureState = tempOffState;
    }
}

Determining If “Temperatures Gone Wild”

Based on the current Temperature Heating State and the current Temperature, and elapsed Time, determine if the heating has failed:

//-------------------------------------------------------------
// Watch to make sure the temperature is changing the right amounts
//-------------------------------------------------------------
void handleThermalProtection(TEMPERATURE_CONTROL *pTempControl,TaskMonitor_t *TaskMonitorPtr){
    TEMPERATURE_PROTECTION *pTP = &pTempControl->TemperatureProtection;
    uint32_t elapsedTime=0;
    double tempDiff=0;
    
    if (pTempControl->CurrentTemperature >= pTP->FailHighTemperatureCausingShutdown)
        pTempControl->TemperatureState = tempFailState;
    
     
    switch(pTempControl->TemperatureState) {
        
        case tempRampUpState:
            // an open NTC thermistor reads -273 degrees C. on psoc
            if (pTempControl->CurrentTemperature < 0.0) {
                GCodePrintString(HEATING_TEMP_TOO_LOW_STRING CRLF);
                goto lTempFailed;
            }

            // if we do not have a "last" temperature (is 0), then grab the current temperature
            if (pTP->LastCurrentTemperature==0.0){
                pTP->LastCurrentTemperature=pTempControl->CurrentTemperature;
            }

            // if the temperature has not gone up the "increase watch amount", 
            // and time is past watchpoint timeout, then is error.
            tempDiff =  pTempControl->CurrentTemperature - pTP->LastCurrentTemperature;            
            //if (tempDiff < 0.0) tempDiff = -tempDiff; // simple absolute()function
            
            // if temperature has not increased enough
            if (tempDiff < pTP->RampingTemperatureChangeDegreesC ){
                // then if enough time has passed.
                if (elapsedTime > pTP->RampingTemperatureChangeMillisecondsWatchPeriod){
lTempFailed:                    
                    // then there is an error
                    GCodePrintString(HEATING_FAILED_STRING);
                    GCodePrintString(pTempControl->HeaterASCIIidString);
                    GCodePrintString(CRLF PRINTER_HALTED_STRING CRLF);
                    TaskDelay(100,TaskMonitorPtr);// let messages clear out
                    vDoFullStop(pdTRUE,TaskMonitorPtr);// halt printing            
                }                              
            } else {
                // reset up current temperature "monitor from" point
                pTP->LastCurrentTemperature = pTempControl->CurrentTemperature;
                // the temperature has changed enough, reset the monitor time.
                pTP->RampingTemperatureBreachedAtThisMilliseconds = 0;
            }        
        break;
        
        
        case tempReachedState:
        case tempMaintainedState:
            // if we are in the "maintaining" mode, then 
            // check hysteresis to make sure temperature is not continuing
            // to rise, or suddenly turning off
        
            // Note an open NTC thermistor reads -273 degrees C. on psoc
            if (pTempControl->CurrentTemperature < 0.0) {
                GCodePrintString(HEATING_TEMP_TOO_LOW_STRING CRLF);
                goto lTempFailed;
            }
                        
            tempDiff = pTempControl->DesiredTemperature - pTempControl->CurrentTemperature;
            if (tempDiff < 0.0) tempDiff = -tempDiff; // simple absolute()function
            
            // if temperature is out of our range (either high or low)
            if (tempDiff > pTP->TemperatureHysteresisDegreesCWhileMaintaining ){
                if (pTP->MaintainingTemperatureBreachedAtThisMilliseconds==0){
                    pTP->MaintainingTemperatureBreachedAtThisMilliseconds = getSystemTimeInMs();                    
                }
                
                elapsedTime = getElapsedTimeInMilliseconds(pTP->MaintainingTemperatureBreachedAtThisMilliseconds);
                // if it has been out of our range too long, do a kill()
                if (elapsedTime > pTP->MaximumNumberOfMilliSecondsToWaitWhileMaintaining){
                    goto lTempFailed;
                }
            } else {
               // temperature is within range. reset the breach timer.
                pTP->MaintainingTemperatureBreachedAtThisMilliseconds=0; // reset our breach timer.                
            }
        break;
        
        
        case tempOffState:
        case tempRampDownState:
            // if less than 0, probably thermistor disconnected
            if (pTempControl->CurrentTemperature < 0.0) {
                GCodePrintString(HEATING_TEMP_TOO_LOW_STRING CRLF);
                goto lTempFailed;
            }
                
            // if we do not have a "last" temperature (is 0), then grab the current temperature
            if (pTP->LastCurrentTemperature==0.0){
                pTP->LastCurrentTemperature=pTempControl->CurrentTemperature;
            }

            // if the temperature has not gone down any, and time is past watchpoint
            // then is error.
            tempDiff =  pTempControl->CurrentTemperature - pTP->LastCurrentTemperature;            
            
            // if temperature has not decreased enough
            if (tempDiff > - pTP->RampingTemperatureChangeDegreesC){
                // if breach not yet registered, register it
                if (pTP->RampingTemperatureBreachedAtThisMilliseconds==0){
                    pTP->RampingTemperatureBreachedAtThisMilliseconds = getSystemTimeInMs();
                }
                
                elapsedTime = getElapsedTimeInMilliseconds(pTP->RampingTemperatureBreachedAtThisMilliseconds);
                //  if enough time has passed, then see if an error
                if (elapsedTime > pTP->RampingTemperatureChangeMillisecondsWatchPeriod){
                    // if we are close to room temperature (25C), ignore
                    if (pTempControl->CurrentTemperature <= 30.0 ){
                        pTempControl->PIDControls.offset = 0;
                        break;
                    }
                    // else there is an error
                    goto lTempFailed;
                }
            } else {
                // the temperature has changed enough, reset the monitor time.
                pTP->RampingTemperatureBreachedAtThisMilliseconds = 0;
                // regather the last temperature for the new watch-from point
                pTP->LastCurrentTemperature=pTempControl->CurrentTemperature ;
            }
        break;
            
        case tempFailState:
                GCodePrintString(HEATING_TEMP_TOO_HIGH_STRING CRLF);
            goto lTempFailed; 
        break;
    }
        
}

Short Overview (Theory of Operation)

When the temperature is turned on by setting the desire temperature, the if statement in the task for() loop triggers monitoring the temperature and calculating the new PWM value.

The state machine handles what mode the heater is in. Essentially the modes are heating, maintaining, or cooling. The Thermal Protection code determines whether or not to kill the printer due to “over temperature” or “no change in temperature” issues. If the heater overshoots dramatically or fails to heat after a set period of time, something is wrong. The print must be abandoned. This simple action can save hours of cleaning extruders or beds (and possibly prevent small fires).

Next Time

Next Time I hope to show you how to use a “broken off” finger stub in Windows (or Mac or Linux) as a TTL UART (or even back to the CY8CKit-059 it was broken off from).

If you need the complete project (which is still a work in progress), contact me and I will email you a copy of the project. Once completed, the project will exist on GitHub (for now).

Enjoy!

Add a Comment

Your email address will not be published. Required fields are marked *