圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21730|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>7 {5 ]/ L+ M) ^! j
  2. #include <Wire.h>
    8 y+ r2 U- F- y! a, c+ E( @8 G
  3. #include <Servo.h>" g( v# t4 y/ P4 Y# K, K: y# v, t7 m

  4. # t/ x1 |1 y7 L$ r$ n" p
  5. #include "MePort.h"
    + X- M, ], p7 `) K0 @: q
  6. #include "MeUltrasonic.h"
    4 T7 Q& X' a: E) ~% n
  7. #include "MeDCMotor.h"  A# g: t0 _+ L8 i: k

  8. 8 S0 h& b1 X9 K1 z
  9. //double Input, Output ;* s. P+ o5 V2 l
  10. float MaxSpeed = 255;
    " @  F6 M9 V* o3 G2 Z7 D
  11. float MaxPower = 180;
    . P! N9 ?) r5 j: e3 ^( \4 d
  12. float MinPower = 120;2 C* c- m: E0 m! d, f
  13. float Error,ErrorAcc,ErrorDec;  ~$ Q1 X  f. j7 z: z
  14. 2 o% e& E  N; {% W0 t2 n
  15. float Kp=0.14;
    3 T4 \( m- s+ G
  16. float Kd=0.00020;//23;
    2 H0 ~5 _" o2 C* Z% j% m+ p( D
  17. float Ki=0.000201;9 y" g- Y2 e4 f# R  a1 a2 m) u

  18. - D. _- x' s* g
  19. float nPower;
    0 w/ R# C9 w; [" q) i" B" ^
  20. MePort lightsensor_6(6);, d/ [( G6 a) Q, r
  21. MePort lightsensor_8(8);! b# b3 Q- D: J
  22. MeDCMotor motor_9(9);+ D4 K( D% G  z) M4 }+ h% [
  23. MeDCMotor motor_10(10);. A* }" g3 e" [; \; A6 @
  24. unsigned long previousMillis = 0;
    + J- ?3 w* V  n8 r
  25. const long interval = 1;
    ; {  h& z2 x6 o! {# B
  26.   J  L" \$ N4 Y4 J! z6 j
  27. void setup(){/ M: G7 D/ M3 p* v2 G
  28.     lightsensor_6.dWrite1(1);
    : n) S/ {# T" q/ m7 y; W
  29.     nPower = 160;) `+ W: k, V) T7 a7 |
  30.     Error=0;0 x6 `& C" f: ?3 M
  31.     ErrorAcc=0;: b8 I- s. G3 A! @4 {
  32. }
    2 F/ Q; t( E  v5 j8 E% r6 r, s7 D

  33. 2 L' ?' P8 i7 L; q9 h( M4 t6 x
  34. void loop(){
    ; l1 J2 m6 q" P
  35.   unsigned long StartTime = millis();
    + n6 z7 ?/ r4 T) M- w5 H
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){* r# F$ Q2 Y4 j5 f' ^2 t7 {
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    3 A7 V% K* V& ~- J) m
  38.     ErrorAcc +=  nError*Kd ;
    2 e! `' K; g# C8 P6 Q* A
  39.     ErrorDec -=  nError*Ki ;" `$ K. i: C- a. b+ \) N6 Q
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    7 B/ K$ L5 l6 A) s6 N
  41.     if(nError < 80 && nError > -80){
    * N% |+ T0 r1 ?
  42.       if(nPower < MaxPower){
    / f" G2 d+ Y0 k8 A9 Z
  43.         nPower += 3;! r8 o6 r$ F" B0 U. K: ?0 c
  44.       }4 r8 `8 f8 |2 i' n* l) o
  45.     } else{
    . j/ k8 N0 ?+ H  A) x
  46.       if(nPower > MinPower){
    / M! N  U$ l* c9 p" q5 f
  47.         nPower -= 2;; s' p: c* F* t5 Q
  48.       }
    " C5 V6 i! U6 Q" r  w# N* v
  49.     }
    0 i4 {! o0 s6 e% U; ~& k
  50.     MotoL(nPower-Error);
    ( I: K3 c2 z2 t3 Z
  51.     MotoR(nPower+Error);    * @3 q0 S0 G' T$ \" v& F2 n. w
  52.   }else{
    ; F% y9 y: E2 G+ `. C: t$ _$ ^
  53.     motor_9.run(0);
    & r6 D  e6 C- r" j% Y4 {
  54.     motor_10.run(0);4 M4 p4 N; e- k
  55.   }$ u, W$ `& s5 Y$ A( u: p. a9 u
  56.   do{}while(millis() - StartTime < interval);: [. A' g$ t# c, Y+ y
  57. }; W2 B$ o) [8 i4 F9 Z

  58. : u- K& X1 Y1 T! V
  59. void MotoL(int Power){5 a  ^9 a; v( W' d$ A0 b: v
  60.   if (Power > MaxSpeed){) e9 B  t" F0 x1 K( G3 l
  61.      Power = MaxSpeed;- ~8 Q2 O0 N. G
  62.   } 1 T6 n" T+ U7 _0 _5 |
  63.   if (Power < -MaxSpeed){- B3 H! X9 a' k# ]1 B4 ?
  64.      Power = -MaxSpeed;
    5 s. t7 ^. \7 U+ Z; _
  65.   }
    + S" ^+ e% ]) S+ v2 K( |& n9 {
  66.   motor_9.run(Power);: [1 e$ o9 H8 O( ^. d% }' r+ J
  67. }  9 v4 N5 L4 B4 \! F5 B8 O: g

  68. 9 n* d4 [& M2 Z
  69. void MotoR(int Power){/ c  h0 ]" [5 _! Q# Q
  70.   if (Power > MaxSpeed){
    # M/ i0 K# \7 o% i5 W. k
  71.      Power = MaxSpeed;
    # R0 k' s# ]+ I9 w' k7 ^
  72.   } ! @9 b+ U, O3 g4 v, s' [- i  E
  73.   if (Power < -MaxSpeed){& e4 A; P- e  f! i
  74.      Power = -MaxSpeed;& L+ N& }" o6 }3 b6 Y
  75.   }
    - |6 F# R2 J+ u7 E2 g$ b# G- V/ r
  76.   motor_10.run(Power);
    2 J7 b1 }  E# K/ h
  77. }  
複製代碼

, C# i0 x) [! z9 ~& }! |' O; I. t: ~( o
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯
* q: Y. L4 Y7 W0 f7 T: u2 L  U% x- S
您好,不知是否能向您請教。
# P' k/ C5 O$ S$ o# n目前和宋修賢老師在處理Ardui Car
7 G5 D1 x8 H) [' R, R雖然已使用較繁雜的方式處理了跑出黑線外的狀況6 x! J& b2 }; M3 Y9 b* I
/ M9 n' X$ v0 p1 ~% B. w
但基於想追求更精簡的程式所以還是想請問一下
6 w5 C. U; a  u; d& H, v就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝7 d' ?) J7 I, n/ l+ Y
不知道您是否願意教我可以如何處理
( c  D. h6 M; b% R* E5 w: f( \: W* U/ I: W: I; G1 ^
4 r. U- `( m& Q$ u' i; J
以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;
      P# b7 s& S( s3 k
  2. double CNY70Val = 1000;  ~7 E8 z7 s! F: _% \  k* a
  3. int interror = 0;
    + B6 o: ]7 j+ C# M! z$ M
  4. int olderror = 0;
    5 C# Y% b* \6 j$ @6 _6 v' d! _' q  p
  5. double values;
    8 }4 D+ X) e2 y1 k, I

  6. + x3 ^, R5 ?, ^( T( Q; z) R
  7. * W8 C5 ~/ E; n
  8. void CNY70()
    1 F& ~( h" ?7 B+ [
  9. {
      z; f- ~- d. j+ \1 d  |6 X
  10.   valuesRR = analogRead(RR)
    ' g- e9 \1 \9 J0 d9 W
  11.   valuesMR = analogRead(MR);0 J) j# A9 M; T' f( p
  12.   valuesMM = analogRead(MM);
    * p8 w: K, R# O1 J# o7 C) }
  13.   valuesML = analogRead(ML);
    " v) @3 E& d' N& H
  14.   valuesLL = analogRead(LL);4 {3 y: p% N/ I
  15. / I! T/ E4 |$ V. _9 T0 }
  16.   if (valuesRR > CNY70Val)
    . y6 L" k7 P# m" i# `
  17.     valuesRR = CNY70Val;
    ' u2 O- g2 E5 w! m' z$ }# k5 M
  18.   if (valuesMR > CNY70Val)7 P" C8 r, d5 Q4 {4 c. b: @/ ~
  19.     valuesMR = CNY70Val;
    . }6 G: J, K, [' a& N+ k( U
  20.   if (valuesMM > CNY70Val)
    8 L  i8 b" o. _% l& K  H
  21.     valuesMM = CNY70Val;
    * X" ^4 k! n7 d: l: y
  22.   if (valuesML > CNY70Val)% H) ~( y" Q4 ]" C
  23.     valuesML = CNY70Val;" s7 C% i$ {( E  |! I, g
  24.   if (valuesLL > CNY70Val)( @6 ~5 z3 f5 ~; f
  25.     valuesLL = CNY70Val;
    * o$ x% L* g5 L- u  v
  26. " |  y9 i' w3 j1 ?1 Y1 e2 B8 k
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    ; G) X, t" U4 K" w
  28. }
      a/ O0 p4 |" c/ m- l
  29. : M( ]4 y1 ]* x& G
  30. void Car()
    # C9 Y0 O) f  R
  31. {; r2 H/ Y# S6 ?, c: ^: y
  32.   while (1) {
    ) G, V4 c/ n/ r2 a' u8 O
  33.     CNY70();* ?+ R. f: p; Z! g6 F
  34. 1 {4 E9 I7 `9 N9 K/ c% {2 T
  35.     int error = ((int)values);
    5 U$ |' r5 d* ~8 U
  36.     interror += error;6 Q4 {* J  Q5 F/ I7 I
  37.     int lasterror = error - olderror;* \4 V6 Z7 d4 @
  38.     olderror = error;
    7 b1 Y- C# S& }4 X8 p% D
  39.     int power = error / 5 + interror / 10000 + lasterror;6 u* _  O- ~' a
  40. : X+ x! a9 e7 j* |
  41.     if (power > MotoSpeed), {% g* |8 |- A8 E* C# A
  42.       power = MotoSpeed;& v( s6 l  {; V* ]
  43.     if (power < -MotoSpeed)) R0 E. X; s9 U  P8 w1 G" [
  44.       power = -MotoSpeed;
    % E' ]: M+ Z. J  T0 v

  45. , ?6 J1 O5 X& X! y. r
  46.     if (power > 0)4 K1 @2 U1 O- O9 t, T% k
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。# J/ L5 @( S7 @* H! `
  48.     else
    . u) t# i0 x4 v
  49.       Speed(MotoSpeed + power, MotoSpeed);# ?' B3 F# F- S. ~' |% `
  50.   }
    9 S) L) [: H3 ~% s8 y& {
  51. }
複製代碼
( e' g7 A% ^% S& P9 B/ N& P7 s1 R/ z

1 h7 ~" z# M9 o# }5 @$ n+ r2 X, A3 c, `* ^
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

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

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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