82 lines
1.5 KiB
Plaintext
82 lines
1.5 KiB
Plaintext
#include "pico/stdlib.h"
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
|
|
#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
|
|
|
|
int kp = 10;
|
|
int ki = 100;
|
|
int kd = 1000;
|
|
|
|
int set_temp = 30;
|
|
|
|
int* e_prev;
|
|
|
|
// Perform initialisation
|
|
int* create_array(int size) {
|
|
int* array = (int*)malloc(size * sizeof(int));
|
|
|
|
for (int i = 0; i < size; i++) { //fill with 0s so it doesnt confuse the controller
|
|
array[i] = 0;
|
|
}
|
|
|
|
return 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;
|
|
}
|
|
|
|
int* push_array(int* array, int value) {
|
|
for (int i = 1; i < MAX_STORED_VALUES; i++) {
|
|
array[i] = array[i-1];
|
|
}
|
|
|
|
array[0] = value;
|
|
|
|
return array;
|
|
}
|
|
|
|
void init(void) {
|
|
*e_prev = create_array(MAX_STORED_VALUES);
|
|
gpio_init(PICO_DEFAULT_LED_PIN);
|
|
gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT);
|
|
}
|
|
|
|
void pico_set_led(bool led_on) {
|
|
gpio_put(PICO_DEFAULT_LED_PIN, led_on);
|
|
}
|
|
|
|
int calculate_pid(int* error_array, float x) {
|
|
int e = set_temp-x;
|
|
|
|
push_array(error_array, e);
|
|
|
|
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;
|
|
}
|
|
|
|
int main() {
|
|
//init();
|
|
stdio_init_all();
|
|
while (true)
|
|
{
|
|
/* code */
|
|
printf("sadf");
|
|
sleep_ms(100);
|
|
}
|
|
|
|
|
|
}
|