|
|
- #include <Arduino.h>
: E, B" ^, g a' b/ P - #include <Wire.h>
9 l" s7 z" F; U$ r7 d - #include <Servo.h>
# A# U9 w3 a6 L% P, ]1 T - " Q& ]% i* T! P4 H1 W5 x
- #include "MePort.h"2 c8 _/ z6 y, F2 m2 m o: X
- #include "MeUltrasonic.h"
_; D% b1 T' a - #include "MeDCMotor.h"
$ _. U5 S. P+ |" F6 o8 y3 O - % A& D7 z7 z# x% w; h& d0 d
- //double Input, Output ;
' q0 L3 ?0 X, K9 C6 Q/ T - float MaxSpeed = 255;8 l; c4 x. T. v7 w% C% N; q* i
- float MaxPower = 180;
, ^. Y3 `+ M. U, v - float MinPower = 120;
8 |4 `: _+ g% O) c% B - float Error,ErrorAcc,ErrorDec;
2 F# ?, l1 ?" I% k$ a9 |
, _- P' x; V" i6 l* T _. }- float Kp=0.14;
' f$ N- j2 W5 n* C1 b5 L/ B - float Kd=0.00020;//23;, i, U, M2 _! u8 }# [9 \: D
- float Ki=0.000201;6 @7 R9 v$ ]; x4 b* X
- A( @9 M, O( B/ y4 L- float nPower;' J" |2 _. j/ _5 M1 {$ B
- MePort lightsensor_6(6);" W2 v5 e$ w4 G
- MePort lightsensor_8(8);
; q* X. g0 V) G - MeDCMotor motor_9(9);
5 X0 Z- ?6 F5 ^7 x+ B O - MeDCMotor motor_10(10);& }. H7 E0 g6 N' X# p
- unsigned long previousMillis = 0;
% y& P( ]1 V3 Y" W, K: x6 _ - const long interval = 1;
4 P; }8 X2 T/ i9 v
: O5 y1 r5 n) D- void setup(){& W5 l e( B, C7 `, X
- lightsensor_6.dWrite1(1);/ R3 V6 s% @1 ?/ f/ m2 n. T
- nPower = 160;
; Z7 w3 Z% _% F: x - Error=0;1 K& b6 b6 l5 t( w4 ~
- ErrorAcc=0;
5 `6 T8 \6 i) x3 T - }
0 X4 K4 G3 ]' t0 `" [3 F) k3 e
7 c3 F( ~; o' o+ Z# x7 A- void loop(){' P9 a, C" G; y: l$ ^
- unsigned long StartTime = millis();
9 t" B8 M+ |8 i! F7 X - if(ErrorAcc < 18000 && ErrorAcc > -18000){
0 S, g& z. p2 [! k8 _3 p1 b, R - float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();1 Q, R1 J9 h/ V& G" t V u# L
- ErrorAcc += nError*Kd ;1 S. X9 G& Q( h' W, U
- ErrorDec -= nError*Ki ;
0 ]* u+ C% {2 L+ ], j1 E - Error = int(nError*Kp+ErrorAcc+ErrorDec);. W1 x6 J0 h0 v8 B0 Z1 @! E7 I0 t
- if(nError < 80 && nError > -80){
0 Q. p O% ]. I$ r - if(nPower < MaxPower){
2 s8 G3 R# m) v# f& A - nPower += 3;
; h7 u! ?6 {" r/ c$ ]& @7 p - }
& S9 X/ ?: M4 S- C - } else{
& N2 {& k" P' L: V) w+ n2 N - if(nPower > MinPower){
1 a) |* d5 m! u# J - nPower -= 2;$ [! I) L$ D- h' s2 N$ b
- }
: B- J+ u2 f- E - } ) F2 N- k3 }+ Q5 _' K( W: @9 L
- MotoL(nPower-Error);/ {0 @* g& O w, b7 d
- MotoR(nPower+Error); ' h# z @ R3 I, j/ o( k
- }else{
( j2 f* ?+ E" Z' w+ [* O) J" P - motor_9.run(0);! o9 w3 b4 t- V3 U7 d$ w" e( m
- motor_10.run(0);- N8 `% ?. w4 Y' ]; Y4 [& E
- }
. f' G: \+ Y4 X6 m( f; s% s% S - do{}while(millis() - StartTime < interval);
9 @6 k7 e1 t: o7 E* ~( \ - }- R5 f$ p9 y! Q+ u1 X
- a7 u8 U) K8 ^/ n
- void MotoL(int Power){0 b" L7 c1 Z+ E$ m
- if (Power > MaxSpeed){
6 ~$ s# o7 n6 I0 n5 W/ b' ~ - Power = MaxSpeed;: N3 T) m: P+ e1 A
- } " f! h3 n, q/ i0 Z
- if (Power < -MaxSpeed){
$ J! {8 K! C' w2 r. j$ E0 V - Power = -MaxSpeed;
0 c, T$ G8 {: Y( ^: w8 D - } ! [* O' @- a/ D8 h W" p/ X6 l
- motor_9.run(Power);
4 t l! i4 }3 X3 ~! v+ Y - } : N- I* e G0 [! l
; M4 l$ \! t% q' t8 S- l+ I- void MotoR(int Power){
% u7 A& l" q- j8 g# Y# U - if (Power > MaxSpeed){7 g! t& H( X+ ~! n6 Q
- Power = MaxSpeed;8 r- e8 C* D( @6 Q. x5 H
- } 8 W6 o1 f% K0 Y K7 I# m* z
- if (Power < -MaxSpeed){5 W7 J; r) H# j
- Power = -MaxSpeed;
* J. I* M4 |& O: i- I( i - } 8 Y+ b9 @/ K$ {
- motor_10.run(Power);
" E( {& b9 x+ r$ I& q! T+ d - }
複製代碼 ! Z/ K- X+ j/ m0 ~3 y2 y
) i! q' F6 e8 Y S& r. m6 U |
|