Temperature PID controller for Arduino

estufa_v1.ino

// Nossas Bibliotecas
#include "Thread.h"
#include "ThreadController.h"
#include "LinkedList.h"
#include "Gaussian.h"
#include "GaussianAverage.h"

// Bibliotecas externas
#include "TimerOne.h"

// Definições Globais
#define runCPU_INTERVAL		1000 // 1ms

// Definições de pinos
#define PIN_STATUS			13
#define PIN_TEMP			A0
#define PIN_PHASE			4
#define PIN_TRIGGER			5
#define PIN_COOLER			11

/*
	GLOBAL variables
*/
float setpoint = 28;
float temperature = 20;

int processEnabled = false;

/*
	Principal Controlador de Threads.

	Callback de Threads começam com `thr`
*/
ThreadController CPU = ThreadController(0);

/*
	Thread responsável por comunicar com a Porta Serial

	Protocol is as follows:
		0bZZXX XXXX
	
	ZZ 		= Command Bits
	XXXXXX 	= Param Bits

	Available Commands:
		00: Ping
			Returns 0xFF; Checks if board is alive

		01: Turn ON/OFF
			Param = 0: OFF
			Param = 1: ON

		10: Set SETPOINT
			Param: 0b10XX XXXX -> 0 - 63
			Scale:  0  = 20Cº
					63 = 36Cº

		// TO DO
		11: Set kP, kI and kD
			Param: 0b1100 0000
			+ 3 bytes of data
			Scale:	0   =  0.0
					255 = 64.0
*/
#define PCComm_INTERVAL	5 // 1ms
Thread PCComm = Thread();

float paramToTemperature(int param){
	return (param/4.0) + 20.0;
}

int temperatureToParam(float param){
	return (param - 20) * 4;
}


int comRaw;
int command;
int param;
void thrPCComm(){

	// // Check for incoming command
	if(!Serial.available()) return;

	comRaw = Serial.read();
	command = comRaw >> 6;
	param = comRaw & 0b00111111;

	if(command == 0x01){
		if(param)
			processEnable();
		else
			processDisable();
		// Serial.println(param ? "ON" : "OFF");
	}else if(command == 0x02){
		// Serial.print("Command: ");
		// Serial.print(command);
		// Serial.print(" Param: ");
		// Serial.print(param);
		// Serial.print(" Parse: ");
		// Serial.print(paramToTemperature(param));
		// Serial.print(" Reparse: ");
		// Serial.println(temperatureToParam(paramToTemperature(param)));
		setpoint = paramToTemperature(param);
	}else if(command == 0x03){
		// TO DO: Set kP, kI and kD
	}
}

/*
	Thread Responsável por controlar disparo do SCR
*/

#define WAITING_RISE 0
#define WAITING_TIME 1
#define WAITING_FALL 2

#define FULL_TIME	1.0/120*1000000

Thread SCR = Thread();
long start = 0;
double triggerTimePercentage = 1.0; // 10ms
int state = WAITING_RISE;

void thrSCR(){
	if(triggerTimePercentage < 0){
		digitalWrite(PIN_TRIGGER, LOW);
	}else if(state == WAITING_TIME){

		// Check if time passed
		long now = micros();

		// if(digitalRead(PIN_PHASE) == LOW){
			// return;
		// }

		if(now - start >= (1.0 - triggerTimePercentage) * FULL_TIME){
			digitalWrite(PIN_TRIGGER, HIGH);
			if((1.0 - triggerTimePercentage) > 0.5)
				digitalWrite(PIN_TRIGGER, LOW);
			state = WAITING_FALL;
		}

	}else if(state == WAITING_RISE){
		// Waits for phase sync
		if(digitalRead(PIN_PHASE)){
			state = WAITING_TIME;
			start = micros();
		}

	}else if(state == WAITING_FALL){
		// Waits for end of phase
		if(!digitalRead(PIN_PHASE)){
			digitalWrite(PIN_TRIGGER, LOW);
			state = WAITING_RISE;
		}		
	}

	// Output to Cooler
	if(triggerTimePercentage >= 0)
		analogWrite(PIN_COOLER, 0);
	else
		analogWrite(PIN_COOLER, triggerTimePercentage * -200.0 + 50);
}

/*
	Thread responsável por controle do PID
*/
#define Process_INTERVAL 100 // 1000ms
Thread Process = Thread();

double kP, kI, kD, kPID, kIMax;

double error;
double lastError;
double P;
double I;
double D;

GaussianAverage averageTemperature(5);
// GaussianAverage averageOut(3);

// Returns current temperature in decimal
const float CELSIUS_BASE = 0.4887585532746823069403714565;
double readTemperature(){
	// temperature += (setpoint - temperature)/10.0;
	return analogRead(PIN_TEMP) * CELSIUS_BASE;
}

void processEnable(){
	processEnabled = true;

	// Clear assist vars
	P = 0;
	I = 0;
	D = 0;
	error = 0;
	lastError = 0;

	// Set default values to constants
	kP   = 30;
	kI   = 0.01;
	kIMax= 50;
	kD   = 0;
	kPID = 0.01;
}

void processDisable(){
	processEnabled = false;
}

void thrProcess(){

	// long start = millis();
	// Add to moving average
	averageTemperature += readTemperature();

	// Compute current average
	double currentTemperature = averageTemperature.process().mean;

	// Set temperature to be output to serial
	temperature = currentTemperature;
	
	// Let's process PID
	error = (setpoint+1) - temperature;
	P  = error * kP;
	I += error * kI;
	D  = (error - lastError) * kD;

	// Filter integral
	if(I >  kIMax) I = kIMax;
	if(I < -kIMax) I = -kIMax;

	// Sum each part to find out the controll output
	double finalProcess = (P + I + D) * kPID;

	// averageOut += finalProcess;

	// if(finalProcess < 0) triggerTimePercentage = 0;
	// else triggerTimePercentage = finalProcess;
	triggerTimePercentage = finalProcess;

	// Serial.println(millis() - start);

	// triggerTimePercentage = finalProcess;

	// Now, let's output it
	// Serial.print("PID: ");
	// Serial.print(finalProcess);
	// Serial.print("\tTEMP: ");
	// Serial.print(currentTemperature);
	// Serial.print("\tOUT: ");
	// Serial.println(triggerTimePercentage);
}

/*
	Sends data to serial about temperature
*/
#define SendData_INTERVAL	83
Thread SendData = Thread();
void thrSendData(){
	// Send data
	// processEnabled
	Serial.write(temperatureToParam(temperature) & 0b00111111);
}


void setup() {
	// Inicia Serial
	Serial.begin(57600);

	// Inicia pinos
	pinMode(PIN_STATUS, OUTPUT);
	pinMode(PIN_TRIGGER, OUTPUT);
	pinMode(PIN_COOLER, OUTPUT);

	// Inicia o Timer para chamar o metodo CPURun
	// Timer1.initialize(runCPU_INTERVAL);

	// Configura e adiciona comunicação com PC
	PCComm.onRun(thrPCComm);
	PCComm.setInterval(PCComm_INTERVAL);
	CPU.add(&PCComm);

	// Configura e adiciona envio de dados pela serial
	SendData.onRun(thrSendData);
	SendData.setInterval(SendData_INTERVAL);
	CPU.add(&SendData);

	// Configura e adiciona função principal de processamento
	Process.onRun(thrProcess);
	Process.setInterval(Process_INTERVAL);
	CPU.add(&Process);

	// Configura Thread que controla o trigger do SCR
	SCR.onRun(thrSCR);
	SCR.setInterval(0);
	CPU.add(&SCR);

	// Inicia threads pelo callback do timer
	// Timer1.attachInterrupt(runCPU);

	// Inicia processo por padrão
	processEnable();
}

/*
	Responsavel por checar e rodar todas as Pseudo-threads
*/
void loop() {
	// Hartbeat. Usado para saber se esta ok (pisca rapido no processo)
	static long lastHartBeat = 0;
	static boolean lastHartBeatState = 0;
	if(millis() - lastHartBeat > 500 + (1000 * !processEnabled)){
		lastHartBeat = millis();
		digitalWrite(PIN_STATUS, lastHartBeatState = !lastHartBeatState);

		// Serial.print("Temp: ");
		// Serial.println(readTemperature());

	}
	// static int dir = 1;
	// triggerTimePercentage += 0.0001*dir;
	// if(triggerTimePercentage > 1.0 || triggerTimePercentage < 0.0){
		// dir *= -1;
	// }

	// Checa e executa threads
	CPU.run();

}