|
|
- #include <Arduino.h>
: j. l' A0 W7 { _ - #include <Wire.h>+ q1 Q+ w" Z" F* d5 Z7 l
- #include <Servo.h>
' b/ v1 J- h8 V0 t# d4 q2 Y8 u, ~ y - + h6 l2 R) p" s! W# T) d5 `( @8 m
- #include "MePort.h"
# L9 \# e( i8 u, d - #include "MeUltrasonic.h"
( m! n( Y9 g3 c$ D4 i2 K( c - #include "MeDCMotor.h"( ~! a0 c0 e5 D1 V$ u5 F
- ; B+ n- U& K# L- d4 o. u8 _
- //double Input, Output ;
- ]' Y m# {/ C7 @: D0 w! O - float MaxSpeed = 255;' c7 Y$ d2 M# Q2 d- s p7 _
- float MaxPower = 180;
2 L5 Q3 r" T) d+ |0 B* n - float MinPower = 120;( X6 a3 C- m6 d7 J1 n
- float Error,ErrorAcc,ErrorDec;
$ E- v# D2 q3 X6 \& k, M# }0 W7 B - a5 i4 B* N7 i
- float Kp=0.14;
8 M& v- j% i* C! k/ r! N - float Kd=0.00020;//23;- e' P5 `' E! w0 f# l i: K
- float Ki=0.000201;
! \. N; O- \/ [ - % A9 N! I! u+ B2 ?+ q' ]
- float nPower;
: W Q$ O2 ~" b9 {( g - MePort lightsensor_6(6);5 v+ g* r, n7 W! l% \; H- I
- MePort lightsensor_8(8);& w2 _: F0 m* W
- MeDCMotor motor_9(9);5 Z0 s2 Q$ X. z( r
- MeDCMotor motor_10(10);
$ m& C- D% M9 @* m9 Y! M - unsigned long previousMillis = 0;
/ f! ^7 X j r1 C - const long interval = 1;
' P5 G0 _4 j: O. \; f3 k, r/ M - ( j2 t9 U G2 K6 U- `( N
- void setup(){: ~& F/ r$ s7 T# g" j0 f
- lightsensor_6.dWrite1(1);2 i8 O4 T$ |* N5 g
- nPower = 160;; }( M4 L; q* N- T( J X
- Error=0;0 i ?4 H! G- k5 x5 W. T# L% l
- ErrorAcc=0;
" ~ K1 `" N" o; @5 H - }8 s4 C. a5 g+ S3 r8 S
6 j+ @9 N: N1 o% `2 w# _! F- void loop(){- k1 C8 r) x" }9 R6 ^
- unsigned long StartTime = millis();
5 c8 ^; H+ @ R4 M; |5 e - if(ErrorAcc < 18000 && ErrorAcc > -18000){$ Z! [- u" r9 E, p% Y
- float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();4 U7 j' n) v w. I Y7 ?( K
- ErrorAcc += nError*Kd ;4 k1 j& U( v2 ~' Y
- ErrorDec -= nError*Ki ;8 Y. n) Q6 B- q- v, D. T
- Error = int(nError*Kp+ErrorAcc+ErrorDec);
8 G; M( @3 W! ~% L' Z6 W& f - if(nError < 80 && nError > -80){( H3 u9 @# z9 l: M$ i' k
- if(nPower < MaxPower){
/ ]- |/ e: f# o [6 a$ l; N' U, L - nPower += 3;
$ w0 B( t& `9 q5 [ - }
+ l- x# q: n7 M W( h7 y* a" m - } else{; q4 V2 _8 Q9 p( }) b
- if(nPower > MinPower){
. D9 q) Q0 \/ @# J0 v& J - nPower -= 2;
7 B8 @4 Z% [( _# N" B% x& d6 ] - }% f% R4 @0 R$ f+ F O- F
- }
$ F, a% d6 M4 m! C! b0 s- I+ s - MotoL(nPower-Error);, a: t. f' @) H9 L
- MotoR(nPower+Error);
9 {0 g5 L5 h# f - }else{
2 h# f# g0 \" ~* l8 ~ - motor_9.run(0);7 ]8 \' r! x& `! M7 f3 ~& x
- motor_10.run(0);4 E6 C1 t! y' b, H7 I6 q A
- }3 W, j- C8 A6 h4 U6 {- A
- do{}while(millis() - StartTime < interval);6 p! V+ C$ z& _9 y4 d P- r1 L2 p
- }5 J9 R+ V. L; v6 {
- - s+ A% I. q8 U, M! `# ~ S
- void MotoL(int Power){
* y) T6 t0 L3 p ?# ? - if (Power > MaxSpeed){
% k8 P* D @+ J- T3 q - Power = MaxSpeed;- }8 q& e2 W7 p0 y2 B4 K( q
- }
$ A. d( q! _4 l% }$ b. D - if (Power < -MaxSpeed){: c) V6 ?6 }8 O5 }5 m4 d
- Power = -MaxSpeed;; f- R/ R0 P7 m& K" A1 ~
- }
0 F0 p4 c0 d& s n* c4 D0 Q9 X - motor_9.run(Power);
5 x$ t4 |: o5 {$ w) {$ `" w - } % ~. o, ~1 l t2 a9 P
5 v/ l. ]: g! W: u- void MotoR(int Power){
3 x3 r e- s& V6 X0 r- g - if (Power > MaxSpeed){ G" v( C$ C. y
- Power = MaxSpeed;
a+ Q! Q) X, `! ^2 j! G - }
' G! N. _) N4 _: D/ v - if (Power < -MaxSpeed){$ G. ?6 U1 w9 D9 U9 V7 |' J
- Power = -MaxSpeed;
6 ]4 p" w0 ]7 B# L - }
3 h2 l4 g4 N. ]) p - motor_10.run(Power);
, z9 |/ ~8 o9 k - }
複製代碼
+ R' _8 e# u. r5 y G1 E, _, \6 |( Z, \& M, W$ e9 ]# i3 G
|
|