#Base

struct Pid pid_new(float kp, float ki, float kd);

#include "pid/pid.h"
struct Pid pid = pid_new(1, 0, 0);
pid_update(&pid, 0, 0, 1);

float pid_update(struct Pid *pid, float target, float actual, float dt);

#include "pid/simulate/push-box.h"
#include "pid/pid.h"
#include <stdio.h>

static struct Pid pid;
static const float target = 100.0;
static const float dt = 1;
static float speed = 0.0;
static float force = 0.0;
static float time = 0.0;

void interrupt_callback() {
  force = pid_update(&pid, target, speed, dt);

  push_box(&speed, force, dt);
  printf("time: %.0fs, \t speed: %.1f, \t force: %.2f\n", time, speed, force);

  time += dt;
}

int main() {
  pid = pid_new_with_integral_clamp(1, 2, 0.1, -300, 300);

  for (unsigned i = 0; i < 10; i++) {
    // need #include <unistd.h>
    // sleep(1);
    interrupt_callback();
  }
}

float warpping(float value, float min, float max) {
  float range = max - min;
  while(value < min) value += range;
  while(value > max) value -= range;
  return value;
}

1#include "pid/pid.h"
2#include "pid/utils/num-limit-macro.h"
3#include "pid/utils/num-warpping.h"
4
5///       ^ 0
6/// -90 <   > 90
7///       v 180
8/// return [-180, 180) left - right +
9/// read MPU6050 and return current direction.
10float get_direction();
11
12/// set motor thrust
13void motor_thrust(float left, float right);
14
15/// turn angle with PID
16///
17/// turn: left - right +
18///
19/// # Example
20/// ```c
21/// turn_angle(90.0); // Turn right 90°
22/// turn_angle(-45.0); // Turn left 45°
23/// ```
24void turn_angle(float turn) {
25  motor_thrust(0, 0);
26  struct Pid pid = pid_new(1, 0, 0);
27  const float origin = get_direction();
28  // if origin < -90 turn 90 then target = 0 ^
29  const float target = WARPPING(origin + turn, -180, 180);
30
31  float current = get_direction();
32  // if current <- -90 target ^ 0 then diff = 90
33  // For the truth, the target is on his right. diff = 90.
34  // so he need to turn right.
35  float diff = WARPPING(target - current, -180, 180);
36
37  // |diff| < 1.0 is turning angle to target value.
38  while (ABS(diff) > 1.0) {
39    // error = target - current = 0 - 90 = -90
40    // The angle between target truth and target angle is zero.
41    // but actual angle diff is 90.
42    // return diff_thrust is negative because error and kp is negative.
43    // and need turn right. left speed + and right speed -
44    float diff_thrust = pid_update(&pid, 0, diff, 1.0);
45    motor_thrust(-diff_thrust, diff_thrust);
46
47    current = get_direction();
48    diff = WARPPING(target - current, -180, 180);
49  }
50}
51
52int main() { turn_angle(90); }

float pid_weighted_sum(struct Pid *pid, float proportional, float integral, float derivative);

pid->kp * proportional + pid->ki * integral + pid->kd * derivative

1float pid_update_with_default(struct Pid *pid, float error, float dt) {
2  float differential = (error - pid->previous) / dt;
3
4  float average = (error + pid->previous) / 2.0;
5  pid->integral += average * dt;
6
7  pid->previous = error;
8
9  return pid_weighted_sum(pid, error, pid->integral, differential);
10}