|
|
- #include <Arduino.h>8 A' c' @/ }& ~. U& w5 x
- #include <Wire.h>$ R) Y6 B; z9 y8 m$ Q
- #include <Servo.h>
# M5 E$ m% W7 \& [0 n
' Q! L0 J ]. E5 f- #include "MePort.h": n, O1 r! p6 v7 R' Y
- #include "MeUltrasonic.h". `& g4 w, B5 `3 {4 }
- #include "MeDCMotor.h"
F; p7 B( }) P+ x/ G8 u: A
! D- Y+ D% A7 y5 y6 q- //double Input, Output ;7 J; z. y4 J6 x* z
- float MaxSpeed = 255;/ Q! g/ g$ ^/ r ]' F
- float MaxPower = 180;
, y! s9 G3 N! [ B - float MinPower = 120;
- x$ c- J3 Q" A6 O- m - float Error,ErrorAcc,ErrorDec;2 Y& t$ k& |. C I9 R+ @
- . K- v/ k {! U& M& o
- float Kp=0.14;
7 X6 ]$ t. P' A" M& A - float Kd=0.00020;//23;+ z! W( ?4 N2 C
- float Ki=0.000201;: Y& _& z7 D6 N) e, ^% E4 n: ~ i
- }8 u, I: ?( K1 b* O3 o. s8 L
- float nPower;
% G1 i8 u R% P6 D' p) w - MePort lightsensor_6(6);# m% E% h& ?3 k2 M' n
- MePort lightsensor_8(8);
/ ?" s% A: A+ z/ Q/ ?, ^' f - MeDCMotor motor_9(9);* r4 w2 g$ m- E1 d8 d" ]
- MeDCMotor motor_10(10);* `- S0 p6 V. |$ e4 u7 z6 }( F
- unsigned long previousMillis = 0;' P9 W0 ?+ \9 |7 L! s
- const long interval = 1;
% O) d& M& {: U( b
e+ M- N6 U2 z8 m0 B- e- void setup(){- `% n2 x9 h- n9 E f n. n9 E2 [
- lightsensor_6.dWrite1(1);) _# r X; U$ H) y: o
- nPower = 160;" ]7 F- u8 {; [, ^- V/ O% q/ @( F
- Error=0;+ R* m2 r# |8 k. {6 U- v
- ErrorAcc=0;) k$ J5 a0 F4 A% l) F& k9 o8 a/ B
- }5 N/ p& ?6 ]0 Y+ C' k* w/ ?2 c0 e
' K3 S, W; O5 x- Y2 m# `- void loop(){1 Z5 V+ a$ F3 \- c6 e, Q
- unsigned long StartTime = millis();
& u/ M9 M5 @4 T - if(ErrorAcc < 18000 && ErrorAcc > -18000){
7 D* B' f. ~- p6 v - float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
9 O0 ^4 p- l% \4 f8 t - ErrorAcc += nError*Kd ;" f+ m% D$ r: i: ?9 l
- ErrorDec -= nError*Ki ;2 V& B, q5 Z7 ~. E$ t: X
- Error = int(nError*Kp+ErrorAcc+ErrorDec);
$ u4 ? Y/ q% X) Q: i, f - if(nError < 80 && nError > -80){0 J0 G( s0 I, q9 O& A
- if(nPower < MaxPower){
% Y1 |' @; t: z5 R4 o D - nPower += 3;
3 I6 R U% h. [! h6 n - }
; v* _0 ?5 F7 g1 Y - } else{
M% d1 B: O; x - if(nPower > MinPower){/ R* t1 Y0 q7 B( a0 [
- nPower -= 2;
5 w( p2 X' M7 D0 P/ y - }6 A' L" `( m. O% U* n
- } + ?- V2 G9 q' }1 L
- MotoL(nPower-Error);4 Q, Y2 b3 i$ U5 Q/ r
- MotoR(nPower+Error);
) ~; I" r, D# N U: B - }else{+ z0 f% s+ V! p d! Y
- motor_9.run(0);
3 U! M6 ^$ E6 Q) y4 _' i% p2 r- Q( Z - motor_10.run(0);7 V! a5 A2 b6 f, i+ Q" I
- }
+ B2 C, H9 K! S: Q% W9 X) r - do{}while(millis() - StartTime < interval);
' R8 y0 N% f$ |% M; D3 I( G - }$ A7 v2 M# m; v. E7 S: j* P% h6 c
8 w `0 u7 l2 F9 m$ f- void MotoL(int Power){
) h! M3 b+ O. m - if (Power > MaxSpeed){& u# I& z6 l: N2 F. X. h) r
- Power = MaxSpeed;
) e, F0 {. c' r - }
5 H: e' x( z, L3 ~: A+ m - if (Power < -MaxSpeed){
4 K! p4 j0 N! ^ G& P! t - Power = -MaxSpeed;$ I( P9 n S& f# }: N1 h
- } ) `: n; t- O+ Z
- motor_9.run(Power);
/ P4 [; W" U% f$ c& c0 J - } 5 C: G3 T- t+ z9 Z& k6 u" o
! w% j) I1 O3 t9 I9 ?5 v- void MotoR(int Power){
7 h' Q0 ?: Z( T5 L% R - if (Power > MaxSpeed){! M v0 m7 l3 x! S' Q K
- Power = MaxSpeed;
( m4 j7 t( L. q9 x: p7 ] B - }
; C; n9 d2 F# O: I% ?( X A( G - if (Power < -MaxSpeed){7 f& ^* v' q1 B9 D7 \6 ^
- Power = -MaxSpeed;
6 _4 q" j7 P2 O0 e$ O, O3 [: E: S - } $ K! _! \. H" S. H0 o* h- x# t
- motor_10.run(Power);, c; S9 @6 e( a( Z% [( j, _8 d Q
- }
複製代碼
8 u5 Z' \" u. l, y( W5 V
2 ~! V/ d+ J5 c3 H% o |
|