157 lines
3.9 KiB
C++
157 lines
3.9 KiB
C++
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <array>
|
|
#include "pico/stdlib.h"
|
|
#include <math.h>
|
|
#include "hardware/gpio.h"
|
|
#include "hardware/adc.h"
|
|
|
|
using namespace std;
|
|
|
|
#define POLL_PERIOD 50 //time between polls (ms)
|
|
#define MAX_STORED_VALUES 10 //number of stored values to calculate e_dot and e_dot_dot values
|
|
#define REFERENCE_VOLTAGE 3.3 //reference voltage for the adc converter
|
|
|
|
/*
|
|
trying to keep the code clean, so some conventions:
|
|
classes: PascalCase
|
|
function: camelCase
|
|
var names: snake_case
|
|
*/
|
|
|
|
//thermistor equation is R=R_0*e^(1/T-1/T_0)
|
|
|
|
class CustomArray {
|
|
float* items;
|
|
int size;
|
|
|
|
public:
|
|
CustomArray(int s = MAX_STORED_VALUES) {
|
|
size = s;
|
|
items = (float*) malloc(size * sizeof(int));
|
|
for (int i = 0; i < size; i++ ) items[i] = 0;
|
|
}
|
|
|
|
void PushItem(int item) {
|
|
for (int i = 1; i < MAX_STORED_VALUES; i++) {
|
|
items[i] = items[i-1];
|
|
}
|
|
|
|
items[0] = item;
|
|
}
|
|
|
|
float getElement(int index) { return items[index]; }
|
|
float getSum() {
|
|
float sum = 0;
|
|
|
|
for (int i = 0; i < MAX_STORED_VALUES; i++ ) {
|
|
sum += items[i];
|
|
}
|
|
|
|
return sum;
|
|
}
|
|
|
|
~CustomArray() {
|
|
free(items);
|
|
}
|
|
};
|
|
|
|
class PID {
|
|
private:
|
|
int kp, ki, kd, //PID
|
|
pwm_pin, thermistor_pin, //thermistor
|
|
index //misc
|
|
;
|
|
float desired_temp, temp, //temperature
|
|
thermistor_resistance, voltage_devider_resistance, thermistor_beta, thermistor_ref_temp, //thermistor
|
|
adc_conversion_factor
|
|
;
|
|
bool on;
|
|
CustomArray error_array;
|
|
|
|
public:
|
|
PID(
|
|
int index,
|
|
int pwm_pin,
|
|
int thermistor_pin,
|
|
float desired_temp = 0,
|
|
int kp = 1000,
|
|
int ki = 100,
|
|
int kd = 10,
|
|
float thermistor_resistance = 100000,
|
|
float thermistor_beta = 3950,
|
|
float thermistor_ref_temp = 25, //C
|
|
float voltage_devider_resistance = 100000
|
|
) {
|
|
error_array = CustomArray();
|
|
|
|
this->kp = kp;
|
|
this->ki = ki;
|
|
this->kd = kd;
|
|
this->thermistor_pin = thermistor_pin;
|
|
this->thermistor_resistance = thermistor_resistance;
|
|
this->thermistor_beta = thermistor_beta;
|
|
this->thermistor_ref_temp = thermistor_ref_temp + 273;
|
|
this->voltage_devider_resistance = voltage_devider_resistance;
|
|
this->desired_temp = desired_temp;
|
|
this->index = index;
|
|
this->adc_conversion_factor = 3.3f / (1 << 12);
|
|
|
|
measureTemp();
|
|
}
|
|
|
|
float getTemp() {
|
|
return this->temp;
|
|
}
|
|
|
|
float measureTemp() {
|
|
adc_select_input(thermistor_pin);
|
|
uint16_t result = adc_read();
|
|
float v = (float)result * adc_conversion_factor; //calculate ratio of measured voltage and max voltage
|
|
float r = v / (3.3f - v) * voltage_devider_resistance;
|
|
|
|
float T = 1/(1/thermistor_beta * log(r/thermistor_resistance) + 1/thermistor_ref_temp) - 273;
|
|
|
|
this->temp = T;
|
|
return T;
|
|
}
|
|
|
|
int calulatePid(float desired = -100) {
|
|
if (desired != -100) desired_temp = desired;
|
|
float e = desired_temp-temp;
|
|
|
|
float e_dot = (e - error_array.getElement(0)) / POLL_PERIOD;
|
|
float e_int = error_array.getSum() * POLL_PERIOD;
|
|
|
|
return e * kp + e_dot * kd + e_int * ki;
|
|
}
|
|
};
|
|
|
|
void init() {
|
|
stdio_init_all();
|
|
adc_init();
|
|
|
|
gpio_init(25);
|
|
gpio_init(13);
|
|
gpio_set_dir(25, GPIO_OUT);
|
|
gpio_set_dir(13, GPIO_OUT);
|
|
|
|
adc_gpio_init(27);
|
|
}
|
|
|
|
int main()
|
|
{
|
|
init();
|
|
|
|
PID test_pid = PID(0, 6, 1);
|
|
|
|
gpio_put(13, 1);
|
|
|
|
while (true) {
|
|
float val = test_pid.measureTemp();
|
|
float pid = test_pid.calulatePid(80);
|
|
printf("temp: %f\npid: %f", val, pid);
|
|
sleep_ms(500);
|
|
}
|
|
}
|