|
|
- #include <Arduino.h>
: D& Z: ?0 Y0 O- Q4 j2 Q - #include <Wire.h>
2 b$ U* y# U1 `4 U C - #include <Servo.h>- U$ s: \7 f9 h; H' H# m1 j
- 0 o7 Y `* ^: g1 s( k
- #include "MePort.h"
5 D+ i* Q. N5 ~( X) n* n: j - #include "MeUltrasonic.h"
2 |' n H0 _) k" R4 \' N, ?1 p - #include "MeDCMotor.h"
& a+ F2 m" K$ S6 z1 g
& B; D- |& d' a$ Z) b! G- H- //double Input, Output ;( [. ~ N/ Y# O9 z
- float MaxSpeed = 255;
2 G' n! v# r: d2 I# v - float MaxPower = 180;
& e/ |1 p( B" g, Q5 _ - float MinPower = 120;" x+ k: {% J( E5 b! c# c" N7 N
- float Error,ErrorAcc,ErrorDec;2 o* [ Q; y5 ^. ~8 h
- - P2 A5 d+ g \+ G% W' N- X! z
- float Kp=0.14;$ M# v8 U, u' ?
- float Kd=0.00020;//23; [1 B6 q& y9 y. ?9 z: V4 }
- float Ki=0.000201;+ b- L- U* N& M; c0 R( J
- ' F' s2 N8 l5 K. Z
- float nPower;
$ S/ L% x; L( d6 g1 P - MePort lightsensor_6(6);
# `. q% g: F* S" c* Q3 { - MePort lightsensor_8(8);- M R- k7 m: }5 z, R
- MeDCMotor motor_9(9);8 I3 l4 [. ` h- b2 [
- MeDCMotor motor_10(10);
9 T7 z3 z; ^- {- I! S: k - unsigned long previousMillis = 0;* K2 _' O$ |" p- z8 K
- const long interval = 1;
$ t0 B& Z6 d- t - $ t' w2 b. D' v) E
- void setup(){% p. y: j( D5 y$ U: l
- lightsensor_6.dWrite1(1);
$ w/ Y$ w$ ]; |$ L- I$ F: ?, X - nPower = 160;
/ Y2 D# U! J. k, {6 f - Error=0;
/ G" Z. d5 G/ H: R - ErrorAcc=0;
! T+ G3 H9 H. {) P# g( {% n* `( T. ?+ H - }
\, N% A% }( o+ I
1 ~9 L3 p4 L5 s$ F4 W1 ^- void loop(){+ g& D, \$ V: M1 v! b' A4 g7 S
- unsigned long StartTime = millis();
% e) \4 e8 H+ S3 u \9 { - if(ErrorAcc < 18000 && ErrorAcc > -18000){
/ D7 k5 F3 i J - float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
2 M4 s9 _- P: L2 D5 } - ErrorAcc += nError*Kd ;
6 B, F# X! T# K. ]2 A/ j5 w2 Q1 { - ErrorDec -= nError*Ki ;% W8 g8 J/ f' O
- Error = int(nError*Kp+ErrorAcc+ErrorDec);, a y, v- v7 J: J+ @
- if(nError < 80 && nError > -80){
2 A+ n) i2 f0 T - if(nPower < MaxPower){
: X/ Z, ?& W/ A3 w - nPower += 3;$ I3 B3 }$ q; z( b6 V( V) B4 m: f
- }: k' Z9 o' Y( ?3 Q$ W! }! @
- } else{- K8 D; p& x5 \* d F
- if(nPower > MinPower){
6 S, h7 a* N* ^( Y L3 J3 w - nPower -= 2;7 N9 g" f$ ^& ]$ W/ N. G! k
- }
7 m) s0 E/ w! x - }
* _3 C& A/ O& V p) }3 s - MotoL(nPower-Error);
, O1 A" T" q3 m% p5 D P - MotoR(nPower+Error); # t$ l% @6 e- a% d& z, M
- }else{2 z% t S7 J9 j, a0 d: v7 e7 h
- motor_9.run(0);) R9 A; t& f, J* d# W4 b! i
- motor_10.run(0);" e5 a9 t/ V, V) N* K$ T
- } A4 j" V8 [. M e$ I; d! `% y
- do{}while(millis() - StartTime < interval);3 `2 e4 u5 A2 h0 X, r
- }- {1 W; ~$ ?8 t4 [% b) V0 G
- + I$ R9 z+ C3 ~& g7 c
- void MotoL(int Power){" m2 A) m0 q1 q
- if (Power > MaxSpeed){
, ` d- _0 {9 `. J2 g& q9 L - Power = MaxSpeed;! A% N. k% r( }. N4 f
- } 7 ^ `: o d/ S" p# P
- if (Power < -MaxSpeed){
# {+ X; G# C& w( Y: r% N( m$ y - Power = -MaxSpeed;/ A- z4 T- b8 C, _! |
- } 4 u& d9 H- y& ^3 _
- motor_9.run(Power);" v$ V* d! g$ c, M" u1 b
- }
) g) Z& V! A' u2 d# ]" _; I
1 C t- B4 d8 s _1 S5 S+ F- void MotoR(int Power){
1 F. q$ b! E) N- F$ R5 ? - if (Power > MaxSpeed){
9 {6 s5 V; s. [$ M& u9 i - Power = MaxSpeed;
9 b& `- R8 L3 c- a! i - } 0 Q- i# c, @! x* ]+ f: ] B. l
- if (Power < -MaxSpeed){4 }! n1 ^5 R! y/ O0 b: M/ C q
- Power = -MaxSpeed; H/ c# D, c* s0 d1 x# w( e* e
- }
: f! f9 t/ N, Z0 N$ X0 M; { - motor_10.run(Power);. P) Z" j0 o7 F* g& I( w
- }
複製代碼 3 z, A$ a: n! ~2 Y1 p! w+ r
- C; `- ^3 g" M! [# ? |
|