unlockable/HardwareDesign
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

能工作力！

Browse Source
This commit is contained in:
unlockable
2023-07-06 16:45:00 +08:00
parent 3af54a2a52
commit 3fac49b296
1 changed files with 86 additions and 116 deletions
Download Patch File Download Diff File Expand all files Collapse all files

202
Homework/Homework5/Encoder/Encoder.ino
View File

@@ -10,140 +10,110 @@ const unsigned long RESET_COUNT_PERIOD_IN_US = 50000;
const int EDGES_PER_CYCLE = 13; const int EDGES_PER_CYCLE = 13;
const int REDUCTION_RATIO = 20; const int REDUCTION_RATIO = 20;
int count = 0; int count = 0;
int forward_count = 0; int current_rpm = 0;
int backward_count = 0; double output_level = 0;
unsigned long aFallingTime = 0;
unsigned long bFallingTime = 0; double err = 0, diff_err = 0, accumulate_err = 0;
const double Kp = 0.4, Ki = 0, Kd = 0.5;
const int goal_rpm = 100;
void setup() { void setup() {
pinMode(IN1, OUTPUT); pinMode(IN1, OUTPUT);
pinMode(IN2, OUTPUT); pinMode(IN2, OUTPUT);
pinMode(ENABLE, OUTPUT); pinMode(ENABLE, OUTPUT);
pinMode(A0, INPUT); digitalWrite(IN1, HIGH);
pinMode(A1, INPUT); digitalWrite(IN2, LOW);
digitalWrite(IN1, LOW); pinMode(ENCO_B, INPUT);
digitalWrite(IN2, HIGH); pinMode(ENCO_A, INPUT);
pinMode(ENCO_B, INPUT); Serial.begin(9600);
pinMode(ENCO_A, INPUT); Timer1.initialize();
Serial.begin(115200); Timer1.setPeriod(RESET_COUNT_PERIOD_IN_US);
Timer1.initialize(); Timer1.attachInterrupt(resetCountAndPrint);
Timer1.setPeriod(RESET_COUNT_PERIOD_IN_US); attachInterrupt(digitalPinToInterrupt(ENCO_A), onEncoAChange, CHANGE);
Timer1.attachInterrupt(resetCountAndPrint); attachInterrupt(digitalPinToInterrupt(ENCO_B), onEncoBChange, CHANGE);
// attachInterrupt(digitalPinToInterrupt(ENCO_A), onEncoAChange, CHANGE); analogWrite(ENABLE, output_level);
// attachInterrupt(digitalPinToInterrupt(ENCO_B), onEncoBChange, CHANGE);
attachInterrupt(digitalPinToInterrupt(ENCO_A), onEncoAFalling, FALLING);
// attachInterrupt(digitalPinToInterrupt(ENCO_B), onEncoBFalling, FALLING);
analogWrite(ENABLE, 50);
} }
void loop() { void loop() {
// int statusA = digitalRead(ENCO_A); // if (Serial.available()) {
// int statusB = digitalRead(ENCO_B); // switch (Serial.read()) {
// // Serial.print("A: "); // case 's':
// Serial.print('$'); // analogWrite(ENABLE, 50);
// Serial.print(statusA); // break;
// // Serial.print("B: "); // case 'p':
// Serial.print(' '); // analogWrite(ENABLE, 0);
// Serial.print(statusB); // break;
// Serial.println(";"); // }
// }
// bFallingTime = micros();
// aFallingTime = micros();
// Serial.println(bFallingTime - aFallingTime);
} }
void resetCountAndPrint() { void resetCountAndPrint() {
Serial.print("Rpm: "); current_rpm = (double)count * 60 * 1000 * 1000 / RESET_COUNT_PERIOD_IN_US /
// Serial.println((double)count * 60 * 1000 * 1000 / RESET_COUNT_PERIOD_IN_US / REDUCTION_RATIO / EDGES_PER_CYCLE); REDUCTION_RATIO / EDGES_PER_CYCLE / 4;
// Serial.print(forward_count); count = 0;
// Serial.print(' '); output_level += calculatePID();
// Serial.print(backward_count); if (output_level > 255) {
// Serial.print(' '); output_level = 255;
count = forward_count - backward_count; } else if (output_level < -255) {
Serial.print(forward_count); output_level = -255;
Serial.print(' '); }
Serial.print(backward_count);
Serial.print(' '); if (output_level < 0) {
Serial.println(count); digitalWrite(IN1, LOW);
count = 0; digitalWrite(IN2, HIGH);
forward_count = 0; analogWrite(ENABLE, -output_level);
backward_count = 0; }
else {
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
analogWrite(ENABLE, output_level);
}
Serial.print("goal:");
Serial.print(goal_rpm);
Serial.print(" rpm:");
Serial.println(current_rpm);
} }
void onEncoAFalling() { double calculatePID() {
if (digitalRead(ENCO_B) == LOW) { diff_err = goal_rpm - current_rpm - err;
backward_count++; err = goal_rpm - current_rpm;
} accumulate_err += err;
else { return Kp * err + Ki * accumulate_err + Kd * diff_err;
forward_count++;
}
// aFallingTime = micros();
}
void onEncoBFalling() {
// if (digitalRead(ENCO_A) == LOW) {
// forward_count++;
// }
// else {
// backward_count++;
// }
bFallingTime = micros();
} }
void onEncoAChange() { void onEncoAChange() {
if (LOW == digitalRead(ENCO_A)) { if (LOW == digitalRead(ENCO_A)) {
if (LOW == digitalRead(ENCO_B)) { if (LOW == digitalRead(ENCO_B)) {
// Serial.println("A 1"); count--;
// count--;
backward_count++;
}
if (HIGH == digitalRead(ENCO_B)) {
// Serial.println("A 2");
// count ++;
forward_count++;
}
} }
else { if (HIGH == digitalRead(ENCO_B)) {
if (HIGH == digitalRead(ENCO_B)) { count++;
// Serial.println("A 3");
// count--;
backward_count++;
}
if (LOW == digitalRead(ENCO_B)) {
// Serial.println("A 4");
// count++;
forward_count++;
}
} }
} else {
if (HIGH == digitalRead(ENCO_B)) {
count--;
}
if (LOW == digitalRead(ENCO_B)) {
count++;
}
}
} }
void onEncoBChange() { void onEncoBChange() {
if (LOW == digitalRead(ENCO_B)) { if (LOW == digitalRead(ENCO_B)) {
if (HIGH == digitalRead(ENCO_A)) { if (HIGH == digitalRead(ENCO_A)) {
// Seria/l.println("B 1"); count--;
// count--;
backward_count++;
}
if (LOW == digitalRead(ENCO_A)) {
// Serial.println("B 2");
// count++;
forward_count++;
}
} }
else { if (LOW == digitalRead(ENCO_A)) {
if (LOW == digitalRead(ENCO_A)) { count++;
// Serial.println("B 3");
// count--;
backward_count++;
}
if (HIGH == digitalRead(ENCO_A)) {
// Serial.println("B 4");
// count++;
forward_count++;
}
} }
} else {
if (LOW == digitalRead(ENCO_A)) {
count--;
}
if (HIGH == digitalRead(ENCO_A)) {
count++;
}
}
} }