对喽！

Homework/Homework5/Encoder/Encoder.ino

@@ -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 double Kp = 0.6, Ki = 0.15, Kd = 0.1;
-const int goal_rpm = 100;
+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 /
+  current_rpm = (double)count * 60 * 1000 * 1000 / RESET_COUNT_PERIOD_IN_US / REDUCTION_RATIO / EDGES_PER_CYCLE / 4;
-                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.print(" rpm:");
-  Serial.println(current_rpm);
+  Serial.print(", ");
+  Serial.print(current_rpm);
+  Serial.println(";");
 }
 
 double calculatePID() {

Homework/Homework5/Homework5/Homework5.ino

@@ -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++;
+    }
+  }
+}