#include <Arduino.h>. [& g( p4 U( C
#include <Wire.h>8 J4 J* A' A l
#include <Servo.h># F. p) H8 D# g. {
: z2 y4 Y& E0 ^3 V: S1 p #include "MePort.h"
! `0 k, c7 G" P' S4 K8 ^3 B #include "MeUltrasonic.h"
; Y9 P% l; c1 q9 U2 L1 ^ #include "MeDCMotor.h"
# p, F. V2 ]" B7 Y) a" |# P2 f$ A 4 m& h O% D2 B! V% K9 g7 s
//double Input, Output ;6 N. O: V1 D9 O9 Y! z& V( ~1 q% e
float MaxSpeed = 255;2 a, y* y# _) `# A/ ? I
float MaxPower = 180;
" H, l4 j2 w6 M2 x U- b2 \+ y7 |" Q1 \0 F float MinPower = 120;2 A2 D( J! u! W n0 F* d9 A
float Error,ErrorAcc,ErrorDec;1 A E6 @6 c" d1 C8 q2 K2 G1 `1 n
2 d( g0 S9 O- h6 P ^& @6 x
float Kp=0.14;
, P W8 A7 H* g) G' ]2 K* Q7 | float Kd=0.00020;//23;
9 G$ N3 C, ]( g2 B4 ~# T float Ki=0.000201;7 z- k. O5 r1 W3 S' r5 V
4 M5 B8 o+ \9 P% E float nPower;
6 Y) n% k" U5 V" ~ MePort lightsensor_6(6);
0 C6 U1 V+ e/ @" Y- ~: [ MePort lightsensor_8(8);
9 a* N: l* E! O4 F n' W" O! s( w MeDCMotor motor_9(9);
9 `$ x R* B! a! Z+ A9 k MeDCMotor motor_10(10);
6 A$ [' ^/ y8 R; {( v# `: O unsigned long previousMillis = 0;
8 c* C" d- g2 b4 w const long interval = 1;+ \1 V0 n, X! `# ~0 ?
8 Y7 {5 j7 |: t K, k+ z void setup(){: \9 X0 l0 P$ G3 F# L4 i
lightsensor_6.dWrite1(1);
" w7 D/ |# t5 i9 d nPower = 160;
6 e6 w7 `! ? [$ L Error=0;) {9 ~% k' n: R; ]; U% w
ErrorAcc=0;; l2 R% M2 v S8 j/ q" o
}
) \' e( g: F( P9 b2 E; q( w8 L % B: w: r n9 s5 y3 O8 ^
void loop(){
) z. M* \* w$ M$ v5 k unsigned long StartTime = millis();
1 q0 L$ {% Z4 w5 _3 g; v if(ErrorAcc < 18000 && ErrorAcc > -18000){
5 q# z' w `9 e5 ] float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();6 \2 r8 @: m- K
ErrorAcc += nError*Kd ;
# X+ C, Q) d3 _ ErrorDec -= nError*Ki ;6 Q9 O C) i d+ _( i
Error = int(nError*Kp+ErrorAcc+ErrorDec);
! j" I! i+ J1 t' n" F if(nError < 80 && nError > -80){
" T! R6 O' U4 u9 {4 m1 ?5 W( s/ u if(nPower < MaxPower){
8 @! O+ J# T7 u! y5 ~4 p nPower += 3;
% c5 A& K. Z! ]) h. a+ q/ T }/ \; t! x+ w% G9 f7 c! }
} else{
8 s% B, U- A. p7 i* g- |7 ~ if(nPower > MinPower){ _' f' A* \, H/ g7 `# A. H# K
nPower -= 2;
% `# M( m9 D/ m4 i4 x; i1 k }6 J' y ^/ [0 f: H$ j" K
}
3 j, i2 D, Z& @% S4 H" { MotoL(nPower-Error);
) D5 z7 n1 E* Z) T" _+ t+ K: ] MotoR(nPower+Error);
, K) m' k! N+ D9 ?$ H }else{
9 {9 `& ~( ~+ S \8 z motor_9.run(0);9 N( H+ C8 X4 M, U- b5 ^
motor_10.run(0);! l3 J1 j/ \* M( ]1 ~
}& u" I; F- g" H6 `: U
do{}while(millis() - StartTime < interval);
$ {: v- P% R' S$ Y }
1 K% @" _3 v# |- j( O* Z 1 v9 ]) q4 X+ d8 _! u3 Z' s
void MotoL(int Power){: ~( B/ m( T/ {
if (Power > MaxSpeed){. m. V$ a6 h5 b+ I8 b r* A' g
Power = MaxSpeed;
8 o& o$ Y u% a# Z6 R+ F. Q } ' J3 C# P2 N6 E' X) n4 n, \
if (Power < -MaxSpeed){( m8 ~$ ^7 t9 C/ y" J4 c
Power = -MaxSpeed;1 @' o; l. A5 L, }" ^6 ?7 ^& l
} 4 B0 F8 F2 P0 m6 x. B) b
motor_9.run(Power);
. u8 P5 S% q3 c+ F; |: Y5 t } 2 W. e" l9 Y k$ ~
" Z) w% H+ C( C) K8 L; c( x
void MotoR(int Power){
8 O3 n( \! T. Z: P P/ k if (Power > MaxSpeed){
8 A3 o: E" J" c9 L" H8 o* k Power = MaxSpeed;% V7 F# w8 p7 v
}
8 [( l5 Y7 l; |8 @5 A: z if (Power < -MaxSpeed){. ~% u4 B( r7 D6 F& f# T
Power = -MaxSpeed;0 h3 i( |8 P! @( Z5 b# g$ m$ d) \
} & s; O, u3 i7 ?
motor_10.run(Power);
' g7 m' |. b7 ` } 複製代碼 " r- G# k- i3 y! h( m
1 {+ ^3 i8 S0 Y" H+ s% A