BalancingBot/pidControl.ino at master · jam643/BalancingBot · 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
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
#include "PinChangeInterrupt.h"
#include <Wire.h>
#include <math.h>

class PID{
  private:
    double Kp;
    double Ki;
    double input;
    double output;
    double error;
    double error_int = 0;
    unsigned long pid_time = 0;

  public:
    double setpoint;

    PID(double Kp, double Ki, double setpoint) :
    Kp(Kp), Ki(Ki), setpoint(setpoint){}

    double update(double input){
      error = input - setpoint;
      if (input !=0 ){
        error_int += error * (micros() - pid_time) / 1e6;
      }
      pid_time = micros();
      output = Kp * error + Ki * error_int;
      return output;
    }
};

class Motor{
  private:
    const double rpp = 1/1320.;
    volatile byte C1_state = LOW;
    volatile byte C2_state = LOW;
    unsigned long time_motor = 0;
    const double Vdead = 1.4;
    const double Vmax = 12;
    const double v2byte = 255 / (Vmax - Vdead);
    PID pid;
    double V_out;

  public:
    const byte pinC1;
    const byte pinC2;
    const byte pinM1;
    const byte pinM2;
    const byte pinPWM;
    volatile int dist = 0;
    double prev_dist = 0.;
    double speed;

    Motor (const byte pinC1, const byte pinC2, const byte pinM1, const byte pinM2, const byte pinPWM) :
    pinC1(pinC1), pinC2(pinC2), pinM1(pinM1), pinM2(pinM2), pinPWM(pinPWM), pid(15,10,1) {
      pinMode(pinC1, INPUT_PULLUP);
      pinMode(pinC2, INPUT_PULLUP);
      pinMode(pinM1, OUTPUT);
      pinMode(pinM2, OUTPUT);
      pinMode(pinPWM, OUTPUT);
    }

    void update() {
      speed = ((dist - prev_dist)*1e6)/(micros() - time_motor);
      prev_dist = dist;
      time_motor = micros();
    }

    void setSetPoint(double setpoint){
      pid.setpoint = setpoint;
    }

    void setVoltage(double Vin){
      if (Vin < 0){
        digitalWrite(pinM2,LOW);
        digitalWrite(pinM1,HIGH);
      }
      else{
        digitalWrite(pinM1,LOW);
        digitalWrite(pinM2,HIGH);
      }
      if (Vin == 0){
        analogWrite(pinPWM,0);
      }
      else{
        analogWrite(pinPWM,min(int((abs(Vin)+Vdead)*v2byte),255));
      }
    }

    void speedPID(){
      V_out = pid.update(speed);
      Serial.println(V_out);
      setVoltage(V_out);
    }

    void C1change() {
      C1_state = digitalRead(pinC1);
      if (C1_state == C2_state){
        dist -= 1;
      }
      else{
        dist += 1;
      }
    }

    void C2change() {
      C2_state = digitalRead(pinC2);
      if (C1_state == C2_state){
        dist += 1;
      }
      else{
        dist -= 1;
      }
    }

};

Motor motor1(8,9,11,12,10);
unsigned long time_init = 0;
unsigned int count = 0;
bool end = false;

void setup() {
  Serial.begin(115200);

  attachPCINT(digitalPinToPCINT(motor1.pinC1), M1C1ISR, CHANGE);
  attachPCINT(digitalPinToPCINT(motor1.pinC2), M1C2ISR, CHANGE);
}

void loop() {
  count += 1;
  motor1.update();
  Serial.print(motor1.dist);
  Serial.print(" ");
  Serial.println(micros()/100);
  if((millis() - time_init) > 200 & !end){
    motor1.setVoltage(-1);
  }
  delay(7);

  if((millis() - time_init) > 5000 & !end){
    motor1.setVoltage(0);
    Serial.println(count);
    while (true){

    }
  }
}

void M1C1ISR() {
  motor1.C1change();
}

void M1C2ISR() {
  motor1.C2change();
}