|
|
- #include <Arduino.h>! k% e- j# v# {+ n) M$ J7 S
- #include <Wire.h>* u$ C2 s9 L" T! Z% \
- #include <Servo.h>
2 m: O$ y+ F5 A; F' y9 o4 ^
& m& ~: k' N% V: G( A. Y% I0 t- #include "MePort.h"
) V3 J" b# A, D& h. V9 K& x# k - #include "MeUltrasonic.h"6 T+ U8 Q8 c T
- #include "MeDCMotor.h"8 D# H; _& `) \' i) u; L
- 1 _: M) U' o2 F& F
- //double Input, Output ;; s s6 t, J3 H( B' N
- float MaxSpeed = 255;
1 d3 |0 P% ~ J# w - float MaxPower = 180;
! H( C% W: a2 v - float MinPower = 120;
6 ?( C7 ]; [8 [2 q - float Error,ErrorAcc,ErrorDec;
; ~! G# B- |- L4 Q& @ - ( o. f+ d: }; |, q
- float Kp=0.14;/ K7 ~/ i; _: [3 v9 m$ ]$ ~
- float Kd=0.00020;//23;
+ `% l! r3 Z1 i, l5 i, h - float Ki=0.000201;/ e! J6 S; }. l9 a9 P# E/ E3 H9 e
2 [- g! Q- `! y9 m, w: [# f' t- float nPower;
! v9 k0 ~1 l# ?. O0 c5 T - MePort lightsensor_6(6);
8 X3 Q7 k) ~7 P; i% i - MePort lightsensor_8(8);; b4 }; P9 t# U7 f/ ]4 n
- MeDCMotor motor_9(9);
& J3 ]* x5 B3 B0 n5 I, R9 x: h7 O+ [ - MeDCMotor motor_10(10);
/ j* u/ S( p" j2 [ - unsigned long previousMillis = 0;, U" j* J7 R; I3 n# r
- const long interval = 1;
, K7 q2 M7 N% Y6 |: B2 {- ?& C
+ D" b% x1 }3 i4 ~( z Z7 ^$ j- z- void setup(){( N3 S" U2 h5 h+ r# C) u: W
- lightsensor_6.dWrite1(1);8 g2 U+ B) B% O4 Y6 h, ~- C
- nPower = 160;
% }7 l9 X8 E' h& ]) Q, W4 Z - Error=0;2 C- D! r& c+ D* ?1 q8 @- k* C
- ErrorAcc=0;
2 g9 a. I1 G! O0 _ - }
$ `& e- Z& u U7 }8 ~' `' d- N - ; d0 d, m' O* H0 d: W
- void loop(){5 @0 C% B7 q; p: z6 W
- unsigned long StartTime = millis();; [8 m5 z& ~; P
- if(ErrorAcc < 18000 && ErrorAcc > -18000){
- C, l0 b: Y& Y0 d - float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
5 q* B1 H( ]) B: E7 e8 \- e- x - ErrorAcc += nError*Kd ;! Z! R/ t4 C( V" S2 y" r: M
- ErrorDec -= nError*Ki ;
" N+ N% a6 k# V" a+ a - Error = int(nError*Kp+ErrorAcc+ErrorDec);! c6 _* F( w: @1 I% W. w
- if(nError < 80 && nError > -80){' q* c% R' {/ i3 F+ h
- if(nPower < MaxPower){
! k8 f- U; a* c5 w" U - nPower += 3;
: p6 v$ `5 o( X+ c2 }* V - }
; T' Z+ M" `# p7 t( `" k - } else{
. c1 T/ \" b: L. e2 ]) `2 B, q) v - if(nPower > MinPower){
2 ^7 D3 ?- G' s5 p2 [' |& ? - nPower -= 2;
+ L: d! v/ Z; i2 f/ ^ - }9 m; D W/ x% B$ n: {6 T2 ~
- } : \8 M7 k" Q u: R- d
- MotoL(nPower-Error);
+ y& A* a6 {) @" a; L/ J - MotoR(nPower+Error);
% x4 m1 Q6 a6 @ - }else{
" k1 X# U2 I. u" M3 v4 o" T" S - motor_9.run(0);* v, m) I# [* Y9 @! I( u! [4 E
- motor_10.run(0);
5 u' X- B5 n4 D2 |3 p - }- v6 I6 z) n/ A- m: q3 I& T
- do{}while(millis() - StartTime < interval);; D' {& W1 X+ K% }7 p/ f
- }; Q1 O- |: ]" B+ h0 Q2 Q( b* T" H
- ; w- f; n; p: n! m: M: X: a
- void MotoL(int Power){3 C8 ]* X+ v0 L$ p2 T* Y
- if (Power > MaxSpeed){6 [, _" }! ]2 W4 O
- Power = MaxSpeed;
' F3 j- i4 F, r6 A/ k/ ~ - }
! p8 i3 n' @5 Y# X - if (Power < -MaxSpeed){9 J4 z9 t4 O/ C4 w
- Power = -MaxSpeed;
- b# B" a! B+ u, Q4 f+ h0 c3 m( Y3 D - }
; z# k1 Y0 V: W) H4 ? y/ a5 G* Z - motor_9.run(Power);- z9 g2 s Q% Q% e0 C1 c
- }
4 M& T+ ?& `0 H# k) k
0 Q. y# y+ c/ G; @# Z( Q- void MotoR(int Power){4 D3 R! q- _8 g; f. M: o7 M
- if (Power > MaxSpeed){
# H. D4 ` Q, N) K# m6 ?5 t - Power = MaxSpeed;6 |) x$ H9 C7 W" `; |+ N" G+ Q3 @
- }
% F4 @1 @0 I' W" i" C3 G* e) U - if (Power < -MaxSpeed){
3 G8 O7 e y! p. { - Power = -MaxSpeed;1 F- M, t, ]6 y' K+ n' o% {! [- p
- }
$ |6 f! [! Q+ J: V - motor_10.run(Power);- |! E: |" V* U% X7 ?1 U
- }
複製代碼
3 N4 T; H/ F2 g# y* s; z9 J& r) i; w7 l" }2 g
|
|