MotorPIDControl/motor_test.ino at master · sgrsn/MotorPIDControl · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
#include "RotaryEncoder.h"
#include "MotorControl.h"
#include "PID.h"

void tickPath();
RotaryEncoder encoder = RotaryEncoder(2, 3, &tickPath);
MotorControl motor(12, 13, 4, 5, 6);
PID pid;
int32_t Registar[0x80] = {};

void tickPath()
{
  encoder.tick();
}
void setup()
{
  Serial.begin(115200);
  //pid.setParameter(0.00080, 0, 0);
  pid.setParameter(0.00080, 0.123077);
}
float angle = 0;
float prev_angle = 0;
int nowtime = 0;
int prev_time = 0;
float omega = 0;
int controlFlag = 0;

float target_omega = 2500;

void loop()
{
  pidSeq();
}

void pidSeq()
{
  prev_angle = angle;
  angle = encoder.getAngle();
  nowtime = micros();
  omega = (angle - prev_angle) * 1000000 / (nowtime - prev_time);
  prev_time = micros();
  if(omega != 0)
  {
    Serial.print(micros());
    Serial.print(", ");
    Serial.println(omega);
  }
  delay(15);

  if(controlFlag==1)
  {
    float e = pid.control(target_omega, omega);
    motor.control(e);
    Serial.print(e);
    Serial.print(", ");
  }
  else
  {
    motor.control(0);
  }
}

void serialEvent()
{
  if(Serial.available() > 0)
  {
    Serial.read();
    pid.reset(target_omega);
    controlFlag = !(controlFlag);
  }
}

void test()
{
  motor.control(1);
  delay(3000);
  motor.control(0);
  delay(3000);
  motor.control(-1);
  delay(3000);
  motor.control(0);
  delay(3000);
}