PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   
3. #include <Servo.h>
   
4. 
   . R0 ]0 n# v% g& G; `
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   $ u% J7 }2 }- R" l
7. #include "MeDCMotor.h"
   
8. 
   8 L9 m) A4 \+ ^1 e4 f1 T0 s# }: D3 B
9. //double Input, Output ;
   3 ?# F- u& ~) }5 }0 n! A1 O
10. float MaxSpeed = 255;
    6 E' j( k7 `& G% V
11. float MaxPower = 180;
    
12. float MinPower = 120;
    6 C. l1 K4 }* q* _/ O5 v3 \" t
13. float Error,ErrorAcc,ErrorDec;
    
14. 
    
15. float Kp=0.14;
    ' l: u6 z+ n) K- |* b
16. float Kd=0.00020;//23;
    1 y/ d4 }* g$ R: S
17. float Ki=0.000201;
    " V( `" Q- `, Y; q! c( Y
18. 
    & @% u) S& \4 P
19. float nPower;
    - r+ D" b. H/ y& |
20. MePort lightsensor_6(6);
    / ~/ s% B1 M9 [4 O# b0 x
21. MePort lightsensor_8(8);
    7 `( ^* U. ?: z& B
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    ; z, Y- N* l8 }$ k0 {
24. unsigned long previousMillis = 0;
    " S% d% w' y7 a+ R! o9 I5 I( S
25. const long interval = 1;
    
26. 
    
27. void setup(){
    " [/ d% q5 c+ {
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    1 Z  b9 q3 k( E0 k9 C( H
30.     Error=0;
    ' Q9 k  K5 `5 A. U# z
31.     ErrorAcc=0;
    
32. }
    5 J9 j2 w! q$ E" s# H9 I
33. 
    
34. void loop(){
    0 V8 j' D/ s6 W
35.   unsigned long StartTime = millis();
    
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    . K; u0 @' \3 ]% X* v- M, w
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    " c+ c4 a  ?& i7 {
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    7 @2 _. ^% j$ p  Y
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    
43.         nPower += 3;
    
44.       }
    
45.     } else{
    
46.       if(nPower > MinPower){
    9 z2 |, ^0 C& F7 R& [
47.         nPower -= 2;
    5 B3 u# b8 k' p, P7 j% l3 F! y+ h
48.       }
    - C) r: p) I0 Z  i" M
49.     }
    
50.     MotoL(nPower-Error);
    % r0 q( j. m* q- E! T$ [
51.     MotoR(nPower+Error);   
    1 o1 _9 G% K4 T
52.   }else{
    ) w" b' ^3 z$ W" G) o$ K3 S, A
53.     motor_9.run(0);
    0 p! R! d1 {  {0 {3 N' V
54.     motor_10.run(0);
    
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    ( g) _. h0 X7 q1 s% w& I
57. }
    
58. 
    . V3 Q/ ^: D) H0 L1 T+ m& o
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    5 l0 M  M! _; t. V4 a4 i
61.      Power = MaxSpeed;
    
62.   }
    
63.   if (Power < -MaxSpeed){
    9 C6 {0 d( g! L+ ~2 p
64.      Power = -MaxSpeed;
    
65.   }
    : h- \" J1 a7 S: b7 M$ H
66.   motor_9.run(Power);
    
67. }  
    
68. 
    7 `+ Y, ?. z* T" d
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    
72.   }
    
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    
75.   }
    
76.   motor_10.run(Power);
    2 S( q% H% W! ], U9 r; y2 ]
77. }  

複製代碼

0 P) ?7 \5 [' g& R
2 g8 x6 y; r/ O6 y3 r

! ?" h' w* Q* A! `" _% N
' r) ?- ^" b; y您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況
5 E6 A) y/ }) f: i- |, |  A1 Q1 s% ^
但基於想追求更精簡的程式所以還是想請問一下
: Y+ c$ M$ [; F% Q5 t5 X! `/ u# G就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
7 z7 L, e- D! L) w不知道您是否願意教我可以如何處理
  M" w, Z! y) G# r1 A2 N+ C

以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   
2. double CNY70Val = 1000;
   ' J3 E$ ~; s! X: b# s  u# z& k% {  o
3. int interror = 0;
   - a6 x0 n5 b9 `( g
4. int olderror = 0;
   * A, d8 }+ M" l- L" U
5. double values;
   
6. 
   ; ^/ z) i" S% Y! h1 Y4 V# U
7. 
   % ]# ^% F$ O( o3 N: P
8. void CNY70()
   
9. {
   + t- Z: w& W& f
10.   valuesRR = analogRead(RR)
    + V" U; m3 e7 ^& V" u, W
11.   valuesMR = analogRead(MR);
    1 g8 E" {1 U4 p) T2 }0 t9 o
12.   valuesMM = analogRead(MM);
    ) M# u% {0 h3 F0 P( Q1 J/ F
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    0 p5 i% X7 j3 E7 ]" d/ m
15. 
    
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    / m* B7 {3 p4 D: D
21.     valuesMM = CNY70Val;
    6 V: [# Y0 l8 U
22.   if (valuesML > CNY70Val)
    # V- Y  d1 G8 Z$ X/ ^. E( M4 n
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    9 }, U2 @+ z; |$ l& E# [
28. }
    
29. 
    + j# z1 s; E: I1 H
30. void Car()
    0 x+ b' F- i: _2 Z. {5 g3 ?" X9 z2 |
31. {
    
32.   while (1) {
    
33.     CNY70();
    6 F' J* k6 m0 Y* m
34. 
    & C7 ~3 g# Q' t4 i+ T+ w5 f
35.     int error = ((int)values);
    " T) F* l( H" i3 w& j- N) C6 y. N
36.     interror += error;
    
37.     int lasterror = error - olderror;
    ' J5 {5 B6 h7 f9 P$ b# [5 |0 }% C
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    ! d! g' o$ ~. R# ~
41.     if (power > MotoSpeed)
    ' J: \- r3 I8 G! _+ D0 C$ l8 R
42.       power = MotoSpeed;
    & G8 M/ k0 @, U( x- z  _
43.     if (power < -MotoSpeed)
    ' u: P/ a" v/ i( n- e4 \
44.       power = -MotoSpeed;
    
45. 
    
46.     if (power > 0)
    * o% n. p4 m% M& |
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    
51. }

複製代碼

$ R  f& j$ y2 p' P

感謝分享

感謝分享