|
|
- #include <Arduino.h>3 J8 a G5 N5 l% E7 w, o" D
- #include <Wire.h>4 T0 s7 M$ k4 E# l
- #include <Servo.h>
, Q# i. m% v$ E8 p - # ~1 _" T/ R" S
- #include "MePort.h"
1 t9 C; }6 M- K - #include "MeUltrasonic.h"
& m; M: u7 L) i7 f& p% u0 t. ? - #include "MeDCMotor.h"
' A+ d8 e+ `1 z, c% O
5 Q: N7 e8 `) ]8 s" N- //double Input, Output ;5 b3 @/ s( `8 J- R/ h5 j
- float MaxSpeed = 255;5 D* ?2 A. h/ d) x3 ?4 i
- float MaxPower = 180;
4 p5 w! a7 T7 v: s M7 y - float MinPower = 120;& y4 P( f7 i1 Z0 D
- float Error,ErrorAcc,ErrorDec;) b- S4 E% m* ^1 z2 g& M
' R- ^2 h% f: b& P7 A- float Kp=0.14;
0 ?, `2 i9 N* r- C - float Kd=0.00020;//23;
! p( ^4 s* S/ r1 i - float Ki=0.000201;, U2 p3 D# x; H4 h
( \, g- p7 }# I. U- float nPower;2 W: K0 ^. Z: c7 @
- MePort lightsensor_6(6);# ?2 [2 T4 C# J* P* a3 a: J
- MePort lightsensor_8(8);
0 _+ n0 {" n T) a% M - MeDCMotor motor_9(9);
3 e7 D" C- x& t+ V. N: l+ V5 B- r) H - MeDCMotor motor_10(10);6 P5 V/ }7 m2 E3 m' w/ h
- unsigned long previousMillis = 0;. E' z) B0 \; s$ _3 l
- const long interval = 1;
" m" S/ U# K8 O1 B1 x; c T - + D8 t) r9 u7 O3 R
- void setup(){
, `$ ^. p: g, b+ S - lightsensor_6.dWrite1(1);! e4 v. n& }5 E3 g: M
- nPower = 160;5 n: k/ t- V5 i }; c
- Error=0;$ q; E% ^8 e, u) _1 R, ~: [! r
- ErrorAcc=0;
/ [4 I4 K7 f- P" l* x1 ` - }3 |4 M* T$ b% A, ^
/ e& }) v B& t" v3 r- void loop(){" L9 A E8 a( l# }" \. o% x+ I
- unsigned long StartTime = millis();
' g$ p2 s2 T5 t+ y2 ] - if(ErrorAcc < 18000 && ErrorAcc > -18000){
) Z& W) z0 }% s# Y9 J J - float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
5 Z5 T" D1 Y: v" \ - ErrorAcc += nError*Kd ;9 j4 ]- n8 }0 l
- ErrorDec -= nError*Ki ;
1 D, i' m8 Z- C# U" v - Error = int(nError*Kp+ErrorAcc+ErrorDec);
9 z+ h* ^7 e1 R; E5 f0 S( J - if(nError < 80 && nError > -80){
; F$ z5 C2 ?9 K2 d" W5 T - if(nPower < MaxPower){# M& I3 ^& Y' Q$ O
- nPower += 3;! ]0 B% P9 M% q: C2 s3 J; ?) T
- }
' ]# ?3 t) s6 F9 n4 g* s" ^ - } else{
6 e9 Q( F8 z4 |+ T# j - if(nPower > MinPower){
* {6 h9 s0 {8 w' R' ? - nPower -= 2;
5 U# Z5 ?* ]* `5 s+ E) [ - }
* s( j# q0 A, c- M4 O* Y( P3 O - } 7 k. }! d& ]1 _% o+ I
- MotoL(nPower-Error);0 Q$ ^" Q+ x6 Q6 p# e' U
- MotoR(nPower+Error); 4 V* x6 V! ]1 o+ G5 g$ ?
- }else{
/ H+ U! @$ \9 F4 t0 N - motor_9.run(0);
' Z2 d# o7 f2 j& ]2 F# r* { - motor_10.run(0);, _1 A# Y; [/ u' J: ^0 h- q j
- }) E& F6 c3 y' d$ b/ h6 w2 q
- do{}while(millis() - StartTime < interval);- P4 l9 S; t1 G. ^ B
- }" D2 Q5 f3 P9 d+ k% U
- 8 P, D7 [/ v* b& _) {/ ~
- void MotoL(int Power){; ]$ ]7 d: c/ W2 I2 o
- if (Power > MaxSpeed){; y) G/ p3 n, ]$ O) L
- Power = MaxSpeed;
$ ?( c/ a5 W4 p: s - }
. f. F( d' x# ?/ }! _3 a4 L - if (Power < -MaxSpeed){: C- }4 B5 f! \/ ?. l/ E
- Power = -MaxSpeed;
4 _1 O% l7 j6 @$ H7 O - } " I: L7 D8 R' e" k
- motor_9.run(Power);6 i5 k% I# y; a' X7 K1 Y
- } * A* d, W& }- G. O/ Z; |; R
- ]1 L3 ~7 Y8 ^. x/ A
- void MotoR(int Power){
) y! w8 G* q: I! A8 ~ - if (Power > MaxSpeed){# W r0 D! R8 s8 M$ n
- Power = MaxSpeed;
" ^$ m* B' a3 r# J0 O/ X2 A( e* ` - }
# h6 e. i; J# Y0 i6 { - if (Power < -MaxSpeed){
3 U* l0 |# x) y5 T7 X - Power = -MaxSpeed;
, X: |+ @7 F! ? - }
0 l! a9 }& O3 n* N - motor_10.run(Power);' t& y. U7 Z9 r) {* t8 z' ^+ q: Z
- }
複製代碼 $ P: M, c1 g7 I, X: `
( ~) r1 ?, p$ H& j: q c
|
|