PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   
3. #include <Servo.h>
   
4. 
   
5. #include "MePort.h"
   4 e8 g$ _( R! C4 |( \' Y' t
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   
8. 
   
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    
12. float MinPower = 120;
    6 z5 u6 r4 G* A1 U+ P
13. float Error,ErrorAcc,ErrorDec;
    1 B& z) E' s5 r( F' l& v' K" h, @
14. 
    % ?% A' x# @, W; h- P" k6 m
15. float Kp=0.14;
    5 T3 I2 N5 A& R  |4 H; X' I
16. float Kd=0.00020;//23;
    ) o' C: P, q0 V7 I  N. m% M
17. float Ki=0.000201;
    
18. 
    ; U- k9 h2 j  z1 \6 B. r2 l' Z
19. float nPower;
    4 I& `4 l& g+ `' p: x0 A# s' [
20. MePort lightsensor_6(6);
    ( f; C5 y' S' ]* y2 P/ B" r7 \: n9 j
21. MePort lightsensor_8(8);
    / D7 v3 ~+ ^7 H
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    / p0 Q; o9 q' \6 h9 h) i1 ?3 N
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    2 H8 L7 e$ v- _/ {* v" t- H
26. 
    " Z( X4 I  |( |+ r) K
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    & p- p$ F* c4 F& N" D+ ?1 Z
30.     Error=0;
    & r6 I$ A4 ]3 \; E& n
31.     ErrorAcc=0;
    , g# m/ }: d1 T! |3 k+ c, B4 e5 s$ B
32. }
    + x/ X. Y7 w8 q% H9 d3 |. y$ r
33. 
    2 G4 _7 O) L0 r+ `3 H. H
34. void loop(){
    ! _4 r1 \. a! X1 j- D  ^  k( n- X1 G
35.   unsigned long StartTime = millis();
    
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    , g* h1 Z+ D4 f2 c- v( C% W2 z
38.     ErrorAcc +=  nError*Kd ;
    : M* a& R$ n. {
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    & B9 m/ o* B, K  p( Z# y
43.         nPower += 3;
    
44.       }
    
45.     } else{
    ( ^2 p1 S7 ]2 J1 U* s
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    2 ~* e3 [8 p) j1 D) c0 n) u( }
48.       }
    
49.     }
    
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    
52.   }else{
    $ Q! q7 W) r5 ?: ~! y: `: D+ e2 r' G
53.     motor_9.run(0);
      z1 t+ @1 ^/ ^3 ], s
54.     motor_10.run(0);
    
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    
57. }
    ' B& f0 a/ Y" u" ?5 \
58. 
    2 K8 b/ E8 a* i6 F) X) j* c/ ]
59. void MotoL(int Power){
    : H1 x3 j: u9 I) k: `( P
60.   if (Power > MaxSpeed){
    4 D4 Z+ m9 `, M" l! l
61.      Power = MaxSpeed;
    
62.   }
    0 u- j, R6 i2 j$ t4 J0 y
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    / i# \- Z; B/ R
65.   }
    
66.   motor_9.run(Power);
    
67. }  
    
68. 
    
69. void MotoR(int Power){
    8 M$ ]0 }0 \/ X6 D
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    
72.   }
    
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    
75.   }
    5 o' h, D0 r3 b- x! o! H
76.   motor_10.run(Power);
    , b! A: u6 i  d6 w/ D
77. }  

複製代碼

' k+ l2 j/ M) h& M% Q# v- u9 z: Z( L

/ P' z4 Q! n; V* {$ x
# N4 S/ h/ O; l! \# }+ X) c! M您好，不知是否能向您請教。
+ O7 Q: }7 H5 U/ M# R+ M目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況

但基於想追求更精簡的程式所以還是想請問一下
就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
" j: ?1 Q. n0 m1 t不知道您是否願意教我可以如何處理


8 v* G* J2 ^1 Y# @; @以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   : w: z, p3 _# w  h+ U0 r. Z( s
2. double CNY70Val = 1000;
   
3. int interror = 0;
   
4. int olderror = 0;
   
5. double values;
   ! C' f& a6 ^7 }6 t4 r- b3 C$ L0 l
6. 
   1 ?  o' [9 z; R6 B' |. H# N
7. 
   
8. void CNY70()
   
9. {
   + |/ T  Z$ s& j& G: o+ E
10.   valuesRR = analogRead(RR)
    ' d$ {! k" u! {8 m, h
11.   valuesMR = analogRead(MR);
    ( o9 C+ L! K9 n1 n7 R6 X2 Z; Y' ]; r6 O
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    8 ^4 t; p7 J2 Z/ ]( I
14.   valuesLL = analogRead(LL);
    7 z2 l( {% R; \- ^  l; I& P
15. 
    , o' B8 a- J! l, j* [1 \
16.   if (valuesRR > CNY70Val)
    2 X: i4 Q7 g4 E. d3 w! d
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    3 T. f( v. U0 V* w$ }" K9 L
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    7 d$ F1 M, u2 j$ L0 T
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    & }1 p0 ?) C* u  z( @7 ?6 _
24.   if (valuesLL > CNY70Val)
    9 n2 x( G1 ?: k) Z+ T
25.     valuesLL = CNY70Val;
    7 ^6 |1 u. _2 {2 _9 c& p  h
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    9 d, n& g$ q8 N8 k
28. }
    
29. 
    % T/ ~7 i: x0 K5 P6 H
30. void Car()
    , r9 G6 i! y- q' \$ Y8 d
31. {
    
32.   while (1) {
    
33.     CNY70();
    2 b0 W" n% a! ~2 b* v3 z4 l: o, `
34. 
    ; b5 O/ w7 U9 F4 G
35.     int error = ((int)values);
    
36.     interror += error;
    & E( {" n( Q  M" C/ D5 y) s% S: l
37.     int lasterror = error - olderror;
    / ]5 H9 D- q7 W" x+ P2 K) [
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    3 q4 x# D$ J& E- y' D$ b, I0 ?/ X7 e
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    $ A8 x7 b! B8 w' X5 f  v
45. 
    6 b3 {2 B6 y6 i$ G8 h6 N
46.     if (power > 0)
    - t) l$ ^) V: E  w9 V5 O
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    9 x* j5 r* a$ V1 ^7 L1 |$ V
49.       Speed(MotoSpeed + power, MotoSpeed);
    2 Q6 Y' b% @8 b; ^
50.   }
    
51. }

複製代碼

( g$ J) c' S2 k! z

感謝分享

感謝分享