129 lines
2.8 KiB
C++
129 lines
2.8 KiB
C++
#include <TimerOne.h>
|
|
|
|
const byte IN1 = 10;
|
|
const byte IN2 = 11;
|
|
const byte ENABLE = 6;
|
|
const byte ENCO_B = 3;
|
|
const byte ENCO_A = 2;
|
|
|
|
const unsigned long RESET_COUNT_PERIOD_IN_US = 50000;
|
|
const int EDGES_PER_CYCLE = 13;
|
|
const int REDUCTION_RATIO = 20;
|
|
int count = 0;
|
|
int current_rpm = 0;
|
|
double output_level = 0;
|
|
|
|
double err = 0, diff_err = 0, accumulate_err = 0;
|
|
const double Kp = 0.6, Ki = 0.15, Kd = 0.1;
|
|
int goal_rpm = 100;
|
|
|
|
void setup() {
|
|
count = 0;
|
|
current_rpm = 0;
|
|
output_level = 0;
|
|
pinMode(IN1, OUTPUT);
|
|
pinMode(IN2, OUTPUT);
|
|
pinMode(ENABLE, OUTPUT);
|
|
digitalWrite(IN1, HIGH);
|
|
digitalWrite(IN2, LOW);
|
|
pinMode(ENCO_B, INPUT);
|
|
pinMode(ENCO_A, INPUT);
|
|
Serial.begin(9600);
|
|
Timer1.initialize();
|
|
Timer1.setPeriod(RESET_COUNT_PERIOD_IN_US);
|
|
Timer1.attachInterrupt(resetCountAndPrint);
|
|
attachInterrupt(digitalPinToInterrupt(ENCO_A), onEncoAChange, CHANGE);
|
|
attachInterrupt(digitalPinToInterrupt(ENCO_B), onEncoBChange, CHANGE);
|
|
analogWrite(ENABLE, output_level);
|
|
}
|
|
|
|
void loop() {
|
|
// if (Serial.available()) {
|
|
// switch (Serial.read()) {
|
|
// case 's':
|
|
// analogWrite(ENABLE, 50);
|
|
// break;
|
|
// case 'p':
|
|
// analogWrite(ENABLE, 0);
|
|
// break;
|
|
// }
|
|
// }
|
|
// Serial.print(goal_rpm);
|
|
goal_rpm = 100;
|
|
// delay(3000);
|
|
// Serial.print(goal_rpm);
|
|
// goal_rpm = 150;
|
|
// delay(3000);
|
|
}
|
|
|
|
void resetCountAndPrint() {
|
|
current_rpm = (double)count * 60 * 1000 * 1000 / RESET_COUNT_PERIOD_IN_US / REDUCTION_RATIO / EDGES_PER_CYCLE / 4;
|
|
count = 0;
|
|
output_level = calculatePID();
|
|
if (output_level > 255) {
|
|
output_level = 255;
|
|
} else if (output_level < -255) {
|
|
output_level = -255;
|
|
}
|
|
|
|
if (output_level < 0) {
|
|
digitalWrite(IN1, LOW);
|
|
digitalWrite(IN2, HIGH);
|
|
analogWrite(ENABLE, -output_level);
|
|
} else {
|
|
digitalWrite(IN1, HIGH);
|
|
digitalWrite(IN2, LOW);
|
|
analogWrite(ENABLE, output_level);
|
|
}
|
|
|
|
// Serial.print("goal:");
|
|
Serial.print('$');
|
|
Serial.print(goal_rpm);
|
|
// Serial.print(" rpm:");
|
|
Serial.print(", ");
|
|
Serial.print(current_rpm);
|
|
Serial.println(";");
|
|
}
|
|
|
|
double calculatePID() {
|
|
diff_err = goal_rpm - current_rpm - err;
|
|
err = goal_rpm - current_rpm;
|
|
accumulate_err += err;
|
|
return Kp * err + Ki * accumulate_err + Kd * diff_err;
|
|
}
|
|
|
|
void onEncoAChange() {
|
|
if (LOW == digitalRead(ENCO_A)) {
|
|
if (LOW == digitalRead(ENCO_B)) {
|
|
count--;
|
|
}
|
|
if (HIGH == digitalRead(ENCO_B)) {
|
|
count++;
|
|
}
|
|
} else {
|
|
if (HIGH == digitalRead(ENCO_B)) {
|
|
count--;
|
|
}
|
|
if (LOW == digitalRead(ENCO_B)) {
|
|
count++;
|
|
}
|
|
}
|
|
}
|
|
|
|
void onEncoBChange() {
|
|
if (LOW == digitalRead(ENCO_B)) {
|
|
if (HIGH == digitalRead(ENCO_A)) {
|
|
count--;
|
|
}
|
|
if (LOW == digitalRead(ENCO_A)) {
|
|
count++;
|
|
}
|
|
} else {
|
|
if (LOW == digitalRead(ENCO_A)) {
|
|
count--;
|
|
}
|
|
if (HIGH == digitalRead(ENCO_A)) {
|
|
count++;
|
|
}
|
|
}
|
|
} |