rdq44 / AMR_develope star
gyeongsangseaman 7 hours ago
비상 정지 후 급발진 방지 코드 개선
@6c3a4b2d294963da99b0cbf467af39d4e4f37d3d
 
6c3a4b2
vdwd3.0.ino (added)
+++ vdwd3.0.ino
@@ -0,0 +1,194 @@
+// Ch1 : Left
+#define DIR_1 8
+#define ST_1 9
+#define PWM_1 45
+#define ENCA_1 2
+#define ENCB_1 3
+
+// Ch2 : Right
+#define DIR_2 22
+#define ST_2 24
+#define PWM_2 44
+#define ENCA_2 18
+#define ENCB_2 19
+
+int setRPM1 = 0;
+int setRPM2 = 0;
+
+double kp1 = 0.5, ki1 = 3.0, kd1 = 0.085;
+double kp2 = 0.5, ki2 = 3.0, kd2 = 0.085;
+double kb1 = 3, kb2 = 3;
+double alpha = 0.3;
+
+volatile long encoderPos1 = 0, encoderPos2 = 0;
+double v = 0, w = 0;
+double rpm1 = 0, rpm2 = 0;
+int calPWM1 = 0, calPWM2 = 0;
+double error1 = 0, prevError1 = 0, integral1 = 0, derivative1 = 0;
+double error2 = 0, prevError2 = 0, integral2 = 0, derivative2 = 0;
+double prevRPM1 = 0, prevRPM2 = 0;
+double backError1 = 0, backError2 = 0;
+
+const unsigned long interval = 30;
+unsigned long prevTime = 0;
+double dt = interval / 1000.0;
+
+const double WHEEL_RADIUS = 0.065; // 65mm
+const double WHEEL_DISTANCE = 0.437; // 437mm
+
+void doEncoderA1() { if (digitalRead(ENCB_1) == LOW) encoderPos1--; else encoderPos1++; }
+void doEncoderB1() { if (digitalRead(ENCA_1) == HIGH) encoderPos1--; else encoderPos1++; }
+void doEncoderA2() { if (digitalRead(ENCB_2) == LOW) encoderPos2++; else encoderPos2--; }
+void doEncoderB2() { if (digitalRead(ENCA_2) == HIGH) encoderPos2++; else encoderPos2--; }
+
+double calculateRPM(long counts) {
+  return ((double)counts / (4096.0 * 2)) * (60.0 / dt);
+}
+
+int computePID(double currentRPM, double &error, double &prevError,
+               double &integral, double &derivative, int targetRPM,
+               double kp, double ki, double kd, bool isMotor1) {
+  double absTarget = abs(targetRPM);
+  double absCurrent = abs(currentRPM);
+  error = absTarget - absCurrent;
+  integral += error * dt;
+
+  double rawDerivative = -(absCurrent - (isMotor1 ? prevRPM1 : prevRPM2)) / dt;
+  double filteredDeriv = alpha * rawDerivative + (1 - alpha) * (isMotor1 ? derivative1 : derivative2);
+  if (isMotor1) prevRPM1 = absCurrent; else prevRPM2 = absCurrent;
+
+  double P = kp * error;
+  double I = ki * integral;
+  double D = kd * filteredDeriv;
+  double output = P + I + D;
+  double outputSat = constrain(output, 0, 255);
+  double backError = outputSat - output;
+  integral += (isMotor1 ? kb1 : kb2) * backError * dt;
+
+  Serial.print(abs(targetRPM)); Serial.print('\t');
+  Serial.print(currentRPM, 2); Serial.print('\t');
+  if (isMotor1) {
+    Serial.print("P1 : "); Serial.print(P, 2); Serial.print('\t');
+    Serial.print("I1 : "); Serial.print(I, 2); Serial.print('\t');
+    Serial.print("D1 : "); Serial.print(D, 2); Serial.println('\t');
+  } else {
+    Serial.print("P2 : "); Serial.print(P, 2); Serial.print('\t');
+    Serial.print("I2 : "); Serial.print(I, 2); Serial.print('\t');
+    Serial.print("D2 : "); Serial.print(D, 2); Serial.println('\t');
+    Serial.println("---------------------------------------------------------");
+  }
+
+  return (int)(P + I + D);
+}
+
+void serialSetTwist() {
+  if (Serial.available()) {
+    String input = Serial.readStringUntil('\n');
+    input.trim();
+
+    int vIndex = input.indexOf('V');
+    int wIndex = input.indexOf('W');
+
+    bool vUpdated = false;
+    bool wUpdated = false;
+
+    if (vIndex != -1) {
+      int end = (wIndex != -1 && wIndex > vIndex) ? wIndex : input.length();
+      String vStr = input.substring(vIndex + 1, end);
+      vStr.trim();
+      double parsedV = vStr.toFloat();
+      if (String(parsedV) == vStr || vStr.length() > 0) {
+        v = parsedV;
+        vUpdated = true;
+      }
+    }
+
+    if (wIndex != -1) {
+      int end = (vIndex != -1 && vIndex > wIndex) ? vIndex : input.length();
+      String wStr = input.substring(wIndex + 1, end);
+      wStr.trim();
+      double parsedW = wStr.toFloat();
+      if (String(parsedW) == wStr || wStr.length() > 0) {
+        w = parsedW;
+        wUpdated = true;
+      }
+    }
+
+    if (vUpdated || wUpdated) {
+      double pkjR = 1;
+      double pkjL = 1;
+      double rpm_r = (v + (WHEEL_DISTANCE / 2.0) * w) / WHEEL_RADIUS * 60.0 / (2 * PI);
+      double rpm_l = (v - (WHEEL_DISTANCE / 2.0) * w) / WHEEL_RADIUS * 60.0 / (2 * PI);
+      setRPM1 = (int)(rpm_r * pkjR);
+      setRPM2 = (int)(rpm_l * pkjL);
+
+//      Serial.print("Target Updated → ");
+//      Serial.print("V: "); Serial.print(v, 3);
+//      Serial.print("  W: "); Serial.print(w, 3);
+//      Serial.print("  R: "); Serial.print(setRPM1);
+//      Serial.print("  L: "); Serial.println(setRPM2);
+//    } else {
+//      Serial.println("Error : Enter in V0.3 or W-0.2 format.");
+    }
+  }
+}
+
+void setup() {
+  Serial.begin(115200);
+  pinMode(DIR_1, OUTPUT); pinMode(PWM_1, OUTPUT); pinMode(ST_1, OUTPUT);
+  pinMode(ENCA_1, INPUT_PULLUP); pinMode(ENCB_1, INPUT_PULLUP);
+  attachInterrupt(digitalPinToInterrupt(ENCA_1), doEncoderA1, RISING);
+  attachInterrupt(digitalPinToInterrupt(ENCB_1), doEncoderB1, RISING);
+
+  pinMode(DIR_2, OUTPUT); pinMode(PWM_2, OUTPUT); pinMode(ST_2, OUTPUT);
+  pinMode(ENCA_2, INPUT_PULLUP); pinMode(ENCB_2, INPUT_PULLUP);
+  attachInterrupt(digitalPinToInterrupt(ENCA_2), doEncoderA2, RISING);
+  attachInterrupt(digitalPinToInterrupt(ENCB_2), doEncoderB2, RISING);
+
+  digitalWrite(ST_1, LOW); digitalWrite(ST_2, LOW);
+  prevTime = millis();
+}
+
+void loop() {
+  unsigned long now = millis();
+  if (now - prevTime >= interval) {
+    prevTime += interval;
+
+    noInterrupts(); long cnt1 = encoderPos1; encoderPos1 = 0;
+                   long cnt2 = encoderPos2; encoderPos2 = 0; interrupts();
+    rpm1 = calculateRPM(cnt1);
+    rpm2 = calculateRPM(cnt2);
+
+  static int zeroRPMCount = 0;
+  const int maxZeroRPMCount = 10;  // 10*30ms = 300ms
+
+  if (rpm1 == 0 && rpm2 == 0 && (setRPM1 != 0 || setRPM2 != 0)) {
+    zeroRPMCount++;
+  } else if (rpm1 == 0 && rpm2 == 0 && setRPM1 == 0 && setRPM2 == 0) {
+    if (integral1 != 0 || integral2 != 0) {
+      integral1 = 0;
+      integral2 = 0;
+    }
+  } else {
+    zeroRPMCount = 0;
+  }
+  
+  if (zeroRPMCount > maxZeroRPMCount) {
+    setRPM1 = 0;
+    setRPM2 = 0;
+    integral1 = 0;
+    integral2 = 0;
+    calPWM1 = 0;
+    calPWM2 = 0;
+  }
+
+    calPWM1 = computePID(rpm1, error1, prevError1, integral1, derivative1, setRPM1, kp1, ki1, kd1, true);
+    calPWM2 = computePID(rpm2, error2, prevError2, integral2, derivative2, setRPM2, kp2, ki2, kd2, false);
+
+    digitalWrite(DIR_1, setRPM1 >= 0 ? LOW : HIGH);
+    digitalWrite(DIR_2, setRPM2 >= 0 ? LOW : HIGH);
+    analogWrite(PWM_1, constrain(abs(calPWM1), 0, 255));
+    analogWrite(PWM_2, constrain(abs(calPWM2), 0, 255));
+  }
+  serialSetTwist();
+}(파일 끝에 줄바꿈 문자 없음)
Add a comment
List