File name
Commit message
Commit date
//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 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 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("★ 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);
// === 모터 드라이버 전원 상태 감지용 ===
static int zeroRPMCount = 0;
const int maxZeroRPMCount = 10; // 10*30ms = 300ms 동안 0이면 모터 정지 처리
if (rpm1 == 0 && rpm2 == 0 && (setRPM1 != 0 || setRPM2 != 0)) {
zeroRPMCount++;
} 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 ? 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 명령
}