|
|
- #include <Arduino.h>
( i/ M, F. ]; E4 Y3 R2 V8 ? - #include <Wire.h>
1 D8 c( `4 a* n1 B6 n - #include <Servo.h> Q, \- L7 B2 k/ t
- % M# z F x& u# X8 N. O
- #include "MePort.h"
7 z1 Q! q, c+ M( h" v - #include "MeUltrasonic.h"9 ]& B4 m/ [# |
- #include "MeDCMotor.h"
0 S: ^6 I8 ]9 W1 i - 6 T+ U' p, i7 g0 B3 x
- //double Input, Output ;
$ z0 ]/ j' \1 ]5 n/ k - float MaxSpeed = 255;
+ ]' f3 t- V( _/ r - float MaxPower = 180;
- h" h6 T' Y. ^( T' H! t' D - float MinPower = 120;
: X& a0 ?1 }0 e+ q; J - float Error,ErrorAcc,ErrorDec;
' Q1 y- V# ~- x' S6 M& D - $ @* k: N0 e8 M4 b
- float Kp=0.14;, [$ h5 X& {; p$ c
- float Kd=0.00020;//23;
* T1 B3 X$ o! x; |4 Y4 L1 Q1 F - float Ki=0.000201;8 \: F* O3 l" ?9 v0 p
# P8 S* p: @/ a% W- float nPower;2 U* p3 F5 F7 _1 \
- MePort lightsensor_6(6);
1 a- f+ R" j" @, V! J& g5 E/ o - MePort lightsensor_8(8);0 w3 E# m% J& S3 e% c3 h" S, B7 q
- MeDCMotor motor_9(9);
! y! X# W/ n% n O5 T7 ~ - MeDCMotor motor_10(10);
- `' m5 P/ `2 c. l - unsigned long previousMillis = 0;2 F9 H2 x$ \5 Y' n- k3 E# f
- const long interval = 1; i' D1 c2 O7 ?
$ [9 r ]# J Z& o8 O- void setup(){- d3 _7 T6 i( r T1 \" y, A
- lightsensor_6.dWrite1(1);% x' |8 w- c/ x' o' L+ x
- nPower = 160;0 M+ A; r5 v! l. {/ X/ \! E: z
- Error=0;
& Q% h% @- u- }$ t9 i, M+ t1 X - ErrorAcc=0;
5 v3 p) `6 f3 k& ~; V' k1 Y; n - }# a3 u: a3 o* ]3 Z& x! I4 ]: T
- 2 L, X+ v) n2 C% o# [% s& Z5 G
- void loop(){
7 S6 V- E3 P9 x$ c) |# T - unsigned long StartTime = millis();
% h' N: l7 h; Y* I8 T% T- F9 b - if(ErrorAcc < 18000 && ErrorAcc > -18000){" l4 ?; V' ?. z
- float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
% ?1 Q, R0 S: i/ L0 l - ErrorAcc += nError*Kd ;. W2 D, {. r5 Y. Z2 v
- ErrorDec -= nError*Ki ;
5 U5 R+ D' F0 m8 T! P, M - Error = int(nError*Kp+ErrorAcc+ErrorDec);9 P7 `6 b5 F& a& g
- if(nError < 80 && nError > -80){3 P* @' `5 s9 L# c# [
- if(nPower < MaxPower){
8 q" w+ L$ C8 P4 e6 d. t1 F* C! I - nPower += 3;
/ |3 D$ t- o/ @ b* k) u7 ]" b - }
k- k! b1 h1 U( k - } else{& S- `8 m; A9 l6 A
- if(nPower > MinPower){ a1 V+ ]% G- ?' c: x4 l! F
- nPower -= 2;4 c X4 v5 H0 \- b
- }
; A8 Q3 d( r7 o. V - } 4 j- A5 B" n* F0 I( @. ]8 H
- MotoL(nPower-Error);6 m d+ _2 Z( E9 K* }' E
- MotoR(nPower+Error);
5 ]. D& b8 |% n+ p% c+ Y% r - }else{1 q. O6 \6 o9 E8 p6 @' \. F
- motor_9.run(0);
8 u' E, w- x ?+ n7 c0 H8 R* g - motor_10.run(0);3 Z% Z: k0 Y2 D% ~" W# O
- }) d3 R# [0 x& n3 a
- do{}while(millis() - StartTime < interval);
) K! d6 Z1 E. T; p& O f) W - }
8 T5 L/ f+ u8 I; N" }. S9 c
3 n @% C9 m- W8 @' n% b9 s8 S- void MotoL(int Power){: c4 }! a9 p( _" P& Z5 Z2 M; C
- if (Power > MaxSpeed){
( y# b* M' L) l) N& R0 U - Power = MaxSpeed;9 E; ~& J X: S
- }
9 w. `$ ~6 N$ F4 h; w3 ] - if (Power < -MaxSpeed){& B( `6 P6 T1 t1 R* b
- Power = -MaxSpeed;
9 B$ ]: e" H1 T7 k! K - }
6 A) U0 ~# I6 y: N - motor_9.run(Power);
9 ~3 k' O7 j6 l- X. H: U - } + ]8 f1 h: ]- W& s
- ) S3 {5 T2 r& C( o- O5 ?
- void MotoR(int Power){3 @% S: V- `: J
- if (Power > MaxSpeed){, e* O T _' e: o+ a; S& y
- Power = MaxSpeed;. c$ E$ y G- d" X9 m1 O
- }
f) D% U, R4 K x/ Z9 f$ s- w - if (Power < -MaxSpeed){
F. t9 f$ c: F' i - Power = -MaxSpeed;, R* x# h0 P. x5 H
- }
+ H: h j, T O7 d$ l - motor_10.run(Power);
: i2 A# d" B( R - }
複製代碼
- g1 `2 N" T/ Z8 `+ y# U2 e2 {% x( o9 }: R; `
|
|