圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21786|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>7 n6 t; Z9 q' i$ s, h+ l
  2. #include <Wire.h>; h3 n6 |. b, q' |- s. Z
  3. #include <Servo.h>
    8 y5 B& p! c, _5 K
  4. / `1 X/ T& X$ M) ^6 A0 M9 ^
  5. #include "MePort.h"0 q  o! n9 ?: u# H' i
  6. #include "MeUltrasonic.h"7 l  L( R; L- S9 @7 e7 g
  7. #include "MeDCMotor.h"
    6 T  M& t7 H8 c5 Z
  8. 2 o6 g& w0 G+ P( L. J
  9. //double Input, Output ;: X! F) E5 a$ N1 }3 ]
  10. float MaxSpeed = 255;  D6 i4 Z0 q1 z" t* n$ G
  11. float MaxPower = 180;/ J$ f+ B0 t/ D5 ~
  12. float MinPower = 120;6 h- G6 W* h: n" p2 F
  13. float Error,ErrorAcc,ErrorDec;2 X4 T( g+ Z; {3 N$ J; \

  14. # \% x; ?* @9 p+ ]
  15. float Kp=0.14;9 U' a5 \( [- }* k$ h7 z- q
  16. float Kd=0.00020;//23;
    % z1 S: }5 v5 k. W
  17. float Ki=0.000201;; _* n$ z7 \2 C

  18. 7 _1 W* n. d* q! m# y
  19. float nPower;
    9 s2 L, R' l7 j. ~, ?) D
  20. MePort lightsensor_6(6);
    9 A' H0 s/ Q  O) O& F0 i+ p0 z7 ]
  21. MePort lightsensor_8(8);: P+ O' p# t2 ]4 A
  22. MeDCMotor motor_9(9);
    ; O0 V) F4 |: V  D
  23. MeDCMotor motor_10(10);
    ' S9 E0 f( ?2 o4 T! p
  24. unsigned long previousMillis = 0;8 n2 g% z& w7 Q3 ^, V6 {
  25. const long interval = 1;+ C0 r2 @7 R% h6 c2 m4 Y, {
  26. # S& h) n, M. ^
  27. void setup(){! F0 {! h) ]4 l2 }) d# K
  28.     lightsensor_6.dWrite1(1);
    8 X% {- N  z6 Q' F- _, F
  29.     nPower = 160;
      A3 S8 s+ `( }- H7 m8 A# W1 ^
  30.     Error=0;
    4 ^5 R/ J2 O' [9 Z5 N2 q. p% P" J+ g
  31.     ErrorAcc=0;1 u* C1 X9 l6 b
  32. }# B) Z/ b5 B  ?! u7 ?/ b6 {

  33. 9 q8 M' n  z0 y' L
  34. void loop(){  T' D. i3 B0 L$ F' @) [5 i
  35.   unsigned long StartTime = millis();
      Z" X' m" a( Y% y4 i9 A( X
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    & D) {& y$ R) T. }6 T* H
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();, O" V8 u$ c: @" H2 b9 d
  38.     ErrorAcc +=  nError*Kd ;; K3 E, ]8 I# y% f
  39.     ErrorDec -=  nError*Ki ;7 X  r/ `8 Y0 N! g2 r
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);$ z' |7 p1 b' h, C% E
  41.     if(nError < 80 && nError > -80){
    + u' M: g/ d1 F+ |0 D) y
  42.       if(nPower < MaxPower){
    ' P5 `/ p' |) x: i- N2 p
  43.         nPower += 3;# n# ?1 P( Q% Z" f% p/ c  s
  44.       }8 N# g) q" }" N# W5 o# B& o
  45.     } else{
    # j2 L& ?& I" t  f
  46.       if(nPower > MinPower){& ~" X: j# @' d0 e8 o8 |
  47.         nPower -= 2;
    " H, Q3 e  `: Q( ?' l' w& X
  48.       }5 y* R, P0 L$ T7 L+ _
  49.     }
    2 n5 v0 @8 T! l" b# H% j3 K0 ^! [
  50.     MotoL(nPower-Error);
    ( V$ m1 i6 ?5 W& O
  51.     MotoR(nPower+Error);   
    * X; R. ^- {' j* N  k5 v& e
  52.   }else{
    3 g. c: T' L3 e
  53.     motor_9.run(0);
    / }) c) h( m& ^5 t3 C6 }) C1 u
  54.     motor_10.run(0);9 z& Q' p" j/ E! d* Q: D9 e
  55.   }
    7 f5 S0 V$ g$ m: X( h
  56.   do{}while(millis() - StartTime < interval);
    ( K# W( {; f9 l3 i
  57. }5 N. R, K. N7 r/ g3 O
  58. $ t& U/ i7 E+ p* G
  59. void MotoL(int Power){
    / @0 V; p! P- C+ X) W$ D+ C
  60.   if (Power > MaxSpeed){9 m" V: E% o- L# m+ X
  61.      Power = MaxSpeed;/ L/ I5 j6 v6 R' b- {
  62.   }
    ; Q. q; `/ P9 R
  63.   if (Power < -MaxSpeed){; f, p6 E& W$ W0 ^2 @! }
  64.      Power = -MaxSpeed;/ U0 n. t  ], f' }8 T9 d  w
  65.   }
    * V# }' Q2 o' _( ^$ Z* V
  66.   motor_9.run(Power);
    - J2 v: D1 P: ~
  67. }  
    5 L  J: x' M1 z  u0 `" g/ H
  68. 3 g( K1 V+ z: R& U! B3 a7 x
  69. void MotoR(int Power){' h3 ?# J, |% j; z; S3 X
  70.   if (Power > MaxSpeed){$ z9 U, p- C2 M2 q
  71.      Power = MaxSpeed;
    ; B0 R" d& _/ r; s$ n; \
  72.   }
    ' m' g: {. ]$ e3 r0 r" E
  73.   if (Power < -MaxSpeed){
    $ O8 G" G" a; {* w5 Q+ g! j
  74.      Power = -MaxSpeed;, p( m8 I* f  t
  75.   }
    - R( |- q; }2 r2 P% j) @- Y+ f, A% z
  76.   motor_10.run(Power);: ~7 f* F4 Y! {+ i4 b1 D+ `0 {
  77. }  
複製代碼

' [' X+ A) n, S( O1 ~' F0 `4 a
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯
; w% r& \! O# N/ e$ h7 Y/ l1 p" ?* T
您好,不知是否能向您請教。
% t! Y0 w* w- m! W9 V: Y; _( V/ h0 B目前和宋修賢老師在處理Ardui Car2 }  w3 Q( k* S' g# @' T4 O
雖然已使用較繁雜的方式處理了跑出黑線外的狀況
6 X9 k( N5 \# n$ f" P* j; C. e
& v6 F; n  o! c, |, a3 t6 l但基於想追求更精簡的程式所以還是想請問一下; E# x2 H; J* f5 B9 U* A5 X
就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝
$ m! h- O) A5 V: z不知道您是否願意教我可以如何處理
! ]8 z. P8 M6 L3 x6 ~( n9 @; ~1 f/ v1 p/ N2 d7 }
2 ?- s4 g, s$ w& T, B4 G2 `. ]
以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;3 k6 i6 g8 k# W) [+ {. ]( C
  2. double CNY70Val = 1000;8 V$ u1 v, I( P! P+ W8 Z
  3. int interror = 0;) \8 Z) D* x! C0 g4 C
  4. int olderror = 0;
    8 U5 \, j, Q+ ^% K5 u
  5. double values;
    3 H1 |' i# y& [; Y; _; ~& f. T
  6. ! V: Z& `* P8 f: s& n
  7. . {3 }. D; C! e5 ~  `# c6 s
  8. void CNY70()" |+ U' ~$ S0 j1 g( G' [- [# K
  9. {, @0 M. L  F4 T/ b, m; d
  10.   valuesRR = analogRead(RR)
    * l1 `) u, O+ O, |2 |5 g  N
  11.   valuesMR = analogRead(MR);
    ' S; M) U, n2 ~0 d
  12.   valuesMM = analogRead(MM);
    $ `2 I, t! ?$ I* s5 u% V3 p! e
  13.   valuesML = analogRead(ML);: Z  J) _' V4 k$ h0 x2 h4 w& T/ ?
  14.   valuesLL = analogRead(LL);
    2 ~; R- M5 @% G+ D) N2 s! w

  15. - [1 }" a/ l0 D  f) f
  16.   if (valuesRR > CNY70Val)& t6 Y+ M6 f9 K0 }' Q+ c* J$ B  A1 G2 {
  17.     valuesRR = CNY70Val;! W& A- W5 c  y7 M/ o$ H
  18.   if (valuesMR > CNY70Val)
    ( }7 E  D5 i7 K0 h
  19.     valuesMR = CNY70Val;* w" _- F: I' w3 |- v* K
  20.   if (valuesMM > CNY70Val)
    # w4 _. c% I- v4 z( k4 G* w( a
  21.     valuesMM = CNY70Val;
    ( F( j+ @8 |4 p6 ~
  22.   if (valuesML > CNY70Val): _* b, }) C8 e' _% Q- `
  23.     valuesML = CNY70Val;! q2 P( m: @" e9 ^
  24.   if (valuesLL > CNY70Val)7 }9 J" j( d( T, @
  25.     valuesLL = CNY70Val;
    , i/ }" X5 w2 M( O
  26. 3 Z$ d: R7 q- a1 ]% N7 ~
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;, G4 W+ [; C) M3 Q: S/ Z9 c
  28. }. u- q! M) Q* R4 Q4 c
  29. 3 N4 R$ y. S( q9 ]; U
  30. void Car()) ?. ]. q( C* a7 C& Q/ A
  31. {) N" D/ r7 L4 B0 Z6 B% D
  32.   while (1) {5 s/ M  M3 s* w5 e5 L
  33.     CNY70();. ?; ^8 g* B! E4 q. Q
  34. ( {0 O; I" l* W1 {; s# ]8 o6 B
  35.     int error = ((int)values);
    ! k) |7 U; {7 D: S. N8 v0 o2 @
  36.     interror += error;
    2 x3 G# P2 k2 V- J' j5 _2 _
  37.     int lasterror = error - olderror;
    # V0 `1 c( e+ K0 f' z8 J5 U' u
  38.     olderror = error;
    8 t! E# w! w) j4 k7 e( ?
  39.     int power = error / 5 + interror / 10000 + lasterror;* E  r3 R5 m+ C* P6 f5 }0 R5 {/ ?

  40. * e4 V0 ~  V0 V  O
  41.     if (power > MotoSpeed)4 Y  p, Q, g1 m3 p( z7 S. e
  42.       power = MotoSpeed;$ m3 I+ ?1 b4 E5 s9 i+ T, y
  43.     if (power < -MotoSpeed)3 j* c2 n; c: m; b
  44.       power = -MotoSpeed;
    + ^. Z- N8 V/ ~& W1 ^& z7 f
  45. 7 `& Y8 L$ Y  V/ u: x! z  Q; p+ |! Z
  46.     if (power > 0)
    5 Z# I. V0 J# d+ a3 [
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。  A$ }0 g( u3 ?# K& j9 S. R7 G
  48.     else7 ~+ I4 V8 D2 V, J( V
  49.       Speed(MotoSpeed + power, MotoSpeed);
    + n# p/ t) T! |8 J  a' |& n; P
  50.   }
    + K# P3 K4 Q" d' S0 X
  51. }
複製代碼

6 u$ l/ P3 U% O7 F
7 [# x( F: g* z% Y* P
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-11-30 20:24 , Processed in 0.024027 second(s), 18 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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