PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   / g% K  g! r/ ]& f' y
3. #include <Servo.h>
   / X! v  I% ]4 }8 a8 m
4. 
   
5. #include "MePort.h"
   ) h$ \3 e- |2 l+ h1 G5 F
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   3 ~0 H/ k3 Y1 y2 t
8. 
   
9. //double Input, Output ;
   " S6 B1 F7 y+ Z+ M
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    7 V3 W. r/ C+ g% n- q
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    ; O9 [- a# _- w; |- P8 K' B
14. 
    $ a" L: k2 W* o# k7 l# O8 z; I) `# E
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    " j- b1 g. ?3 N3 c
18. 
    
19. float nPower;
    
20. MePort lightsensor_6(6);
    5 e# m; r5 D7 ?1 F) Q, z  T; R5 y
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    
26. 
    
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    3 O) |7 [) Y; y8 f- |
29.     nPower = 160;
    
30.     Error=0;
    
31.     ErrorAcc=0;
    
32. }
    
33. 
    
34. void loop(){
    + _; m1 \: ^- g8 k
35.   unsigned long StartTime = millis();
    
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    4 K8 [! K( ]7 h- l/ O5 u0 J6 d- H+ j
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
    - I+ S5 E$ P6 h( K
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    6 U/ R( l& j9 C, k8 }
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    
43.         nPower += 3;
    
44.       }
    
45.     } else{
    + {% E' a" c3 a
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    
48.       }
    
49.     }
    
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    
52.   }else{
    . h5 A" |& b" E, [* Z
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    4 k* B, n! [1 e) s
55.   }
    # C; K- s8 _4 R4 k
56.   do{}while(millis() - StartTime < interval);
    
57. }
    
58. 
    
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    
62.   }
    
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    
65.   }
    $ N" ~% k% e+ r, m9 A, p
66.   motor_9.run(Power);
    
67. }  
    1 o4 j; n& `8 D+ D/ M4 {
68. 
    
69. void MotoR(int Power){
      I. Y& u3 o9 c  r
70.   if (Power > MaxSpeed){
    6 Q2 G6 k* c* d8 T7 u/ b
71.      Power = MaxSpeed;
    
72.   }
    
73.   if (Power < -MaxSpeed){
    # w8 w$ o$ V4 c
74.      Power = -MaxSpeed;
    & s4 z% `+ |' L" A$ i/ V* E- }
75.   }
    + t  _- Q% q/ u8 m. K* M% w
76.   motor_10.run(Power);
    " {. p8 R  e0 u, M9 ^' c" L7 }
77. }  

複製代碼

您好，不知是否能向您請教。
' J1 P/ `, Q! v: l( X1 T- @" C目前和宋修賢老師在處理Ardui Car
) W& p: }" X/ F/ ~雖然已使用較繁雜的方式處理了跑出黑線外的狀況
7 j1 i8 r% \' `1 `' k5 ]
3 p" U) ~8 t! [0 y4 r$ t6 C但基於想追求更精簡的程式所以還是想請問一下
就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
* o7 }1 W5 d" ?4 f# j1 b# e" x# ^( d不知道您是否願意教我可以如何處理


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

1. int MotoSpeed = 250;
   
2. double CNY70Val = 1000;
   
3. int interror = 0;
   2 D4 d7 L8 ]+ B9 m; P* O# {
4. int olderror = 0;
   
5. double values;
   
6. 
   
7. 
   
8. void CNY70()
   . T1 Z. T" j, Q  e5 Y2 g
9. {
   
10.   valuesRR = analogRead(RR)
    / u" G: N6 L1 q/ {
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    # N6 ~$ `& K; z: _
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    1 I- A, t" F/ h
15. 
    
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    $ E: C( S, C; f! r; d
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    & x# E/ s3 T. w& Z
21.     valuesMM = CNY70Val;
    8 p8 r1 @5 M) [; W. S3 P/ X4 I
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    9 y5 D: y! m! e- @
26. 
    ) k0 Y" x3 m% T2 ?3 x; a
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    1 G; Y% n/ {( Z9 P7 m8 ?+ O9 G- c
28. }
    
29. 
    ( X0 T7 C: Y4 r( }* q3 j
30. void Car()
    4 m3 l8 m7 K+ j1 y
31. {
    
32.   while (1) {
    
33.     CNY70();
    
34. 
    : t" v& q$ J/ v6 Q/ b+ i
35.     int error = ((int)values);
    
36.     interror += error;
    ; Z" l( A! J4 q& R, L# Z0 X; Z2 g
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    0 [# P& A5 m4 ]4 |, |
39.     int power = error / 5 + interror / 10000 + lasterror;
    + u! {  p. E% i0 ~/ L. }) H$ c3 m
40. 
    
41.     if (power > MotoSpeed)
    " M* ]2 I4 `) D
42.       power = MotoSpeed;
    & h8 H5 G# Q6 i3 {4 i
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    
45. 
    . F# E% q0 y: q8 p
46.     if (power > 0)
    2 U$ v+ b" G) B
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    7 f1 ~( f0 ?/ T& x% t( k
50.   }
    
51. }

複製代碼

感謝分享

感謝分享