DC motor :: position control

1/**
2 * JGA25-370 DC12V280RPM
3 * Gear ration: 21.3
4 * 
5 * PWM  frequency configuration
6 * - https://playground.arduino.cc/Main/TimerPWMCheatsheet/
7  */
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  // set PWM frequency on pins 2, 3, 5 to 31250 Hz
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}