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 23:40:30 +08:00
parent 3fac49b296
commit 5ce9b87315
2 changed files with 149 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -14,10 +14,13 @@ int current_rpm = 0;
double output_level = 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;
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);
@@ -45,13 +48,18 @@ void loop() {
// 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;
current_rpm = (double)count * 60 * 1000 * 1000 / RESET_COUNT_PERIOD_IN_US / REDUCTION_RATIO / EDGES_PER_CYCLE / 4;
count = 0;
output_level += calculatePID();
output_level = calculatePID();
if (output_level > 255) {
output_level = 255;
} else if (output_level < -255) {
@@ -62,17 +70,19 @@ void resetCountAndPrint() {
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
analogWrite(ENABLE, -output_level);
}
else {
} else {
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
analogWrite(ENABLE, output_level);
}
Serial.print("goal:");
// Serial.print("goal:");
Serial.print('$');
Serial.print(goal_rpm);
Serial.print(" rpm:");
Serial.println(current_rpm);
// Serial.print(" rpm:");
Serial.print(", ");
Serial.print(current_rpm);
Serial.println(";");
}
double calculatePID() {

129
Homework/Homework5/Homework5/Homework5.ino Normal file
View File

@@ -0,0 +1,129 @@
#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++;
}
}
}