//Vd,Wd 있는 버전 // === 핀 정의 === #define DIR_1 8 #define ST_1 9 #define PWM_1 45 #define ENCA_1 2 #define ENCB_1 3 #define DIR_2 22 #define ST_2 24 #define PWM_2 44 #define ENCA_2 18 #define ENCB_2 19 // === 목표 RPM === int setRPM1 = 0; int setRPM2 = 0; // === PID 계수 및 상태 변수 === 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 prevV = 0, prevW = 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++; } // === 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, 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; // 출력 if (isMotor1) { Serial.print("R : "); Serial.print(targetRPM); Serial.print('\t'); Serial.println(currentRPM, 2); } else { Serial.print("L : "); Serial.print(targetRPM); Serial.print('\t'); Serial.println(currentRPM, 2); Serial.println("------------"); } return (int)(P + I + D); } // === 확장: V0.2 W-0.1 형식의 입력 === 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 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); setRPM2 = (int)(rpm_l); Serial.print("★ V,W 명령 적용 → "); 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("형식 오류: V0.3 또는 W-0.2 형식으로 입력하세요."); } } } // === 초기화 === 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); 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 ? HIGH : LOW); digitalWrite(DIR_2, setRPM2 >= 0 ? HIGH : LOW); analogWrite(PWM_1, constrain(abs(calPWM1), 0, 255)); analogWrite(PWM_2, constrain(abs(calPWM2), 0, 255)); } serialSetTwist(); // 추가된 V, W 명령 }