94 lines
1.8 KiB
C++
94 lines
1.8 KiB
C++
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <array>
|
|
#include "pico/stdlib.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
|
|
|
|
/*
|
|
trying to keep the code clean, so some conventions:
|
|
classes: PascalCase
|
|
function: camelCase
|
|
var names: snake_case
|
|
*/
|
|
|
|
class CustomArray {
|
|
int* items;
|
|
int size;
|
|
|
|
public:
|
|
CustomArray() {
|
|
size = MAX_STORED_VALUES;
|
|
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, pwm_pin, thermistor_pin, index, desired_temp;
|
|
bool on;
|
|
CustomArray error_array;
|
|
|
|
public:
|
|
PID(int kp) {
|
|
error_array = CustomArray();
|
|
this->kp = kp;
|
|
}
|
|
|
|
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() {
|
|
gpio_init(25);
|
|
gpio_set_dir(25, GPIO_OUT);
|
|
}
|
|
|
|
int main()
|
|
{
|
|
stdio_init_all();
|
|
|
|
|
|
while (true) {
|
|
//printf("Hello, world!\n");
|
|
gpio_put(25, 0);
|
|
sleep_ms(250);
|
|
gpio_put(25, 1);
|
|
sleep_ms(250);
|
|
}
|
|
}
|