PID 循跡自走車範例

1. #include <Arduino.h>
   * p+ ?8 [! i  W4 z$ d
2. #include <Wire.h>
   
3. #include <Servo.h>
   ! \6 z, C2 a) H) J/ x# @
4. 
   ( C: c$ s& T$ B- h
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   
8. 
   4 z. h) E  N8 Q7 S3 [" p
9. //double Input, Output ;
   0 \9 G0 R1 p2 m2 p. V0 T
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    
14. 
    
15. float Kp=0.14;
    6 S1 ]+ z5 ?. r  g! d
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    
18. 
    : a. H* i) [6 E& N6 m0 ~9 \$ j- ]
19. float nPower;
    ; h5 @5 |( Z* g. y$ _4 `" n
20. MePort lightsensor_6(6);
    : z# m' g: \: ~" N' d2 i
21. MePort lightsensor_8(8);
    ; O3 A1 R, b0 p1 c0 I, o$ p
22. MeDCMotor motor_9(9);
    . z) R% n" x% w, Y, C* {5 V
23. MeDCMotor motor_10(10);
    9 w# J: i$ F8 E! Z  k; d5 R% E
24. unsigned long previousMillis = 0;
    2 A9 v3 m9 t7 g: x$ G: \
25. const long interval = 1;
    ' v/ `2 }# R  k/ ~5 ?
26. 
    
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    
30.     Error=0;
    
31.     ErrorAcc=0;
    # l* u% Y. S" W- ^' w& K
32. }
    
33. 
    " F; ?! h7 d8 i
34. void loop(){
    
35.   unsigned long StartTime = millis();
    + B; _% E2 y' E
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    9 A0 p# z6 Y5 y, H8 J9 e
41.     if(nError < 80 && nError > -80){
    ; o2 H5 q3 g+ ^* ?! o  Z
42.       if(nPower < MaxPower){
    1 J# @( p  D- s3 h* Q, p
43.         nPower += 3;
    
44.       }
    : w4 K. k! B& }  p2 @$ g: m
45.     } else{
    
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    
48.       }
    
49.     }
    0 M! Z" l3 i! u) V9 P/ v0 H
50.     MotoL(nPower-Error);
    & e- r( h( m/ O$ t
51.     MotoR(nPower+Error);   
    1 D6 R. i8 [$ G5 J$ L4 }/ p
52.   }else{
    ' E$ W% N) f( x0 b5 x5 S
53.     motor_9.run(0);
    % A. ^5 C/ A( m( C8 G
54.     motor_10.run(0);
    
55.   }
    - B* [+ J; C7 U2 v
56.   do{}while(millis() - StartTime < interval);
    
57. }
    
58. 
    9 }. P; A& R9 Y, M& @
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    " m+ e, x0 t, H' O% u
61.      Power = MaxSpeed;
    
62.   }
    3 o0 _3 Z. ~8 Y- Q2 w( j+ h
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    
65.   }
    7 m0 l2 Z' w$ _8 P* K; U% I; u
66.   motor_9.run(Power);
    / u. t) T6 T& ]; t0 u, i
67. }  
    7 Q3 |6 ~: O: ~$ M$ v7 u
68. 
    % C& L* i" M4 c8 N0 @6 M
69. void MotoR(int Power){
    0 ~( V$ O; m# Q, H
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    8 h& m: f+ Y3 b0 L: o. O, p
72.   }
    
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    
75.   }
    9 s2 m9 N1 j9 j# Q- [  e
76.   motor_10.run(Power);
    + T& v  ~+ k  o* k1 [
77. }  

複製代碼

- W3 O/ Y$ @& v7 O
: w& r+ n2 K" L$ b$ D0 A9 r

您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
) M0 P2 E' X  _# A4 U  k  ^雖然已使用較繁雜的方式處理了跑出黑線外的狀況

但基於想追求更精簡的程式所以還是想請問一下
  C8 {  i( o% s! C3 Z! ^就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理
% T# {1 W) j" G* Z7 W' ]/ U- ^& |
' ?8 R0 C& l; U; @* q/ D
  z6 O9 n. T% V. Z8 r2 L; F1 v以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   
2. double CNY70Val = 1000;
   
3. int interror = 0;
   
4. int olderror = 0;
   # a( C9 D$ {( g) S, W+ S9 q( \
5. double values;
   
6. 
   
7. 
   
8. void CNY70()
   4 }4 u2 M! o  g" [
9. {
   
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    4 P! y, U$ V' E6 f
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    
15. 
    
16.   if (valuesRR > CNY70Val)
    9 I* r! K  X" a) B/ _8 T/ G
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    0 U  p/ s8 t! s0 K! S
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    $ X9 H- m5 k! y1 h) Y
23.     valuesML = CNY70Val;
    8 U+ k% u4 ]% M1 f4 H4 M: d8 v7 I
24.   if (valuesLL > CNY70Val)
    6 i8 [# @8 @5 K' W. I* m) F8 s7 i
25.     valuesLL = CNY70Val;
    
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    " F( Z+ u/ f& Z% G3 L& e4 |
28. }
    
29. 
    ( L# o& Y4 o+ _, x, Q- k4 k3 J' n
30. void Car()
    
31. {
    - I( ^3 S7 q: \+ j2 D) v! g! N
32.   while (1) {
    ! Z9 W) _8 F  y# i3 u
33.     CNY70();
    ( n$ F7 ^" H* V( T: }2 T8 O
34. 
    
35.     int error = ((int)values);
    . D9 F$ `! ^% n+ Y- ~
36.     interror += error;
    
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    $ ?2 i- ]* |! Q9 y
40. 
    + d7 |, d0 t  H& |# J$ s
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    3 u, i  W+ f" s/ `: K' X
45. 
    # A; h  ^5 ]5 d) _
46.     if (power > 0)
    2 r' Y# W0 W( U8 n9 i' J( o  m' h+ e
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    + \' ^* L2 M' T! l3 B/ G% b
51. }

複製代碼

感謝分享

感謝分享