80 lines
2.1 KiB
C
80 lines
2.1 KiB
C
#include <stdio.h>
|
|
#include "pico/stdlib.h"
|
|
//#include <stdlib.h>
|
|
//#include <math.h>
|
|
|
|
const float Kp = 1.0f; // SE
|
|
const float Ki = 0.1f; // SE
|
|
const float Kd = 0.01f; // SE
|
|
|
|
// Function to compute the control signal
|
|
float
|
|
compute_pid(float setpoint,
|
|
float current_value,
|
|
float *integral,
|
|
float *prev_error
|
|
) {
|
|
|
|
// float error = *current_value - setpoint; // LE
|
|
float error = setpoint - current_value;
|
|
|
|
*integral += error;
|
|
|
|
float derivative = error - *prev_error;
|
|
|
|
// Extra multiplication by 0.1 (LE)
|
|
// float control_signal = Kp * error + Ki * (*integral) + Kd * derivative * 0.1;
|
|
float control_signal = Kp * error + Ki * (*integral) + Kd * derivative;
|
|
|
|
*prev_error = error; // LE
|
|
|
|
return control_signal;
|
|
}
|
|
|
|
int
|
|
main() {
|
|
float setpoint = 100.0f; // Desired position
|
|
float current_value = 0.0f; // Current position
|
|
float integral = 0.0f; // Integral term
|
|
float prev_error = 0.0f; // Previous error term
|
|
|
|
float time_step = 0.1f;
|
|
int num_iterations = 100;
|
|
|
|
// Missing init (SE)
|
|
stdio_init_all();
|
|
|
|
// Simulate the control loop
|
|
for (int i = 0; i < num_iterations; i++) {
|
|
// Syntax error, prev_error pass by reference missing & (SE)
|
|
// float control_signal = compute_pid(setpoint, current_value, &integral, prev_error);
|
|
float control_signal = compute_pid(
|
|
setpoint,
|
|
current_value,
|
|
&integral,
|
|
&prev_error
|
|
);
|
|
|
|
// Pseudocode shows * 0.1 (LE)
|
|
// float motor_response = control_signal * 0.05; // Motor response model
|
|
float motor_response = control_signal * 0.1;
|
|
|
|
current_value += motor_response; // LE
|
|
|
|
// SE: Control Signal = %f, not %d
|
|
printf("Iteration %d: Control Signal = %f, Current Position = %f\n",
|
|
i,
|
|
control_signal,
|
|
current_value
|
|
);
|
|
|
|
|
|
// Sleep for time_step seconds missing (LE)?
|
|
sleep_ms((int)(time_step * 1000));
|
|
// usleep((useconds_t)(time_step));
|
|
|
|
}
|
|
|
|
return 0;
|
|
}
|