|
- #include <Arduino.h>
% H% y4 K. {* ~- b& w$ q - #include <Wire.h>
6 l4 b/ L6 ]/ Q, a - #include <Servo.h>
. @1 F0 F+ r4 u% M0 \1 \& @
7 L8 g0 E a$ L0 ?" g2 V- #include "MePort.h"# A. _) t$ F( f. X1 ]' O
- #include "MeUltrasonic.h", p) p3 }* P* | J. L
- #include "MeDCMotor.h"
& U$ i6 V2 [8 O, h$ A% z$ H) V - , ~ V& m P$ C2 x( t. c
- //double Input, Output ;
2 o1 J# n' x2 d+ z* h& [: [/ T3 X - float MaxSpeed = 255;/ p$ ^" Z x: }( b- u
- float MaxPower = 180;
7 H3 W8 }0 g. m - float MinPower = 120;- n. [$ a7 f' z" B9 r6 Z3 t% q4 p
- float Error,ErrorAcc,ErrorDec;
' G! X: G2 U9 P, ^' h; n8 t! N - / D3 Z. ]" g9 G3 E* T) b2 U
- float Kp=0.14;
4 r% a: }7 m2 W8 {# K( K( \ - float Kd=0.00020;//23;* E1 W+ C. U3 I: \0 H$ A! r
- float Ki=0.000201;) g( Q, b8 l5 V. e4 G# y, X, p
- ; n6 B8 H: V3 {
- float nPower;* g3 k, V. B0 Q2 x- ~7 S
- MePort lightsensor_6(6);
" P4 D0 B5 C+ W0 e0 p* S$ i - MePort lightsensor_8(8);: U' |4 Z) W7 c: H+ n x
- MeDCMotor motor_9(9);
. G3 W; ~# }) M( |/ z5 z2 P - MeDCMotor motor_10(10);
& X D1 F L2 D: t3 t - unsigned long previousMillis = 0;# h0 G6 t6 ?) }" `/ w; x
- const long interval = 1;
' A0 O0 ?9 I- t
; F7 X& j+ E4 z, y1 ?. C6 C# J- void setup(){& E d; h' }& {; @6 U$ \; T) t) f
- lightsensor_6.dWrite1(1);8 N2 H9 k/ f# x& U$ c
- nPower = 160;" h# T" |9 W, i
- Error=0;, ?/ @$ n5 `4 Y+ A, f0 n0 }
- ErrorAcc=0;
' y) s" S$ W, |5 U5 ? - }
7 C/ K6 t$ F9 ]* a& u
1 o1 q7 d) `0 L: d- void loop(){
8 i$ U, a$ j* @2 P& F% \. {& D - unsigned long StartTime = millis();
5 P9 L6 o: n% h' R, C - if(ErrorAcc < 18000 && ErrorAcc > -18000){2 \& x+ I( }% g0 ? P, I$ _1 ^: v) c
- float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();, u2 l% n5 U/ v. K
- ErrorAcc += nError*Kd ;
: }& C, f4 b* r( \% h0 n - ErrorDec -= nError*Ki ;, e& Q, @7 g% Y4 O, B7 D) z9 ?* l" k
- Error = int(nError*Kp+ErrorAcc+ErrorDec);3 n/ x% u7 q2 I B5 p6 v/ p% \
- if(nError < 80 && nError > -80){; k0 _4 @( k9 V7 y A& y
- if(nPower < MaxPower){
3 U$ @% v6 H* u' ?2 K$ N - nPower += 3;3 ?( [9 H* Y/ v6 q7 |! Z U) Q
- }( j" x2 y+ F' ~3 ]8 N" ^9 U
- } else{4 |0 W; [& |3 _7 {
- if(nPower > MinPower){' w$ ]" K' s5 W, Y/ e n% \
- nPower -= 2;
1 J0 S( d. D+ K6 C9 i& [ - }' U. [+ \5 Q5 a+ W
- }
7 T5 K7 f3 x2 R$ G r- {5 k - MotoL(nPower-Error);
; G: F- S, P9 L) O" z - MotoR(nPower+Error); h( H+ X( n$ D! ?
- }else{
2 ^% p. V q1 Z' n. x - motor_9.run(0);
! W" W" A. o3 g2 E! \* ~ - motor_10.run(0);
9 B$ Y6 [# w9 [7 v6 c - }$ _7 L. q& e v3 E: a! }) ~1 L
- do{}while(millis() - StartTime < interval);
) V) r0 a0 a; o- o, r& { - }; R8 Z* b2 R6 X" [0 ?( Z4 X1 N8 y
- - k; j5 E+ N E
- void MotoL(int Power){
c6 ?, ], z+ q9 L8 J - if (Power > MaxSpeed){! u1 k1 M7 ^ s. l3 |9 d4 c! {
- Power = MaxSpeed;4 O- `8 V8 ]/ ]5 E& U8 X/ n) ~
- } : J' G3 @+ d1 J
- if (Power < -MaxSpeed){
$ A# _7 T$ G7 L# ] - Power = -MaxSpeed;
$ y6 z) H+ n; ?3 C" R- g - }
, d, U$ m5 z. ] W( J3 n" ^ - motor_9.run(Power);# `- V' d8 Y+ q; ~* v
- }
. O7 Y8 t" i4 c4 t; S - N5 F% f5 n( C
- void MotoR(int Power){/ A$ B2 T, e9 A& U2 Z4 O/ q& Y2 l
- if (Power > MaxSpeed){% j7 i& v1 ^8 R9 w, [
- Power = MaxSpeed;7 A/ t% F" u0 X* Q# T
- } 9 `! w/ e$ P1 b! `: `
- if (Power < -MaxSpeed){ Z' V; |& { X n8 n0 U; U( D8 y
- Power = -MaxSpeed;
2 _1 z; V ~. b* N9 F* D# p% N - } ! I) x# w6 c8 J
- motor_10.run(Power);6 H) R; h" P3 f% k
- }
複製代碼 5 ?" F* ?" _; N4 R/ t
( J0 a+ I* r7 [2 ^, ~' N* y/ b |
|