147 lines
3.3 KiB
C++
147 lines
3.3 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 {
|
|
int* items;
|
|
int size;
|
|
|
|
public:
|
|
CustomArray(int s = MAX_STORED_VALUES) {
|
|
size = s;
|
|
items = (int*) 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;
|
|
}
|
|
|
|
int getElement(int index) { return items[index]; }
|
|
int getSum() {
|
|
int 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
|
|
thermistor_resistance, voltage_devider_resistance, thermistor_beta, thermistor_ref_temp, //thermistor
|
|
desired_temp, temp, //temperature
|
|
index; //misc
|
|
bool on;
|
|
CustomArray error_array;
|
|
|
|
public:
|
|
PID(
|
|
int index,
|
|
int pwm_pin,
|
|
int thermistor_pin,
|
|
int desired_temp = 0,
|
|
int kp = 1000,
|
|
int ki = 100,
|
|
int kd = 10,
|
|
int thermistor_resistance = 100000,
|
|
int thermistor_beta = 3950,
|
|
int thermistor_ref_temp = 25, //C
|
|
int 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->voltage_devider_resistance = voltage_devider_resistance;
|
|
this->desired_temp = desired_temp;
|
|
this->index = index;
|
|
|
|
measureTemp();
|
|
}
|
|
|
|
int getTemp() {
|
|
return this->temp;
|
|
}
|
|
|
|
int measureTemp() {
|
|
//adc_select_input(thermistor_pin);
|
|
//uint16_t result = adc_read();
|
|
|
|
this->temp = 10;
|
|
|
|
return this->temp;
|
|
}
|
|
|
|
int calulatePid( int x ) {
|
|
int e = desired_temp-x;
|
|
|
|
int e_dot = (e - error_array.getElement(0)) / POLL_PERIOD;
|
|
int 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) {
|
|
printf("%f\n", test_pid.getTemp());
|
|
//printf("asdf");
|
|
sleep_ms(500);
|
|
}
|
|
}
|