rdq44 / AMR_develope star
gyeongsangseaman 07-30
아두이노 PID 제어
@d9e74fa274f5e72c74728e5e8bc7a802aa4606b4
 
d9e74fa
2Motor_CTRL.ino (added)
+++ 2Motor_CTRL.ino
@@ -0,0 +1,213 @@
+// July 30th 10:17 Updated
+
+// === 1번 모터 핀 정의 ===
+#define DIR_1 8      // DIR1 (드라이버 갈색 2번, 점퍼선 주황색)
+#define ST_1 9       // START/STOP1 (드라이버 핑크 4번, 점퍼선 초록색)
+#define PWM_1 45     // SPEED_IN1 (드라이버 주황색 6번, 점퍼선 노란색)
+#define ENCA_1 2     // A상 (드라이버 보라색, 점퍼선 파란색)
+#define ENCB_1 3     // B상 (드라이버 회색, 점퍼선 흰색)
+
+// === 2번 모터 핀 정의 ===
+#define DIR_2 22    // DIR2 (드라이버 갈색 16번, 점퍼선 주황색)
+#define ST_2 24     // START/STOP2 (드라이버 핑크 17번, 점퍼선 초록색)
+#define PWM_2 44    // SPEED_IN2 (드라이버 주황색 8번, 점퍼선 노란색)
+#define ENCA_2 18   // A상 (드라이버 보라색, 점퍼선 파란색)
+#define ENCB_2 19   // B상 (드라이버 회색, 점퍼선 흰색)
+
+// === 목표 RPM (부호 포함) ===
+int setRPM1 = 100;
+int setRPM2 = 0;
+
+// === PID 계수 (모터별 개별 설정) ===
+double kp1 = 0.5, ki1 = 3.0, kd1 = 0.085;
+double kp2 = 0.0, ki2 = 0.0, kd2 = 0.0;
+
+// === 모터 상태 변수 ===
+// 모터 1
+volatile long encoderPos1 = 0;
+double rpm1 = 0;
+int calPWM1 = 0;
+double error1 = 0, prevError1 = 0, integral1 = 0, derivative1 = 0;
+
+// 모터 2
+volatile long encoderPos2 = 0;
+double rpm2 = 0;
+int calPWM2 = 0;
+double error2 = 0, prevError2 = 0, integral2 = 0, derivative2 = 0;
+
+// 제어 주기
+const unsigned long interval = 100;    // 단위: ms
+unsigned long prevTime = 0;
+double dt = interval / 1000.0;          // ms → 초(s) 단위로 변경
+
+// === 인터럽트 핸들러 ===
+// 모터 1
+void doEncoderA1() {
+  if (digitalRead(ENCB_1) == LOW) encoderPos1++;
+  else encoderPos1--;
+}
+void doEncoderB1() {
+  if (digitalRead(ENCA_1) == HIGH) encoderPos1++;
+  else encoderPos1--;
+}
+
+// 모터 2
+void doEncoderA2() {
+  if (digitalRead(ENCB_2) == LOW) encoderPos2--;
+  else encoderPos2++;
+}
+void doEncoderB2() {
+  if (digitalRead(ENCA_2) == HIGH) encoderPos2--;
+  else encoderPos2++;
+}
+
+// === RPM 계산 함수 ===
+double calculateRPM(long counts) {
+  return ((double)counts / (4096.0 * 2)) * (60.0 / dt);
+}
+
+// === PID 계산 함수 (모터별 계수 전달) ===
+int computePID(double currentRPM, double &error, double &prevError,
+               double &integral, double &derivative, int targetRPM,
+               double kp, double ki, double kd) {
+  error = targetRPM - currentRPM;
+  integral += (error * dt);
+  derivative = (error - prevError) / dt;
+
+  double output = (kp * error) + (ki * integral) + (kd * derivative);
+  prevError = error;
+  return (int)output;
+}
+
+// === 실시간 입력 (R숫자 L숫자 형식으로 입력, 음수 가능) ===
+void serialSetRPM() {
+  if (Serial.available()) {
+    String input = Serial.readStringUntil('\n');
+    input.trim();
+
+    int rIndex = input.indexOf('R');
+    int lIndex = input.indexOf('L');
+
+    if (rIndex == -1 && lIndex == -1) {
+      Serial.println("입력 오류: 'R' 또는 'L'이 포함되어야 합니다. 예: R100 L-80");
+      return;
+    }
+
+    bool validInput = false;
+
+    // === R 처리 ===
+    if (rIndex != -1) {
+      int end = (lIndex != -1 && lIndex > rIndex) ? lIndex : input.length();
+      String rValStr = input.substring(rIndex + 1, end);
+      rValStr.trim();
+
+      if (rValStr.length() == 0) {
+        Serial.println("오류: R 뒤에 숫자가 없습니다. 예: R100");
+      } else {
+        int rpmVal = rValStr.toInt();
+        if (String(rpmVal) != rValStr) {
+          Serial.print("오류: R에 잘못된 숫자 형식 → "); Serial.println(rValStr);
+        } else if (abs(rpmVal) > 400) {
+          Serial.println("오류: R 값은 -400 ~ 400 사이여야 합니다.");
+        } else {
+          setRPM1 = rpmVal;
+          Serial.print("모터1 (R) 목표 RPM → "); Serial.println(setRPM1);
+          validInput = true;
+        }
+      }
+    }
+
+    // === L 처리 ===
+    if (lIndex != -1) {
+      int end = (rIndex != -1 && rIndex > lIndex) ? rIndex : input.length();
+      String lValStr = input.substring(lIndex + 1, end);
+      lValStr.trim();
+
+      if (lValStr.length() == 0) {
+        Serial.println("오류: L 뒤에 숫자가 없습니다. 예: L-80");
+      } else {
+        int rpmVal = lValStr.toInt();
+        if (String(rpmVal) != lValStr) {
+          Serial.print("오류: L에 잘못된 숫자 형식 → "); Serial.println(lValStr);
+        } else if (abs(rpmVal) > 400) {
+          Serial.println("오류: L 값은 -400 ~ 400 사이여야 합니다.");
+        } else {
+          setRPM2 = rpmVal;
+          Serial.print("모터2 (L) 목표 RPM → "); Serial.println(setRPM2);
+          validInput = true;
+        }
+      }
+    }
+
+    if (!validInput) {
+      Serial.println("R숫자 L숫자 형식으로 입력해주세요. 예: R100 L-80");
+    }
+  }
+}
+
+
+void setup() {
+  Serial.begin(57600);
+
+  // 모터 1 핀 설정
+  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);
+
+  // 모터 2 핀 설정
+  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();
+
+  Serial.println("R±RPM | L±RPM 입력");
+}
+
+void loop() {
+  unsigned long now = millis();
+
+  if (now - prevTime >= interval) {
+    prevTime += interval;
+
+    // 모터 1
+    noInterrupts(); long currentPos1 = encoderPos1; encoderPos1 = 0; interrupts();
+    rpm1 = calculateRPM(currentPos1);
+    calPWM1 = computePID(rpm1, error1, prevError1, integral1, derivative1, abs(setRPM1), kp1, ki1, kd1);
+    digitalWrite(DIR_1, setRPM1 >= 0 ? HIGH : LOW);
+    calPWM1 = constrain(abs(calPWM1), 0, 255);
+    analogWrite(PWM_1, calPWM1);
+
+    // 모터 2
+    noInterrupts(); long currentPos2 = encoderPos2; encoderPos2 = 0; interrupts();
+    rpm2 = calculateRPM(currentPos2);
+    calPWM2 = computePID(rpm2, error2, prevError2, integral2, derivative2, abs(setRPM2), kp2, ki2, kd2);
+    digitalWrite(DIR_2, setRPM2 >= 0 ? HIGH : LOW);
+    calPWM2 = constrain(abs(calPWM2), 0, 255);
+    analogWrite(PWM_2, calPWM2);
+
+    // 시리얼 출력
+    // Serial.print("R:"); Serial.println(setRPM1);
+    // Serial.print("RPM:"); Serial.println(rpm1, 2);
+    // Serial.print("PWM:"); Serial.println(calPWM1);
+    // Serial.println(" ");
+    // Serial.print("L:"); Serial.println(setRPM2);
+    // Serial.print("RPM:"); Serial.println(rpm2, 2);
+    // Serial.print("PWM:"); Serial.println(calPWM2);
+    // Serial.println("--------------------");
+
+    // Serial Plotter용 출력 (탭으로 구분)
+    Serial.print(setRPM1); Serial.print('\t');
+    Serial.println(rpm1, 2);
+    // Serial.print(setRPM2); Serial.print('\t');
+    // Serial.println(rpm2, 2);
+  }
+
+  serialSetRPM();
+}
Add a comment
List