|
|
- #include <Arduino.h>7 V7 T# e; E- t) K, P
- #include <Wire.h>5 @( m& ]3 a5 Y i6 Z! Z5 o
- #include <Servo.h>; l5 t. A( B7 R5 S0 M/ D- Q
( x# E3 g9 F* R0 J+ S5 y" j- #include "MePort.h"
4 t1 b, N; O$ ]! t) D - #include "MeUltrasonic.h"
7 Y4 ^- K% E; ?8 U G% l& v: T - #include "MeDCMotor.h"
# o) l1 ~) m% s- t) M9 i - ) l( J1 U; h. p1 j
- //double Input, Output ;
3 j; a+ ^4 m: X( L- |* b - float MaxSpeed = 255;
$ u& | {: p g' z+ |4 r, H6 g7 X k - float MaxPower = 180;
4 I$ }% ?$ J5 b - float MinPower = 120;2 k. U5 l5 s" D, N8 `6 n& c r
- float Error,ErrorAcc,ErrorDec;
* }9 X/ f, G+ Z5 ]5 D+ K - 0 V8 f* I7 @: {% Z# L
- float Kp=0.14;. B" V+ }2 Y. v' X7 `
- float Kd=0.00020;//23;
# G/ ?. X7 ]+ f6 v - float Ki=0.000201;6 q3 @4 ?% V5 G$ u y, S' ^
" A" I- ? Z! r" L- float nPower;
/ x$ A" f: N) `& w+ z. l1 J' b, w - MePort lightsensor_6(6);% _. [' t( w, m; \
- MePort lightsensor_8(8);0 Z1 c! O2 Q0 Z$ k& a8 E
- MeDCMotor motor_9(9);6 j4 b9 y8 ~; V7 d1 c+ |
- MeDCMotor motor_10(10);4 t/ @6 B4 M/ W7 K% x: K7 ~
- unsigned long previousMillis = 0;+ |. X) G7 l9 f; ~# E
- const long interval = 1;0 t) u- m7 o* U6 c a
+ V$ G' u4 D& ]+ H$ d0 j* |5 G- void setup(){
( G2 ~/ q8 t; |2 p" m - lightsensor_6.dWrite1(1);; B( P) }. T8 P& C) ] W
- nPower = 160;
( H1 b/ w/ h1 Y8 B. K - Error=0;
+ {9 X, N& n! q$ |6 Y. }; } - ErrorAcc=0;8 Y' m* t. r+ H8 T* y& D
- }
9 d# ?- {) d/ z- t& S2 {
( `* }& n5 y2 J H6 Z: F- void loop(){
; G. c4 s( K1 v Y- c8 V5 w; A$ T - unsigned long StartTime = millis();
+ R5 e& I' m6 x - if(ErrorAcc < 18000 && ErrorAcc > -18000){6 r0 z$ p6 U4 j. l e
- float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
2 j5 B2 ?8 Z, \4 `1 @ - ErrorAcc += nError*Kd ;
1 G4 D; ?) m) D A" a& W5 f7 } - ErrorDec -= nError*Ki ;* A! ?* o& t$ F6 F8 E+ u, h) @
- Error = int(nError*Kp+ErrorAcc+ErrorDec);
. \2 ^0 k7 F/ M% |6 q" h6 { - if(nError < 80 && nError > -80){! C3 @$ k" T6 @$ T. `* D, \ K
- if(nPower < MaxPower){& z7 f/ L2 R/ U! a* U" r0 B8 w
- nPower += 3;9 v) Y/ d: Q5 ^* o( D0 `3 C9 @* I
- }
' r7 k' Z# ^8 u4 s% n9 } - } else{6 w$ O; b3 I7 I0 a) H* J/ @4 u5 b
- if(nPower > MinPower){
4 N, @. v% |0 b - nPower -= 2;/ H$ D) P4 h: I! m" V. ^5 i
- }
9 t8 Q5 |9 G" n# ~* e( k - } * r: E- K q# U
- MotoL(nPower-Error);; g1 ? F* @8 s3 X1 X. ^, G7 F
- MotoR(nPower+Error);
5 \% u, E( S. z) Z4 { - }else{
! r0 u: A8 w$ F& h9 C2 g - motor_9.run(0);
6 Q8 ~" }& X2 U3 m - motor_10.run(0);
& a! S* m) y4 J( w+ u& D1 \# ? - }7 y4 X5 Y& z: p- @! J* }
- do{}while(millis() - StartTime < interval);. o1 y- Q2 l) @& `! r. k+ [1 _
- }. r# F2 `; G/ g! Y5 C J/ \
- 5 N& @: P, t( M2 b5 ]7 i' M% `7 J
- void MotoL(int Power){- S3 {4 Q0 X/ m
- if (Power > MaxSpeed){
$ e6 _( o8 I- k% K! i2 B, v* c - Power = MaxSpeed;
R5 P6 A$ l, A7 `0 Y, O, j - }
# Z" I4 e8 _, a, ? X; e - if (Power < -MaxSpeed){
! t, F' M m- h2 p9 W) B - Power = -MaxSpeed;% R4 m) J* Z. d8 [
- }
7 x1 S K% w/ ~. S/ P - motor_9.run(Power);" R5 G! u& B8 X, n6 ^9 M `
- } - w, f2 J0 G& k! P' T5 A+ g
- # o. N [3 Z. _7 ^6 \9 o& L/ Y& c
- void MotoR(int Power){
5 _: y, Y. F# m& B' a) }5 d6 a& f - if (Power > MaxSpeed){1 Z6 F" {! [- N3 Z# v
- Power = MaxSpeed;
! q1 Y. s, A0 a' A- F6 i - } 1 i, i/ B" }# m. t
- if (Power < -MaxSpeed){
5 d7 k8 J6 z' D/ V7 o - Power = -MaxSpeed;9 K: ~& u9 l4 p! Y0 A2 I
- } - j1 N: H/ m. g- y/ @1 M
- motor_10.run(Power);6 X7 Y/ @( n6 b; Q; p% i/ X& G
- }
複製代碼
/ _5 Q# p; t0 D- Q: G! a3 U, \9 M8 e4 D, R4 ^2 R* ]( E
|
|