|
|
- #include <Arduino.h>
' g: ^( m$ r9 n2 X1 q- p - #include <Wire.h>
' z' e% o0 `1 |, x% j- A - #include <Servo.h>
: j+ P4 N% V0 f1 Y5 n - " b5 `4 \ \+ I3 g; g/ G
- #include "MePort.h"8 @' `* R) [- a: i& e* K
- #include "MeUltrasonic.h"
- a& x# h" }" t% \1 X- e' t - #include "MeDCMotor.h"4 V, O& T2 u; U1 p8 e3 i
- $ s% C" }& n( o7 N* P0 s5 B. X
- //double Input, Output ;' v8 s, W, d% U9 X
- float MaxSpeed = 255;9 N7 f g" t; I# P3 A- o' p+ }
- float MaxPower = 180;
2 V0 ^6 i* R' h - float MinPower = 120;
2 O4 Z7 \' I" E$ F. U - float Error,ErrorAcc,ErrorDec;
- ]3 s& I* V. S; P7 f( p
6 I/ M! |9 [8 m, K2 q9 \# b2 P- float Kp=0.14;( s1 R$ G8 T/ e1 j- j% E; {
- float Kd=0.00020;//23;- ]1 v$ v+ ^* J; k1 v Y# P
- float Ki=0.000201;4 b7 x+ R, C7 w/ f7 I
) \, X: b, x) Q$ h! u! x9 n4 W: Y- float nPower;
U5 W% O& [/ q3 _- N - MePort lightsensor_6(6);/ _: b' J( @6 J7 }, {
- MePort lightsensor_8(8);& f5 I* r& n' z$ k0 z. ?% k
- MeDCMotor motor_9(9);
/ w. v3 L6 F; k! ] - MeDCMotor motor_10(10);5 O8 Z/ @ [ N% X
- unsigned long previousMillis = 0;
3 e! l; R9 t) ?1 S' C - const long interval = 1;+ b- A6 X( `2 l( h2 g8 t: Z
+ v7 {1 d3 [* y- r4 O- void setup(){
! O$ s& Q7 e! I3 W/ X* B1 M! z' F - lightsensor_6.dWrite1(1);1 r0 O- r; `0 M* e% J% |
- nPower = 160;$ x( `. Y" m5 h+ x/ m8 l
- Error=0;$ h! R- r& j9 A. |; l; G% ^
- ErrorAcc=0;+ O' k a; u+ y( |" R( R" k7 c
- }
7 q7 K0 N: o/ M7 K5 }: V& n - . A$ ]% d/ K0 ~. d1 ]* I) b
- void loop(){$ j' `. n" t P$ [, k
- unsigned long StartTime = millis(); z3 J w$ p" @1 _8 e* h
- if(ErrorAcc < 18000 && ErrorAcc > -18000){' x' Y. @# H0 i f- V+ l
- float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
) C" R0 D! J# K8 L1 ~5 l - ErrorAcc += nError*Kd ;6 L2 q( Y6 Z" T- k& Y2 t* e
- ErrorDec -= nError*Ki ;: P# y; [+ z; c# T# F+ Y1 y
- Error = int(nError*Kp+ErrorAcc+ErrorDec);- ~( ]* S/ J' p8 O6 p! \$ U0 l8 p
- if(nError < 80 && nError > -80){0 |( e0 `* n5 x, Q
- if(nPower < MaxPower){" N, g/ {* b Q! V/ c! d8 K
- nPower += 3;( D$ }7 A; C1 p. Y4 y
- }
$ K2 Y" M" P3 ^9 G! U) y; n7 {+ @ - } else{* x9 a5 g9 V, N" t/ T8 o
- if(nPower > MinPower){
( g3 @7 Y$ q4 e" H7 |: g) B" i - nPower -= 2;
6 _2 O3 H( K8 A7 A - }* i9 i" m' `% h& s5 i7 a0 ]+ w
- }
! q& a1 ?& F q, v: W: w - MotoL(nPower-Error);
8 A7 X) v: J3 [4 }8 l4 K( y( e - MotoR(nPower+Error); 6 W0 F9 C7 Q8 @4 @# ^
- }else{
: G3 x- C+ [% D8 U - motor_9.run(0);. ~ _& L& Y) ?* V s1 w
- motor_10.run(0);4 \) i& r4 I% v @# G
- }
6 ^$ l" S$ J: W - do{}while(millis() - StartTime < interval);
6 a6 Q$ Y% z$ r, M+ ? - }
9 E3 Q+ \' ?+ h$ I) T& a - 8 h j( T3 m3 m
- void MotoL(int Power){, a" G& K0 z0 y4 \
- if (Power > MaxSpeed){
0 J3 p" D3 s/ d - Power = MaxSpeed;
% [9 ~( y7 L9 i - } ) W. m! ?7 e/ t: F0 M, V
- if (Power < -MaxSpeed){
( u# [# F6 t. f0 c3 t* u - Power = -MaxSpeed;
9 S/ D' W0 @' y( @( V& w( J, k - } V+ B- e' l0 D/ ~
- motor_9.run(Power);
8 U' S" F& y" b: G' S - } # h- Q# I! k n' G4 G1 g& Z
- 0 s" a8 V6 i6 C6 m( `! U
- void MotoR(int Power){* b& g! \: }/ h% K, s3 K
- if (Power > MaxSpeed){3 S# t) u, L, Q
- Power = MaxSpeed;! ?/ ~# m" x4 ]
- }
7 u/ g8 D- K* s& H4 e$ O& Q; f; K - if (Power < -MaxSpeed){
/ X0 m: R; }1 E0 d - Power = -MaxSpeed;0 b9 f% S- v# J% l. q5 _
- } ! R/ @& D6 f% r3 i
- motor_10.run(Power);
2 C, W3 o0 z" [7 N* O% G - }
複製代碼 ' `6 i& g& I) q U/ v9 x2 k
6 r5 p$ d3 E1 L) I+ w
|
|