圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21728|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>
    9 l$ d- ~+ Y) R3 l
  2. #include <Wire.h>
    ) f- |9 d' b3 P. x1 C+ @' c) W, g  Y
  3. #include <Servo.h>& J% m& ]) s  U, R* I- x
  4. $ A4 ~1 k( Q4 }, V
  5. #include "MePort.h"3 t3 b+ {. M  p$ g+ J7 i* V2 s- `
  6. #include "MeUltrasonic.h"+ r4 f! g9 f9 M) C
  7. #include "MeDCMotor.h"
    ! y( z% w. w7 i( N* Z. R" b3 c

  8. 5 S0 l, ]( x6 d4 A" x
  9. //double Input, Output ;, M+ ^# `4 z1 L9 `+ s1 M# v
  10. float MaxSpeed = 255;$ p  q5 y* c+ U# p4 n* X6 i7 D
  11. float MaxPower = 180;& b6 Q5 \$ F! ?/ @4 Y
  12. float MinPower = 120;; v8 e4 E$ t1 J: M
  13. float Error,ErrorAcc,ErrorDec;
    * M2 S3 ~! w% C2 t' d

  14. * R0 Z  G) `0 s1 r* Z
  15. float Kp=0.14;0 {0 x! G) A) b# A) v" e, w6 g
  16. float Kd=0.00020;//23;
    " n" f8 w: T# n/ a5 Z, ^% u
  17. float Ki=0.000201;* w4 Y9 [- s3 [1 C1 X- `9 A

  18. - |6 X3 Q+ M0 V
  19. float nPower;: a, q- G8 l; a. B  o* n
  20. MePort lightsensor_6(6);
    . q% H! b; s2 S% X9 F
  21. MePort lightsensor_8(8);1 A; J8 ]; f" k' H4 I
  22. MeDCMotor motor_9(9);- v, ]. E( e: V; D, F
  23. MeDCMotor motor_10(10);
    - Z# F: \  q) }
  24. unsigned long previousMillis = 0;
    . L$ ^0 o- G: a6 l
  25. const long interval = 1;: U& C) z( A0 ~0 k' P& W

  26. % z$ t6 B3 Q. B) }# j
  27. void setup(){
    " F, `1 q$ z+ c# S# w# \; a
  28.     lightsensor_6.dWrite1(1);* D0 Y* f& J7 d3 ^0 {
  29.     nPower = 160;
    & T3 q& r2 b/ J2 @
  30.     Error=0;1 E. y+ K, D1 O2 d2 T  V: a
  31.     ErrorAcc=0;
    + w9 S7 f/ l5 r5 p, ?/ ?3 y6 |
  32. }# `9 o/ X6 G5 B& g  j6 o
  33. - z2 S% O+ ]3 b! Q$ ]
  34. void loop(){: w. K6 \% D8 T! Q9 Z; G
  35.   unsigned long StartTime = millis();! f" X0 Q6 Q& V$ [
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){% G, x3 ~! X8 e
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();( C) n, O, V  P( Y6 T4 b8 j7 k
  38.     ErrorAcc +=  nError*Kd ;
    : S! ~% g5 l5 H' R* ~
  39.     ErrorDec -=  nError*Ki ;# O3 v: [- E$ H; r' E/ ], m
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);' U: A  E8 E# P$ @% W( P
  41.     if(nError < 80 && nError > -80){1 v6 K0 r1 a1 s. |
  42.       if(nPower < MaxPower){$ [9 c9 N$ c, Z/ D7 h  n! K
  43.         nPower += 3;
    5 B, b2 E  `( A  b
  44.       }
    , m. [" u; F. S" g  `
  45.     } else{0 ^7 E- k' l+ H3 a+ ^; E  ^
  46.       if(nPower > MinPower){" T0 F9 Q3 ~% U; ^% b1 p) @
  47.         nPower -= 2;0 ]4 x4 _+ X! D0 X
  48.       }
    - g( G- q! }" C* Y% h
  49.     } : H& t4 ?+ w( h2 X) J
  50.     MotoL(nPower-Error);9 e7 X- x% j: `
  51.     MotoR(nPower+Error);   
    8 g- n/ \6 h6 L% t7 {" R
  52.   }else{8 F2 `( K' b0 f* ?6 Q" G
  53.     motor_9.run(0);
    . b4 g, `3 l4 w' [5 }
  54.     motor_10.run(0);3 d6 l4 {6 k- v3 n
  55.   }
    0 Q2 r6 q( z# Q+ F3 k9 E  H: o
  56.   do{}while(millis() - StartTime < interval);
    1 B$ j; w+ P/ s7 U1 j# @& g
  57. }
    9 H! u' g% q' Y4 F6 R

  58. 0 r% x% R' K; D1 x4 s  U5 g$ q' q
  59. void MotoL(int Power){- Z9 T5 q. w+ z' z/ a
  60.   if (Power > MaxSpeed){  k6 Y1 ~2 j7 c, g& G. E' V& `
  61.      Power = MaxSpeed;; J( D: ?3 X9 y2 D, \
  62.   } : M5 q7 A* c) d0 ^# v, Z) m
  63.   if (Power < -MaxSpeed){# C- j- G" ~; k7 l7 r$ @
  64.      Power = -MaxSpeed;
      }1 p1 F/ B- f2 y5 i* \9 W3 K2 h
  65.   }
    ' e5 V! A: r0 o1 v' I: t5 [0 h
  66.   motor_9.run(Power);. R# _0 J- K6 J7 w
  67. }  
    & D3 L5 S1 M8 M7 N# A
  68. 1 p  K2 W7 E: A$ }+ j1 o
  69. void MotoR(int Power){
    ; i' q  s9 v8 H1 h- y7 b4 y
  70.   if (Power > MaxSpeed){' L1 C5 G9 r8 O# ~, s
  71.      Power = MaxSpeed;6 M# D( I+ O2 Y% ?. M- y3 i, s
  72.   } * ?3 h+ A8 U+ d, F5 t1 l
  73.   if (Power < -MaxSpeed){  i* D, I/ j9 S: _$ u1 w$ O- _! }
  74.      Power = -MaxSpeed;8 X4 @* y; u1 p' W
  75.   } $ Z, G  k; m5 A  S  S4 `  o& G
  76.   motor_10.run(Power);8 `+ |) x- F0 K; ?* E; p; F; r
  77. }  
複製代碼

3 f/ _: F1 @* H
9 T, k! H' W0 Q8 ~5 m. d# \* z
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯
  y$ V# q* ^! T" u2 L
4 p7 O% H9 S. |: o# i3 x* q. w您好,不知是否能向您請教。
9 U3 ^$ n& m3 S目前和宋修賢老師在處理Ardui Car4 C1 q5 H) D: o- c% d& z6 p
雖然已使用較繁雜的方式處理了跑出黑線外的狀況+ i4 R  `# @; P1 q: l7 Z8 ^5 U

8 b7 u4 ?) [6 p8 u8 z2 r' Z) }2 q但基於想追求更精簡的程式所以還是想請問一下/ S5 f7 q- e3 Q1 P# ]
就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝* H$ `9 c' ]* T
不知道您是否願意教我可以如何處理0 ^$ a5 g& x' T2 I4 m

! [4 ?2 ^# a0 d' E* l
- J: p+ a1 O1 Z/ h2 c以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;
    . u$ S; D2 f8 P: N
  2. double CNY70Val = 1000;
    4 f& A$ i5 A* X% i, \" H
  3. int interror = 0;0 Y& A1 O) c3 K# t) a6 U9 W
  4. int olderror = 0;* s/ K& `; ^0 [9 F) {
  5. double values;
    0 \* L2 d/ L7 n: f
  6. / v7 J2 l3 O) U8 g( _, S* a5 n
  7. 7 @* j/ Y/ Z( M% F1 V5 ]7 ^& m
  8. void CNY70()
    ! n0 O# R* J- G/ I
  9. {- z$ |( b% S6 L% a& l
  10.   valuesRR = analogRead(RR)
    * f  k4 d/ i* j) z3 `: V! I7 U: ]
  11.   valuesMR = analogRead(MR);
    8 y& q& x  d; e+ V& }( p
  12.   valuesMM = analogRead(MM);, o- ?8 A% E2 }% \  w% p: ?
  13.   valuesML = analogRead(ML);3 y+ f) \5 q- B
  14.   valuesLL = analogRead(LL);9 F7 ~. C5 t6 d8 x; b5 M2 }
  15. 6 r. L$ h& }3 K3 T; u4 x. a
  16.   if (valuesRR > CNY70Val)
    9 ~3 ~  z* `' A, T
  17.     valuesRR = CNY70Val;
    9 x( I: g- t1 P1 ~
  18.   if (valuesMR > CNY70Val)
    . Y% v' |+ O; N$ B/ ~  [3 o
  19.     valuesMR = CNY70Val;
    9 f0 J7 z2 j3 G! O! K! F
  20.   if (valuesMM > CNY70Val)5 t5 G; p! F; h- a, b) G
  21.     valuesMM = CNY70Val;
    1 x0 ^6 X2 w8 t2 {/ O$ @
  22.   if (valuesML > CNY70Val)  G# c, q2 |, g0 x' U& p- n3 |
  23.     valuesML = CNY70Val;9 |1 j. `3 h+ }, h" }  b
  24.   if (valuesLL > CNY70Val)6 q$ q! U7 H5 V
  25.     valuesLL = CNY70Val;
    / V& ^# \7 s; n: g' \8 ^9 Z
  26. + J) `) i8 u! `/ W* D7 \( y- `+ y
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;; f9 I' j; J* O6 y) N
  28. }
    * k+ n! y$ @% u

  29. . O% l& t4 |, q8 L
  30. void Car()
    8 s# Q2 E8 C+ s; s$ O% _
  31. {
    $ p$ s3 V5 ?& ?3 b
  32.   while (1) {
    $ b! j# G; z$ y, j; S/ k
  33.     CNY70();
    : u4 U9 q2 Y) t
  34. 8 j5 `* [# q3 p4 E. N. J+ V1 E
  35.     int error = ((int)values);
    , C% r' E' {; f5 @
  36.     interror += error;
    9 H& [" c( h" v* q, R% D+ ^4 U$ r
  37.     int lasterror = error - olderror;8 C% h3 L! J' d4 N5 A, N
  38.     olderror = error;; Y: j. d& \/ K/ `) F
  39.     int power = error / 5 + interror / 10000 + lasterror;. E- p2 L: t# M5 m$ V; p* R

  40. 0 F+ K/ ~0 e" V6 V' c. R
  41.     if (power > MotoSpeed)) m$ B8 `3 b  Y1 o/ {
  42.       power = MotoSpeed;8 `: L) `) K9 B
  43.     if (power < -MotoSpeed)+ r# h* Z" n) z; M4 a' j
  44.       power = -MotoSpeed;% W, |; @+ l5 b, q/ c
  45. % b3 L& K2 a! l7 b
  46.     if (power > 0)0 M2 }$ q. \' ^1 [+ l3 |# \$ |
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。3 J9 q8 f" T( V
  48.     else$ R" V1 u  k  x! g+ e2 G$ a! [- _2 W
  49.       Speed(MotoSpeed + power, MotoSpeed);6 b/ o1 y, d( \. c7 {
  50.   }
      C' L$ p% \( d+ W- b' o1 N7 Z. u4 N  v% |
  51. }
複製代碼

4 |- L7 `2 J% Y" B6 p8 h3 i; G0 I3 l7 Z: n$ l
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

QQ|Archiver|手機版|小黑屋|圓創力科技有限公司 IOP Robotic Technology Co.,Ltd Tel: 07-3924582 Fax: 07-3924001

GMT+8, 2025-11-27 17:05 , Processed in 0.024375 second(s), 17 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

快速回復 返回頂部 返回列表