圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21723|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>
    : W2 ^$ Y* x- s  F) h3 [! P8 ~
  2. #include <Wire.h>& T0 I- z2 z/ W3 @' [
  3. #include <Servo.h>! K( u4 X# E& w: g5 ], `  o

  4. % p, `* X& D+ |  e: I) v
  5. #include "MePort.h"9 \+ f  G+ t& a1 ^; ]$ B$ F1 f. v4 f
  6. #include "MeUltrasonic.h"# D9 @; [! }3 Y+ o) R; K+ r
  7. #include "MeDCMotor.h"
    + y- L' D0 `2 z7 z: z. B" T4 X, q
  8. * x% |% R7 z  Z8 n% N; c
  9. //double Input, Output ;
    $ @& m6 t  T8 q( V& _
  10. float MaxSpeed = 255;$ {8 v' S. o+ v# Z2 P
  11. float MaxPower = 180;3 F* u( D8 B* d0 a
  12. float MinPower = 120;
    9 v, R) [$ b, L& H
  13. float Error,ErrorAcc,ErrorDec;
    ) J/ w$ G/ B4 e2 W9 q. i! p* I6 U
  14. ( r' o5 A( x* V$ G- h) a7 a! Q; Y
  15. float Kp=0.14;
    ; ^0 P' J( b9 V; q/ \
  16. float Kd=0.00020;//23;
    - P, B" x2 R, |4 g/ n
  17. float Ki=0.000201;% q) g/ E! B5 j6 g' L; X. T
  18. , m2 V6 d0 h8 \1 o9 |: s
  19. float nPower;. |1 V: i' U9 \. b# t
  20. MePort lightsensor_6(6);* {) `& v( ^' i1 d3 Q5 j+ N+ S- L9 _4 T
  21. MePort lightsensor_8(8);
    3 N/ p  }2 p; g( A/ ?
  22. MeDCMotor motor_9(9);
    # C- F  X8 L- ]& g
  23. MeDCMotor motor_10(10);3 j& X# {5 {* w4 l# |# N1 ^
  24. unsigned long previousMillis = 0;
    1 d$ z" S/ G, U9 _3 q$ m: L% j
  25. const long interval = 1;
    9 c0 s- y  y# X" g1 J& x9 s/ W
  26. ! e' e! _5 n( d- h8 ~9 e
  27. void setup(){
    1 A& W2 g& q" z2 E1 B
  28.     lightsensor_6.dWrite1(1);2 m7 s7 F1 m% W: @
  29.     nPower = 160;
    6 C  x4 j1 E: R
  30.     Error=0;
    ) b' o# B) q8 x6 B
  31.     ErrorAcc=0;
    " \1 i6 O& |8 T" }
  32. }5 |/ z1 ^) t8 d

  33. # z) N% ]; ]. @5 K- j
  34. void loop(){
    0 ]: O! M+ c: z8 p& @) N
  35.   unsigned long StartTime = millis();
    $ t. b, A! U0 M8 O5 j, D/ A5 J
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    7 q0 M5 c* s) x
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();$ z" K% o9 X) O5 H: O
  38.     ErrorAcc +=  nError*Kd ;0 [5 K6 W1 }/ a9 I& D8 _
  39.     ErrorDec -=  nError*Ki ;
    ) ]. M9 i# f/ I* ~0 Z4 a; T: Y
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);5 H7 _) ]2 C! Z$ y* `' t
  41.     if(nError < 80 && nError > -80){
    + {5 l' l. \5 ~* v% {( p
  42.       if(nPower < MaxPower){
    & v" X3 ?  ]7 T( F) O0 X# @( L
  43.         nPower += 3;
    0 P( U: e, c- X6 B: Q
  44.       }
    ( Y' T/ L  }& b8 _# Y; \% U
  45.     } else{/ K0 m1 T0 M5 M/ G
  46.       if(nPower > MinPower){
    ! V( q+ Y+ p2 J
  47.         nPower -= 2;
    ! X1 _* t5 g( f
  48.       }/ H2 d8 g. a4 g- @' M5 N6 H1 \6 I) S
  49.     } + a" `2 K6 i& Z3 {1 r: @8 s1 o
  50.     MotoL(nPower-Error);
    7 b6 ^4 N/ Q$ j6 V
  51.     MotoR(nPower+Error);   
    9 N. h6 V0 l- `  S0 V* K) [, p1 k6 N9 ?
  52.   }else{6 w( m) s+ I% k; l, Y& F
  53.     motor_9.run(0);
    7 i+ y) p8 L, X1 J8 s
  54.     motor_10.run(0);
    6 N9 |- W4 D" x8 x8 M( v# t% l
  55.   }# [; s% e3 ]' Z9 G% G0 G
  56.   do{}while(millis() - StartTime < interval);
    5 |3 J7 B9 O0 r, r' {$ n) e
  57. }, d* f  d1 V* k/ m: E

  58. + F3 X! D5 S0 h- U8 W: P
  59. void MotoL(int Power){
    3 h/ d/ j! M& }1 D) R" u2 I' I
  60.   if (Power > MaxSpeed){3 j) A3 V/ L0 N1 z( V1 A
  61.      Power = MaxSpeed;: V7 [2 o# V; Z, H" F3 {" w4 P# s
  62.   }
    3 u1 B: F# i1 X! B( l
  63.   if (Power < -MaxSpeed){
    & ^) a: U, O" }0 q/ E2 ~
  64.      Power = -MaxSpeed;
    9 ]6 ]3 z, F! u( Q% }
  65.   } 6 w2 d$ \0 u# v2 Q) N' e
  66.   motor_9.run(Power);
    7 E4 K) q% {9 }% V; Y! j2 z
  67. }  
    " Y  H9 d: j+ @& Z2 }% }

  68. % H! M" T" ], ?
  69. void MotoR(int Power){  d% p. z, [3 Q& X) L3 I
  70.   if (Power > MaxSpeed){
    ) E; R" U( C. v' O+ g* S
  71.      Power = MaxSpeed;
    ( B1 g7 ~0 B- F9 D
  72.   } 2 o2 C% b! ]9 m$ v3 `- N
  73.   if (Power < -MaxSpeed){- [& C8 ]7 t" j1 e
  74.      Power = -MaxSpeed;) J  G9 s1 V6 @0 e
  75.   }
    2 W" F, [+ K/ D. ~+ M
  76.   motor_10.run(Power);
    ! p# o" z9 @+ F" ]/ f
  77. }  
複製代碼

- \0 v" Q# I5 n( P( g( `+ Y& T$ z& E3 r( S* I* w& P
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯 4 N7 b( t7 P5 u. }) _4 u5 g
  @3 Y" j- p1 l9 P' [( I4 \& t7 l
您好,不知是否能向您請教。4 z+ ^2 P+ G5 W; ~
目前和宋修賢老師在處理Ardui Car
4 \9 o6 A( ]0 g8 r' l3 H雖然已使用較繁雜的方式處理了跑出黑線外的狀況" m0 C9 [# U1 z2 p+ F2 t/ c, I
2 ]' D' h! {. ~1 C( V+ y( s
但基於想追求更精簡的程式所以還是想請問一下
7 z& F! m' E, t# b/ o6 H# |就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝6 ]+ P: ?0 X* o' K2 b; M1 Z: j: h$ h2 C
不知道您是否願意教我可以如何處理9 l; t- I" r: N

% k" a2 V" V4 |; o3 `! I- z' x2 p' q. r0 A4 M
以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;9 h: H% {  y; G
  2. double CNY70Val = 1000;9 Q% r3 w; U( {3 ]# `
  3. int interror = 0;9 ?0 [, @2 K% |
  4. int olderror = 0;
    5 a! x- S3 @! g7 e7 |9 U
  5. double values;1 M- v* J9 x/ n/ A
  6. + I7 s0 ^* E' V, C: ~8 d0 L# m

  7. 5 E; b: h2 g- o% ~6 ?, H3 G* e9 O- w
  8. void CNY70()% B/ Q8 i/ I( a
  9. {$ {$ r3 j) z) x/ t' u
  10.   valuesRR = analogRead(RR)
    0 N$ c; O+ }  x7 h- p
  11.   valuesMR = analogRead(MR);
    " M1 l; C' B0 W2 g7 ?
  12.   valuesMM = analogRead(MM);  J& Y" G- s; r3 x# M0 E1 S4 s
  13.   valuesML = analogRead(ML);
    # p' g/ ^9 q: b
  14.   valuesLL = analogRead(LL);
    2 u" C$ `- o' r0 z/ X4 k& f0 R
  15. ) j4 c2 h4 E$ O( v+ i7 L% R1 _& `
  16.   if (valuesRR > CNY70Val)3 T4 _! D& r2 x& r; v
  17.     valuesRR = CNY70Val;
    0 D" Y% T' w, W. c" T, K3 w
  18.   if (valuesMR > CNY70Val)
    4 M: L* i, v2 c' ]" W+ F+ F
  19.     valuesMR = CNY70Val;/ d6 K1 R, P  E2 t' [) ~
  20.   if (valuesMM > CNY70Val)
    2 a, C/ f& Q0 A
  21.     valuesMM = CNY70Val;# s% U" ^! m# Q; g+ M  x
  22.   if (valuesML > CNY70Val)- `$ y1 U& @, ^: j9 x. d
  23.     valuesML = CNY70Val;
    $ w6 K* [! p; Y; m
  24.   if (valuesLL > CNY70Val); ]9 D0 O( S( u
  25.     valuesLL = CNY70Val;
    - `' N6 Y  y$ l6 O; k" f

  26. 8 Y8 j; X1 H! P8 ^# \
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;9 u7 f* |8 _/ G: T5 O1 a
  28. }
    ' d. J- \5 z  {; Y; [, _& `
  29. 1 U' f8 ~3 u7 N8 _9 [
  30. void Car()
    - v8 @* T  q8 m4 k4 n9 f
  31. {
    - y) O( L" W8 S& P4 r
  32.   while (1) {
    9 w9 i9 o6 U8 Y
  33.     CNY70();" ?  n% x' ~$ j5 z/ B; @

  34. 5 D1 g( _% a) B# h
  35.     int error = ((int)values);
    ( v; B' [( s3 o( d+ K
  36.     interror += error;  U# P' P2 G5 d/ z# v3 l
  37.     int lasterror = error - olderror;, s+ o8 z, r* @1 s5 R' j
  38.     olderror = error;
    7 i6 S" n2 O$ {$ j( G
  39.     int power = error / 5 + interror / 10000 + lasterror;
    / \2 g: O- D. ~

  40. + L1 e0 _1 |4 x  v9 \, H( j3 W3 m
  41.     if (power > MotoSpeed)4 _$ d' N4 E2 g1 m; S
  42.       power = MotoSpeed;
    1 U/ j# e1 M0 L/ U9 v1 d
  43.     if (power < -MotoSpeed)1 _/ n) L0 D* m# E, f
  44.       power = -MotoSpeed;
    1 _1 P5 n- \, O6 g, g1 \9 C
  45. 2 I7 g" I' l6 y, u1 P- V. @7 p
  46.     if (power > 0)
    5 |- J5 ~( [) ^
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。
    ! Z) {! _" s( P
  48.     else  C" E3 n. w: `; \
  49.       Speed(MotoSpeed + power, MotoSpeed);' e% G* v9 C5 W/ s0 R6 c4 {
  50.   }
    7 `6 G( m  I4 |; q! w, q3 w+ i$ B$ ~
  51. }
複製代碼
4 U0 s2 x# }1 ^" ~3 j& g$ N
# V+ H. A8 M5 e0 e9 Y1 N9 }
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-11-27 03:28 , Processed in 0.027326 second(s), 17 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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