PID 循跡自走車範例

1. #include <Arduino.h>
   - @) Q; b& m, U( a
2. #include <Wire.h>
   
3. #include <Servo.h>
   5 E2 K" F& L6 d# x) v2 k! O
4. 
   ; o+ P& q9 Z2 r# G* n
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   ) _) B1 @- d8 B$ x1 `% c0 G
7. #include "MeDCMotor.h"
   6 k; F& X$ Q& m: }
8. 
   
9. //double Input, Output ;
     X! V) c0 Q" B4 p
10. float MaxSpeed = 255;
    + O/ A9 I: H! y7 Q$ F8 J
11. float MaxPower = 180;
    . U! }7 i& ~7 p7 l2 \: a3 m" y$ o. O
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    1 e/ k# R! G1 Y' ^% N
14. 
    
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    
18. 
    ! J, X% H9 u& e( ?
19. float nPower;
    5 ~  n9 X+ w- e+ A% R
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    % C4 q) s# J3 `
25. const long interval = 1;
    
26. 
    1 k5 ]' h- d  P  Y& A6 h5 K6 `
27. void setup(){
    + W! `& [. K, d: p0 g6 q; [
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    2 F+ ~2 l9 {0 N2 G' ^" z
30.     Error=0;
    
31.     ErrorAcc=0;
    
32. }
    
33. 
    
34. void loop(){
      e' J" N/ }  f( w9 o. C7 i
35.   unsigned long StartTime = millis();
    - t# i# ~7 u% t& W7 b4 ^
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    $ v- W6 Y* R& J7 H
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    
43.         nPower += 3;
    . r& a* d2 [+ d, n) h. O
44.       }
    
45.     } else{
    
46.       if(nPower > MinPower){
    6 l- {& \" h/ g( e) V( I
47.         nPower -= 2;
    
48.       }
    1 z. K& n* }) L8 C7 z& k
49.     }
    
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    , t( b- B! M) C$ w  n+ ~" k. K
52.   }else{
    ! x1 J; |# C6 R- N; y2 I) A! d6 d
53.     motor_9.run(0);
    4 ~% F6 {: K; ]+ r2 q( N) t$ |6 g: p
54.     motor_10.run(0);
    2 Y& Y6 A% g* u" V* k, f$ j
55.   }
    , s/ E; W7 H. e" l6 ~
56.   do{}while(millis() - StartTime < interval);
    
57. }
    
58. 
    
59. void MotoL(int Power){
    ( l1 {; V# i- v
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    4 }! f  l2 o1 j! m
62.   }
    " W: b' X" m! y5 w% \
63.   if (Power < -MaxSpeed){
    ' W1 @: q. n# v
64.      Power = -MaxSpeed;
    
65.   }
    9 p; z3 m: n( l4 j! Y. c
66.   motor_9.run(Power);
    
67. }  
    8 |; o( B1 ]& Z6 b. i1 p/ ]
68. 
    / u7 P9 q( v& x( ]' {0 _
69. void MotoR(int Power){
    . j/ n3 Q4 `2 e7 x  j. ~( @9 `8 z* R' F
70.   if (Power > MaxSpeed){
    , c4 `2 r0 o! I8 p, D
71.      Power = MaxSpeed;
    , `; ]$ k  d4 B# c
72.   }
    ) C9 q& r* i; e4 f  V/ l( m) @# {
73.   if (Power < -MaxSpeed){
    / K$ E( G( j) H( L; n9 {
74.      Power = -MaxSpeed;
    
75.   }
    5 f! U) D# s- c4 }2 b2 w  C
76.   motor_10.run(Power);
    / r, r0 t$ G- \+ @: m( ~
77. }  

複製代碼

您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
5 Q: f( A9 |2 [: u% x雖然已使用較繁雜的方式處理了跑出黑線外的狀況
! ~$ Z" V/ b5 U
8 y$ y- X& ^" K: o; J  y) A6 |  d7 Z但基於想追求更精簡的程式所以還是想請問一下
% b- P" E  Z! B4 q就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
( w9 S( o2 H! w1 D3 s% J* l, z9 E) Q不知道您是否願意教我可以如何處理


; }) {0 `0 Z; [以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   1 e; ?: q5 L+ R8 p6 n5 H
2. double CNY70Val = 1000;
   
3. int interror = 0;
   ( K% Z! g2 h& {5 R, V; s# e
4. int olderror = 0;
   
5. double values;
   
6. 
   
7. 
   
8. void CNY70()
   ( i6 \+ s$ Y: U/ _( g3 k
9. {
   
10.   valuesRR = analogRead(RR)
    7 @" d$ T% S* z4 e3 P" m
11.   valuesMR = analogRead(MR);
    . m) D" T$ l, ?! ^4 I
12.   valuesMM = analogRead(MM);
    $ L" X+ I' _6 }7 M# n
13.   valuesML = analogRead(ML);
    1 v& x% p) N) z* j/ ]7 T
14.   valuesLL = analogRead(LL);
    7 u" L! }$ E5 v5 x2 T( O( Y" ^1 q  ?; S
15. 
    8 U$ {5 u: G" W3 M! `: I5 R
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    2 i, c* _; U+ z4 @/ G8 l
20.   if (valuesMM > CNY70Val)
    ' ]1 A0 d( F, O. B
21.     valuesMM = CNY70Val;
    2 u  t: O! @$ `4 D+ r3 S, v
22.   if (valuesML > CNY70Val)
    4 N: U* v8 A7 Y1 N# T
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    ; ^+ D3 h% S+ v
25.     valuesLL = CNY70Val;
    
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
    
29. 
    " i1 ^( X* _3 t% U$ q9 S, ~
30. void Car()
    
31. {
    8 R4 n; B# x. x8 U7 O' j
32.   while (1) {
    
33.     CNY70();
    
34. 
    
35.     int error = ((int)values);
    1 g9 L: `5 S. }! ~# `
36.     interror += error;
    7 L% E3 ?% T( }% D. j1 |
37.     int lasterror = error - olderror;
    & n- n1 y( o/ ]# d  h. `5 b9 {0 ^
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    1 J) x1 X$ y( d9 X
40. 
    
41.     if (power > MotoSpeed)
    % r# u  @& V) W! S5 b$ ]! {
42.       power = MotoSpeed;
    / ^; a6 }. B4 i3 W5 _+ d6 n* {  T. H
43.     if (power < -MotoSpeed)
    ' g6 G9 w+ N0 I, n. F  m( ~. H+ p4 N
44.       power = -MotoSpeed;
    
45. 
    / \; @, F/ V: p4 r4 T
46.     if (power > 0)
    $ w8 \9 L6 N/ a+ V( ?" I
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    4 ?, T& y! |9 ]1 y
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    & j- K3 ?9 D/ o+ D. X
51. }

複製代碼

& ?9 X" K) D) F% L* h0 ?

感謝分享

感謝分享