|
|
- #include <Arduino.h>
1 _5 ], C3 H: r, J; U - #include <Wire.h>5 K4 Q* Q, ]: F! O4 N5 I
- #include <Servo.h>* k) X/ S5 G: ?- |8 _. X/ L
! [- d. c2 F) Z$ t z9 R4 y- #include "MePort.h"
% A$ k" m5 q) { - #include "MeUltrasonic.h"
2 t8 j8 Q) B) H G7 p - #include "MeDCMotor.h"
- |( |' {- n6 i# h) ~: a - $ t& H' g$ D8 p
- //double Input, Output ;$ Q7 [) g! X) M; R7 ^/ v% X$ W
- float MaxSpeed = 255;6 I1 A. u& }# I& \) l' b, W6 I
- float MaxPower = 180;
4 s g& P2 g0 ~6 |, g9 C+ ]' [" B - float MinPower = 120;5 Z q2 A6 k& s& K8 i. z7 ]
- float Error,ErrorAcc,ErrorDec;# c; M2 j6 S/ x, ~/ O/ M
- 0 a5 ^! G: w: c( K
- float Kp=0.14;
0 y& P: @2 E( ^, a; D: x) Q- A; X, q - float Kd=0.00020;//23;. H% m# }% g8 ~% z
- float Ki=0.000201;. n! `% J3 ]+ G- q& U0 D
5 [# y( ~: T! u$ P1 y- float nPower;
% u+ G! x4 m% V& O - MePort lightsensor_6(6);
) a X1 H' \ c. t* F2 z - MePort lightsensor_8(8);! k8 V' A5 t) \) f# ]$ E+ J
- MeDCMotor motor_9(9);7 H' d, K4 ]0 S' F, ]* c
- MeDCMotor motor_10(10);
0 P5 c9 S- o& V; k: N - unsigned long previousMillis = 0;
5 @; T I) j! R3 z1 y/ e - const long interval = 1;6 ]4 p3 j% N! {8 J6 X1 z5 n
- & L# P& ^5 k' S: O) \6 o2 ~
- void setup(){
5 F' U3 T8 k e- p. \3 ^+ G3 J7 ~ - lightsensor_6.dWrite1(1);% @ t" F, C' S
- nPower = 160;$ X# D1 {% o$ B
- Error=0;
* E: v7 ~: ~$ k1 S# N$ s; ] - ErrorAcc=0;
. M ]' _7 ~4 i/ O - }
" C' j( E0 r3 O9 P% c - 8 B! t- Z0 n6 P7 I( H# t
- void loop(){
: Z" n- O3 Y1 G% p% Y - unsigned long StartTime = millis();
( T( _- a# @5 f; Q - if(ErrorAcc < 18000 && ErrorAcc > -18000){
1 } {: }) i3 p2 I. O - float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();' V) I$ a& T& F/ y# n6 X: B
- ErrorAcc += nError*Kd ;
% |2 ^" \$ m; z/ Z) Q0 _ - ErrorDec -= nError*Ki ;
% r& T# z3 C" l2 d: s0 o) g2 s - Error = int(nError*Kp+ErrorAcc+ErrorDec);
% V! V# Z; V7 U0 j - if(nError < 80 && nError > -80){3 J3 G, t* s8 Y
- if(nPower < MaxPower){
8 L" u4 d0 C+ B - nPower += 3;
, T& {8 Y& G# M - }5 c$ y3 B. w7 K
- } else{
4 e9 T# m0 \) f4 S9 M) R. D - if(nPower > MinPower){; @* q, B0 A% b S
- nPower -= 2;2 B! m' q" _5 D3 c Y F
- }: k* n5 @: M' m4 z. I+ q2 a
- }
{8 P4 L0 D; L/ d5 ^% ?+ x& X - MotoL(nPower-Error);2 O6 F8 r' V; d
- MotoR(nPower+Error);
. N3 ~* o" B( T4 ^ I2 [; v - }else{
?7 f- c8 _* w( M- x, L - motor_9.run(0);3 {' \, [9 X) ?! E1 ~
- motor_10.run(0);
( P4 U3 r4 F& s: y7 e - }) t D7 r3 z( N4 H a/ `8 D7 \
- do{}while(millis() - StartTime < interval);
; i$ Q' A" V8 w7 T, J - }: |5 }1 Z% s' P4 M7 t5 V
7 q$ R6 V2 z2 y/ q' z4 k- void MotoL(int Power){7 ~* g7 O) P3 t* i! h
- if (Power > MaxSpeed){! \$ ]8 N& }* ^: I2 k2 \
- Power = MaxSpeed;
7 K" K) ^; J4 ?( M9 K: A' @ - }
, @( _$ z. o, u5 T, R - if (Power < -MaxSpeed){
U( H, r9 R U/ V! X" d! l - Power = -MaxSpeed;
8 }, q% o1 k+ Z/ H - } 7 }7 P0 Q2 M; X; V, P2 a3 ^% d
- motor_9.run(Power);: B! X2 q9 b' f' r8 R. a
- }
, X1 M. {$ V: k% _' Z2 h' Q1 t4 H
9 Z: b) L3 {# P/ \8 ]4 m- void MotoR(int Power){
% C, r4 ^ D5 U2 a3 [ - if (Power > MaxSpeed){! X5 _ P6 M/ g
- Power = MaxSpeed;
7 g/ O+ E( z* B3 u - } $ O9 y) G+ U. L9 {0 L- U% _
- if (Power < -MaxSpeed){
7 g) e; n7 M; n5 A, L0 ? - Power = -MaxSpeed;4 c. G- C; [5 n' h4 ^4 ]/ t
- } 8 D8 I$ m+ F3 \/ x
- motor_10.run(Power);, T" f* D/ P+ @0 h& |5 h
- }
複製代碼 + c1 r. g" S( {( ~- X
U3 I; P( U6 @' K2 O |
|