|
- #include <Arduino.h>9 ]6 d! O. R2 J6 F& ~
- #include <Wire.h> y& t, D' o2 p6 b3 I c: o+ z
- #include <Servo.h>+ x6 h/ f- L2 R: K4 C' I# l ^
- - }+ M" X/ E$ Q& V
- #include "MePort.h"" L! E0 r% W: r% t
- #include "MeUltrasonic.h"
+ F8 E- h e/ G* ?6 @ - #include "MeDCMotor.h"+ L( l. }% l1 q% ? e
8 c) B0 O9 ?' }+ S. ^- //double Input, Output ;
( o$ ~$ V; S) T7 x, ?! h! K - float MaxSpeed = 255;
# F7 M3 x& m" u. s5 _ - float MaxPower = 180;
- |$ {# O- E( q5 A+ o! X% s - float MinPower = 120;( `. `3 K# d9 i( g: C! |1 M# l
- float Error,ErrorAcc,ErrorDec;
' R1 c9 o' g* c* r% f# ^4 r8 H - , l9 T( s: C0 d" x4 r/ p: r
- float Kp=0.14; r8 H" d# I0 R
- float Kd=0.00020;//23;' d2 G2 m6 p5 r/ t( x# d! Q
- float Ki=0.000201;9 q$ O- E- A4 A9 A1 v
0 H6 u+ G/ K+ S0 T" ?; }" A, S- float nPower;6 q6 N; _" d3 p2 \( i+ ?; ~; V9 Y" L
- MePort lightsensor_6(6);
* Q! |( R4 b$ Q1 T: |6 I8 T0 r - MePort lightsensor_8(8);
4 W4 a0 b. U/ e - MeDCMotor motor_9(9);
, p5 p9 P7 g4 `0 p% @ - MeDCMotor motor_10(10);
" T6 ^/ ]) C% M% V - unsigned long previousMillis = 0;/ Y2 n8 n6 Q7 z( m$ Y
- const long interval = 1;1 G8 M6 q$ o7 O$ s/ |5 ^
4 r7 \+ D+ o4 X9 O1 i: s. Y8 U- void setup(){
& B- d8 k& H9 c. {3 F% E - lightsensor_6.dWrite1(1);8 ~5 R. A& \& p9 _ ^; N; U
- nPower = 160;, s0 G5 z8 j0 T
- Error=0;* L. d' R* p! g/ r ], d
- ErrorAcc=0;
- j9 X/ S2 P. R2 C. y+ D - }
. P1 l- ]) v- D9 `" L9 v
5 u. m6 z, v/ L- }, }- void loop(){0 g# z1 `9 Y5 g* E3 h1 d! l p
- unsigned long StartTime = millis();
& [' i7 [( F$ c" @6 \. n - if(ErrorAcc < 18000 && ErrorAcc > -18000){' ?$ e! w8 f! e
- float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();3 Z% @! y" B1 I2 i
- ErrorAcc += nError*Kd ;6 ~% n% }8 J) m5 h2 \7 ?" l
- ErrorDec -= nError*Ki ;6 A2 g+ Z( `( B4 ?- R
- Error = int(nError*Kp+ErrorAcc+ErrorDec);; g9 o) I- r: w2 D( J# @
- if(nError < 80 && nError > -80){% b' l. e9 Z6 K
- if(nPower < MaxPower){
9 p# }+ e) D& d. p/ m - nPower += 3;
! y5 m; z( @4 N - }
/ @% G- J$ S8 z% I7 o9 { - } else{
5 a1 ]0 S- C* \2 l - if(nPower > MinPower){1 v8 V! F8 W) T6 a; m9 a: w* H
- nPower -= 2;
% s) @, ]) p& P9 s8 q6 H6 L - }# g+ `5 V7 a, _7 I$ o, r& `
- }
* @5 F; w- G# }, u- \8 D0 H - MotoL(nPower-Error);6 ]5 L/ n4 I+ |* v' k
- MotoR(nPower+Error); 5 x1 Y S/ y* H
- }else{5 W- t% O1 l% {' W9 n6 z$ t7 v% M! `* x
- motor_9.run(0);$ R6 o% k+ {- T
- motor_10.run(0);
/ s6 A; c1 r4 J2 i* f8 z - }
9 E- d7 X _% y$ { - do{}while(millis() - StartTime < interval);6 m: s3 M' j& l7 f
- }
" Q$ `, N8 r2 B* q: J - & u" T3 w$ ]! t/ M. B( R
- void MotoL(int Power){
. V- E1 m- F( g9 L T- s( G. Q5 n3 m - if (Power > MaxSpeed){
4 j3 k: ?* d( s6 I - Power = MaxSpeed;
3 L) B5 e) q) | V; ] - } ( J @+ j4 V6 {, {& l% U( U
- if (Power < -MaxSpeed){
5 B4 w J) N h3 L" L* n - Power = -MaxSpeed;
$ Z5 f/ J, V7 r+ l - } / t* L& E m% _; U( g# h5 ]
- motor_9.run(Power);
f8 Y5 A. X. G5 | - } 3 t" f7 r A2 L" Y7 ], Q0 g6 ^
9 V: A2 @" _5 b- void MotoR(int Power){
( w" q) K) F4 }( V - if (Power > MaxSpeed){6 v* p9 D# V' Y
- Power = MaxSpeed;
+ D4 c% S& ` t1 b - }
5 W# T: Q0 d7 U - if (Power < -MaxSpeed){
$ N( z* J; t# ]% `- \6 U/ s; X - Power = -MaxSpeed;; ^5 a% r, k' Q$ _! k9 k
- }
. D: [3 Q3 s/ _. W - motor_10.run(Power);( W$ T# O/ h1 w# \( s( l( ^
- }
複製代碼 . G0 ?, Y! z) w7 T: g/ ?
1 j( ?3 [( F, `7 n |
|