|
- #include <Arduino.h>
( Q y) Q6 x0 r* s - #include <Wire.h>$ K5 O. z4 c- Q3 _! x0 C: d% Q3 K8 B
- #include <Servo.h>
2 p3 V9 C: f8 L$ T
/ K/ y7 X( [* a: Q6 k- #include "MePort.h"! z8 k! t5 q4 Y8 _, a
- #include "MeUltrasonic.h"" s2 d+ e* k" ]% h: E
- #include "MeDCMotor.h"
' k+ p/ d6 g, E& J9 f; \& J- R# i" f' M) r
& }% I) V+ p2 I4 V: o; I- //double Input, Output ;
% P; N5 V8 b1 S- X( U - float MaxSpeed = 255;
r3 {) C x+ v2 H8 J" b - float MaxPower = 180;
0 M& y& d; |$ \$ ^ - float MinPower = 120;
. X' ?( h2 Y4 x5 _; R, L - float Error,ErrorAcc,ErrorDec;* G; n/ G+ K- \
- : v/ E" \3 n+ L' Z+ z
- float Kp=0.14;$ y8 Y7 i5 L. J
- float Kd=0.00020;//23;
' X) J8 W! c/ `$ O" K+ [0 Y - float Ki=0.000201;
/ I8 u: I9 v+ ~8 z3 t0 `, p$ z4 Q - ! m1 }. U: O, I9 \$ A; f& T
- float nPower;
- t) L7 b, }% ?) ]2 s - MePort lightsensor_6(6);; P, ?9 V4 y2 z- p. p' }; I. u/ O& f
- MePort lightsensor_8(8);
1 b$ y) v$ ~ H - MeDCMotor motor_9(9);
$ f" p1 x5 p# T, u% S$ p - MeDCMotor motor_10(10);
, p* P+ K) n, f9 a5 O - unsigned long previousMillis = 0;
+ u, ~7 Y f4 v# _; g" X - const long interval = 1;- h3 a* a7 V4 x- G, R5 ]* ~
- # N( V6 x( Z) F$ W# |' b. G
- void setup(){" e. ^/ F3 S0 h$ h# _
- lightsensor_6.dWrite1(1);; `- d$ s. t# l3 n, n
- nPower = 160;
$ n8 P* n* o: x! x: O( G8 ?# w3 h - Error=0;
$ Q0 S' E6 G) O n: [- m+ s! h - ErrorAcc=0;+ Q" V. H* n' b
- }
6 R! z/ j& G) M) Y5 R0 e - + b$ s/ x$ F3 Z3 ]
- void loop(){
. f) E4 d. v4 T3 f6 S - unsigned long StartTime = millis();
0 T8 B) D5 f' |, i' h( q2 w4 g5 H - if(ErrorAcc < 18000 && ErrorAcc > -18000){
5 \8 @5 G3 J2 U6 ]4 g- {5 Y - float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();$ {9 x$ [$ j9 {" m" W, |
- ErrorAcc += nError*Kd ;) H+ G, Z; M9 O% u2 W
- ErrorDec -= nError*Ki ;
3 S7 l" h+ `3 ^ b/ q - Error = int(nError*Kp+ErrorAcc+ErrorDec);" d' D, R; P- J0 ]$ g& O+ @/ n
- if(nError < 80 && nError > -80){8 W8 R. J' P S/ y @ `; z) V( F
- if(nPower < MaxPower){
0 s( N2 m3 W1 _" q Y - nPower += 3;1 j, x9 [2 B5 A" K: ~
- }9 d; B+ Q* M O. K) ^+ D) l1 h. y. n
- } else{
' l# F! D( Z' j4 I - if(nPower > MinPower){
2 U: _0 z. {1 N" d - nPower -= 2;
5 e; c+ O5 ]4 C& w3 ?5 ` - } |) [" _! w X6 z
- } & x0 n" n8 T3 Z& ~
- MotoL(nPower-Error);
4 [+ Z7 |) O0 ?3 F - MotoR(nPower+Error); ! i, t7 `8 `+ N, s; c
- }else{
2 Q4 h0 k# u1 i1 y( O - motor_9.run(0);
$ Z$ Z1 D9 d( ^) u* E. l - motor_10.run(0);
5 n' b) n; t( s+ `" ? - }& \4 c. {: U5 _$ X% }
- do{}while(millis() - StartTime < interval);
6 r6 o% C/ d; J: _) Y% Z7 ` - }. f( N+ @# t- V7 C- A) E
: L- ^5 G( ] _" H3 `' T Q9 {- void MotoL(int Power){
+ I3 v: w0 x6 p! g - if (Power > MaxSpeed){
9 F6 M9 M, ^' a$ Q X' A - Power = MaxSpeed;- n% p: K9 m5 d- i+ W/ m+ I
- }
* Y' z- o4 s, h5 R/ n8 i - if (Power < -MaxSpeed){
# Z3 S1 I. N) N. H% ~ - Power = -MaxSpeed;8 ]; I2 F7 e3 J0 P1 j
- }
0 S& O; ]' L0 o) c+ q - motor_9.run(Power);
) A- b0 i( o# ^4 _, I - }
# l7 Y7 f, t8 a" r) Y) l( M
7 y5 U1 l* s2 ]- {- void MotoR(int Power){, |- M; Z+ N, V; Q
- if (Power > MaxSpeed){% s; z' b9 d# ^( N9 x0 O! s
- Power = MaxSpeed;$ c) I: S, X: H6 m, @" K5 q: d+ D% ?
- }
s8 ]* _, f7 D - if (Power < -MaxSpeed){
* t) Z. O* W a - Power = -MaxSpeed;
8 [+ \( c8 D# h, [; j - }
7 c: C c! R* u9 }, Q+ ?( r( L' W - motor_10.run(Power);5 `$ C0 G4 \: n
- }
複製代碼
$ R5 |9 T2 K+ V& G
- A1 U1 W" g/ y5 u' h( \% V0 V |
|