85 lines
1.6 KiB
C++
85 lines
1.6 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(int s) {
|
|
size = s;
|
|
items = (int*) malloc(s * sizeof(int));
|
|
for (int i = 0; i < s; i++ )
|
|
{
|
|
items[i] = 0;
|
|
}
|
|
}
|
|
};
|
|
|
|
class PID {
|
|
private:
|
|
int kp, ki, kd, pwm_pin, thermistor_pin, index, desired_temp;
|
|
bool on;
|
|
CustomArray error_array;
|
|
|
|
int calculate_integral(int* array) {
|
|
int sum = 0;
|
|
|
|
for (int i = 0; i < MAX_STORED_VALUES; i++ ) {
|
|
sum += array[i];
|
|
}
|
|
|
|
sum *= POLL_PERIOD;
|
|
return sum;
|
|
}
|
|
|
|
public:
|
|
PID(int kp) {
|
|
error_array = new CustomArray(MAX_STORED_VALUES);
|
|
this.kp = kp;
|
|
}
|
|
|
|
int calulatePid( int x ) {
|
|
int e = desired_temp-x;
|
|
|
|
int e_dot = (e - error_array[0]) / POLL_PERIOD;
|
|
int e_int = calculate_integral(error_array);
|
|
|
|
|
|
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);
|
|
}
|
|
}
|