1
8#define ENCODER_OPTIMIZE_INTERRUPTS
9#include <Encoder.h>
10
11#include "PID.h"
12#include "TimedTask.h"
13
14#define ENC_A 18
15#define ENC_B 19
16
17#define EN1_PIN 7
18#define EN2_PIN 6
19#define PWM_PIN 5
20
21#define PPR (11 * 21.3 * 2)
22
23Encoder encoder(ENC_A, ENC_B);
24
25PID velocity_pid(35.0, -0.5, 35.0, 0.0);
26PID position_pid(10.0, -0.1, 5.0, 0.0);
27float u;
28float w;
29float theta;
30float last_theta = 0;
31float pos_target = .0;
32
33int i_log = 0;
34void log_state() {
35 i_log++;
36 if (i_log > 10) {
37 Serial.print(u / 12);
38 Serial.print("\ ");
39 Serial.print(w);
40 Serial.print("\ ");
41 Serial.print(theta);
42 Serial.print("\ ");
43 Serial.println(pos_target);
44 i_log = 0;
45 }
46}
47
48void run(unsigned long now, unsigned long dt_millis) {
49 float dt = 1e-3 * dt_millis;
50
51 int enc_value = encoder.read();
52 theta = M_PI * enc_value / PPR;
53 w = 1.0 * (theta - last_theta) / dt;
54 last_theta = theta;
55
56 float theta_u = position_pid.getControl(theta, dt);
57 velocity_pid.setTarget(theta_u);
58
59 u = velocity_pid.getControl(w, dt);
60
61 if (u > 0) {
62 digitalWrite(EN1_PIN, LOW);
63 digitalWrite(EN2_PIN, HIGH);
64 } else {
65 digitalWrite(EN2_PIN, LOW);
66 digitalWrite(EN1_PIN, HIGH);
67 }
68 analogWrite(PWM_PIN, (int) min(abs(u), 255));
69 log_state();
70
71 int pos = analogRead(A10);
72 pos_target = M_PI * (1.0 * pos / 512.0 - 1.0);
73 position_pid.setTarget(pos_target);
74}
75
76TimedTask run_task(&run, 5);
77
78void setup() {
79
80 TCCR3B = TCCR3B & 0b11111000 | 0x01;
81
82 pinMode(EN1_PIN, OUTPUT);
83 pinMode(EN2_PIN, OUTPUT);
84 pinMode(PWM_PIN, OUTPUT);
85
86 Serial.begin(115200);
87
88 digitalWrite(EN1_PIN, HIGH);
89 digitalWrite(EN2_PIN, LOW);
90 analogWrite(PWM_PIN, 63);
91}
92
93void loop() {
94 run_task.loop();
95}