Commit @363c1627210c916824b7dbe0cfbddbfe556b8d92

gyeongsangseaman 6 days ago

비상 정지 후 급발진 방지 코드 구현

@363c1627210c916824b7dbe0cfbddbfe556b8d92

6f0fd13

363c162

vdwd2.5.ino (Renamed from vdwd2.0.ino)

--- vdwd2.0.ino

+++ vdwd2.5.ino


     }
 
     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);
-      setRPM2 = (int)(rpm_l);
+      setRPM1 = (int)(rpm_r * pkjR);
+      setRPM2 = (int)(rpm_l * pkjL);
 
       Serial.print("★ V,W 명령 적용 → ");
       Serial.print("V: "); Serial.print(v, 3);

     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);
 

     analogWrite(PWM_2, constrain(abs(calPWM2), 0, 255));
   }
   serialSetTwist();  // 추가된 V, W 명령
-}
+}

Add a comment

Open 0
Closed 0

List

...	...	@@ -117,10 +117,12 @@
117	117	}
118	118
119	119	if (vUpdated \|\| wUpdated) {
	120	+ double pkjR = 1;
	121	+ double pkjL = 1;
120	122	double rpm_r = (v + (WHEEL_DISTANCE / 2.0) * w) / WHEEL_RADIUS * 60.0 / (2 * PI);
121	123	double rpm_l = (v - (WHEEL_DISTANCE / 2.0) * w) / WHEEL_RADIUS * 60.0 / (2 * PI);
122		- setRPM1 = (int)(rpm_r);
123		- setRPM2 = (int)(rpm_l);
	124	+ setRPM1 = (int)(rpm_r * pkjR);
	125	+ setRPM2 = (int)(rpm_l * pkjL);
124	126
125	127	Serial.print("★ V,W 명령 적용 → ");
126	128	Serial.print("V: "); Serial.print(v, 3);
...	...	@@ -161,6 +163,27 @@
161	163	rpm1 = calculateRPM(cnt1);
162	164	rpm2 = calculateRPM(cnt2);
163	165
	166	+ // === 모터 드라이버 전원 상태 감지용 ===
	167	+ static int zeroRPMCount = 0;
	168	+ const int maxZeroRPMCount = 10; // 10*30ms = 300ms 동안 0이면 모터 정지 처리
	169	+
	170	+ if (rpm1 == 0 && rpm2 == 0 && (setRPM1 != 0 \|\| setRPM2 != 0)) {
	171	+ zeroRPMCount++;
	172	+ } else {
	173	+ zeroRPMCount = 0;
	174	+ }
	175	+
	176	+ if (zeroRPMCount > maxZeroRPMCount) {
	177	+ // 모터 드라이버가 꺼진 것으로 간주하여 안전 정지
	178	+ setRPM1 = 0;
	179	+ setRPM2 = 0;
	180	+ integral1 = 0; // 누적된 적분항 초기화
	181	+ integral2 = 0;
	182	+ calPWM1 = 0;
	183	+ calPWM2 = 0;
	184	+ }
	185	+
	186	+
164	187	calPWM1 = computePID(rpm1, error1, prevError1, integral1, derivative1, setRPM1, kp1, ki1, kd1, true);
165	188	calPWM2 = computePID(rpm2, error2, prevError2, integral2, derivative2, setRPM2, kp2, ki2, kd2, false);
166	189
...	...	@@ -170,4 +193,4 @@
170	193	analogWrite(PWM_2, constrain(abs(calPWM2), 0, 255));
171	194	}
172	195	serialSetTwist(); // 추가된 V, W 명령
173		-} (파일 끝에 줄바꿈 문자 없음)
	196	+}

Delete comment