|
|
- #include <Arduino.h>
+ i4 t3 f7 \1 p" i( T( {% \ - #include <Wire.h>
0 n v4 S5 P0 s7 s - #include <Servo.h>" b* e5 A* l- {. @
- 8 a. m1 L- I( A) L- F; D
- #include "MePort.h"
4 X) g) t9 G) k( N - #include "MeUltrasonic.h"& y6 ]- w" S; r* e/ z, R
- #include "MeDCMotor.h"8 O% {/ D# \0 g4 ~. P
- 6 \5 m0 U: C8 k
- //double Input, Output ;
9 R, v5 M. h2 P' G - float MaxSpeed = 255;' q# n$ | v% V, i* K
- float MaxPower = 180;; K" ?1 A. ~/ R7 y, R. Y8 ^
- float MinPower = 120;
! u! b" G4 b' Q' ]& h9 W' x4 F - float Error,ErrorAcc,ErrorDec;
+ r, z) t/ M$ p3 Y, {, Q0 } - 0 g+ k s0 m5 \
- float Kp=0.14;' y8 w& Q3 U. T
- float Kd=0.00020;//23;
, S7 t' N* g9 V% c1 \- | - float Ki=0.000201;
- `! p5 d8 f) t7 r, }( @8 | - % K5 k& a9 v) f/ ^9 k; e9 }* l
- float nPower;
' |* ~5 b/ R, q, _) N' Y - MePort lightsensor_6(6); N0 r1 a+ `2 {) c
- MePort lightsensor_8(8);
& t. P0 i- s. v( x4 D J - MeDCMotor motor_9(9);
( a7 x* E7 B# Y% Y. _3 H: j: p- X0 F( W - MeDCMotor motor_10(10);
. v8 P( u% ]) A3 S - unsigned long previousMillis = 0;
- K' a( {$ k) O- `0 }6 Y - const long interval = 1;, f- Z0 R8 |6 i9 m( K/ c5 e
4 d3 e: G0 r3 u8 K/ z* p3 n3 b6 Y- void setup(){
2 H+ h) Y; R& c$ n! _+ j3 O - lightsensor_6.dWrite1(1);8 A5 _2 m+ o+ e3 Z. `% k
- nPower = 160;' H9 I5 d) E) D; v
- Error=0;4 f7 J& D" h* A j( Z* ~
- ErrorAcc=0;6 n. X$ }' S* S/ Z9 ]- U" G
- }
h8 @' J* \8 @* I r) m3 _( Y# E - ' W: D3 s4 ?4 K+ T- L4 ~9 U; \4 ]
- void loop(){
) T2 }$ h8 t! t$ k - unsigned long StartTime = millis();
: G, G5 \/ L* S% j; f - if(ErrorAcc < 18000 && ErrorAcc > -18000){
; @5 C, S2 D" J+ W, g - float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
* N, r0 S7 p) _ X2 m - ErrorAcc += nError*Kd ;0 j/ L6 A5 V- K; v
- ErrorDec -= nError*Ki ;
4 S2 E4 ~0 s' _& N - Error = int(nError*Kp+ErrorAcc+ErrorDec);
+ [. w; f2 z/ F4 o( K" I - if(nError < 80 && nError > -80){* ^6 w. B7 s) h
- if(nPower < MaxPower){
8 E# h9 f. g# P5 [7 X# x - nPower += 3;
! V7 b7 E- |3 b n$ B* I$ U1 l - }
- {) _. t8 h3 g; w4 q# | - } else{) C1 _2 H+ m/ g2 l# |9 k& m) r
- if(nPower > MinPower){
$ b" Q1 \. _3 C: `5 x( I# T - nPower -= 2;8 W+ p1 D6 i7 N# @% a
- }/ a+ h3 b' c5 V- Z) K: f+ `
- }
- X+ I5 ~: d& K4 J - MotoL(nPower-Error);
) _8 g+ \& S5 ?) T - MotoR(nPower+Error); 3 K9 K( {1 S* P0 p) k9 t
- }else{
P, X% f: ^$ y- n, O& ^7 G- | - motor_9.run(0);4 l; e; E" ?9 B2 B/ n# _- J
- motor_10.run(0);' w+ t0 V: x) {4 r, Y+ f
- }2 P/ |. q/ `0 g( g) m6 _; ?: f7 K
- do{}while(millis() - StartTime < interval);
s2 Y c4 m) N ]" O1 n; Y - }+ F8 p. M4 p& O' n
$ {. k& a1 p2 S7 P- void MotoL(int Power){
1 ^7 |. _( r0 U - if (Power > MaxSpeed){
$ l* C- x: Y, n! @+ D' M; O - Power = MaxSpeed;& Y4 T: G2 W& `+ ?
- } - D2 ?( B1 T5 L8 L1 z* z4 U
- if (Power < -MaxSpeed){ i9 M7 z$ Z% B! s
- Power = -MaxSpeed;
6 o+ F& ^' Y6 @1 c4 E: e% F - } 9 }, I( k$ j( ~3 O
- motor_9.run(Power);" A7 F- E$ t$ N* n4 ~
- } ! n m& \6 f3 c- Y3 A/ w& E1 a! p
- 8 r3 t1 z0 p/ ?, Y' i
- void MotoR(int Power){/ m3 s! |0 k' m
- if (Power > MaxSpeed){
6 @! X8 v( e3 f# ]( \% w - Power = MaxSpeed;
" k: q4 U, ~: l4 T1 j* c ? - } 1 [6 @# X7 _- E% w
- if (Power < -MaxSpeed){
; a8 m6 W/ Q2 A1 i' [7 r* ~ - Power = -MaxSpeed;% a4 r, z6 f6 F9 q- s5 s
- }
8 s" X- m8 k; g g) S3 S7 q - motor_10.run(Power);% h! b) ~7 C4 b8 t1 }
- }
複製代碼 2 T& j6 ]7 N) C& ~7 I$ G: R
2 ?. }/ w! L6 [, j& ? |
|