|
|
- #include <Arduino.h>- {/ F5 H3 y0 u$ r
- #include <Wire.h> n% g }& E8 P, U/ }
- #include <Servo.h>5 F2 S# Y8 I! `" i4 W* T* Q
- / n2 g8 R+ }; y% S
- #include "MePort.h"
: |" `9 b9 M2 z+ p9 x - #include "MeUltrasonic.h"5 k1 m2 c1 M: X+ V' W$ ?
- #include "MeDCMotor.h"
: {0 \% [1 d$ z. D' [# ^# a( o - $ i }2 K) v' W: N
- //double Input, Output ;
" ~% R* i0 t' T" F - float MaxSpeed = 255;" Z O/ x7 X( H4 }# A8 T
- float MaxPower = 180;
. x* P. Z+ U! i8 Y - float MinPower = 120;& F, X* B z: E4 A1 y0 O! ]9 X
- float Error,ErrorAcc,ErrorDec;% e9 k7 D5 ?5 F/ v& e
- * i/ A4 t k4 f# t% W
- float Kp=0.14;
5 X- m/ X l* _/ A: K - float Kd=0.00020;//23;
3 g+ A7 Y( m6 a. `; h2 n - float Ki=0.000201;
B0 f2 U1 I& Z1 X V E) U* w0 ^ - ) G5 u" T. \% G
- float nPower;
?. G& d* f$ |& K, v& L. ^ - MePort lightsensor_6(6);
( ^) \# E" ~6 y9 s# J - MePort lightsensor_8(8);( U; G; Q" s- p8 V! V: \+ \' s! |
- MeDCMotor motor_9(9);
- M9 z+ ?, ?- y8 x! w) q - MeDCMotor motor_10(10);1 X* J% T& q/ T4 i
- unsigned long previousMillis = 0;9 D8 ^# O( ]0 \& p) O" V
- const long interval = 1;
+ e: `' y# A, d- ~0 } N- O2 x- O) j - 6 g9 D4 `5 O1 N( h; d
- void setup(){- L' ~3 A6 s8 d% m. i, T$ ^
- lightsensor_6.dWrite1(1);
R6 ^, ]2 a6 H - nPower = 160;5 v; |7 K) d% Q2 y1 ?
- Error=0;
5 V& ?) z: ]0 r - ErrorAcc=0;: y, C% q9 t4 O' O) y6 l7 S
- }& ?$ q' ]8 S( L) H& U- E |. m
- ' F! m4 v x; w9 h& \0 e+ A4 C
- void loop(){, w0 N1 k& c0 e* H/ Q
- unsigned long StartTime = millis();$ k; Y f# j6 J' E" d. k% [, O6 s
- if(ErrorAcc < 18000 && ErrorAcc > -18000){4 F5 }! l8 b1 H0 O
- float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();; o2 }# c7 R( n
- ErrorAcc += nError*Kd ;
/ b8 y! f3 H# X+ Y) N: A - ErrorDec -= nError*Ki ;5 _- [* ?5 ~+ r% T) B5 w- U
- Error = int(nError*Kp+ErrorAcc+ErrorDec);
. ~. h% j1 J7 b" F" }: | - if(nError < 80 && nError > -80){1 K5 B$ a- w7 {* W4 W' e
- if(nPower < MaxPower){# Q3 s! C8 E/ e5 H/ W; _8 o
- nPower += 3;
$ {+ N5 m G& |- D$ p - }! y! K4 I" |; ]# L) ^3 l
- } else{
: F2 p- z. ?8 A3 {1 j% c! K - if(nPower > MinPower){
) E3 J9 ~2 n" Z# J - nPower -= 2;
% e( E* {$ ~( s! n, Q% O. i6 l; X - }8 t% S0 ~, A6 T2 w0 {- A) P& _; B( Q! J
- } 8 G0 k6 f4 h) A8 O8 X2 C
- MotoL(nPower-Error);- g$ w" k& N% j0 s6 M5 U
- MotoR(nPower+Error); 6 w' j+ t( o8 j7 Z5 a
- }else{
( W, m b6 _# f# D. G - motor_9.run(0);
( H/ y; J& w/ S - motor_10.run(0);
# z' Q/ T8 _8 W - }
6 i0 J/ P: f2 s p; \ - do{}while(millis() - StartTime < interval);
2 f1 l2 K4 O8 n& }. H8 y4 G - }9 l* J" O& W& P& q4 H
- 3 g: E- n; s1 L B* `
- void MotoL(int Power){8 d: p; Q. \" I" }* R- q
- if (Power > MaxSpeed){
* F5 W" d! p. l t0 T - Power = MaxSpeed;
6 [+ z, i# @7 ^- y7 e - }
1 w+ O5 \ N- F6 M0 q' q - if (Power < -MaxSpeed){
7 ^, F9 I; h* x4 O5 w4 P - Power = -MaxSpeed;
# s0 v* [! m+ X. @9 \0 E - }
! Y q: I5 D& z! c! x/ W" H% c - motor_9.run(Power);
0 L3 I7 B# I5 a7 w6 l - }
. x" v, Z2 i$ w8 g* x. m1 W6 z - $ n& Z* h" m/ |/ [. o& L+ ?
- void MotoR(int Power){
& N1 {, j6 T" |- h' V5 E* p - if (Power > MaxSpeed){
) ]' W( y0 m" V7 j5 a9 ^1 t - Power = MaxSpeed;
. g; u+ o6 ^8 P+ |) D2 h! I5 o H - } ( D' Q0 I7 C+ e6 Z; |1 ?; ], l
- if (Power < -MaxSpeed){; M% ^2 i; t7 ?/ W6 m Q
- Power = -MaxSpeed;
" A) k3 D% x. `, W0 K* X5 Q+ {% R - }
% s* f( m* V- P5 n* i - motor_10.run(Power);
) e' o& D) I0 c/ B ~* @7 Q2 z- f - }
複製代碼
$ B8 }* n ~9 r$ @1 v3 G6 K
3 w4 v: g# O! L6 T3 L |
|