|
|
- #include <Arduino.h>
6 \9 D4 G8 G. O) O' o6 g3 F - #include <Wire.h>
) M1 s- O/ u, L - #include <Servo.h>
5 s2 w k# L3 @ x% n8 A2 b5 b
8 w# {: \+ F; \8 E- _) L- #include "MePort.h"
- F. w* w/ A7 {+ H: F/ w) H - #include "MeUltrasonic.h"& m0 k- k* u4 u3 o
- #include "MeDCMotor.h"
; i. Y/ G, R) T. @ - # l: f# g, Z" d
- //double Input, Output ;& e* V4 L0 g, y; F. b
- float MaxSpeed = 255;
; V7 Y1 ]# ~+ k5 ? - float MaxPower = 180;
" V/ Q5 s& f+ @' w0 s! U - float MinPower = 120; I% [ w" J/ u( K5 s+ u4 m* B8 q
- float Error,ErrorAcc,ErrorDec;
5 x, Y2 m# S. c/ G- i
- N2 A& k9 V, [% R# O- float Kp=0.14;
c9 a: @. A2 h6 ?6 D7 ~+ G - float Kd=0.00020;//23;6 x: F' F% c" L' v/ U0 ]9 h% e6 E5 K
- float Ki=0.000201;: j) d) @( U# ]" | T% S+ U5 u4 k% s
4 V( r/ {5 _: x$ ~) _4 Z( C- float nPower;
2 V4 ~ m K: \4 _3 U - MePort lightsensor_6(6);
3 U B6 C' T4 g5 E( b+ o - MePort lightsensor_8(8);$ W, m7 C# l; f% ]
- MeDCMotor motor_9(9);
7 W; V' T& C, k: v* x# ~ _ - MeDCMotor motor_10(10);$ H1 X6 H# |9 Q4 z: Y9 P
- unsigned long previousMillis = 0;
) y5 S6 l% q: B* C, Y; ]$ ^4 E - const long interval = 1;' e' H" m% R1 K+ t0 d2 \
' C* P' i" l2 E' _% t, B# D* q0 N- ]4 O- void setup(){
8 ^; F: }; i" u8 l: u - lightsensor_6.dWrite1(1);
: E9 X1 g' S9 s6 l# X/ E1 Q* G5 B+ ?. ` - nPower = 160;
0 D H! {/ A- ~ - Error=0;
% E: O$ z9 T; I- x( b, { - ErrorAcc=0;
3 s7 U6 c% Y, J4 v - }
% j, A( r2 w# L: U& `7 f! o - : H2 V' ?: J* j1 B# t/ c
- void loop(){" {3 [, N+ A% s D) ^2 T
- unsigned long StartTime = millis();
& i* }1 ]1 b2 A1 [) S* c* K - if(ErrorAcc < 18000 && ErrorAcc > -18000){$ l+ K8 F' [( w& D4 {/ q! y; j
- float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();1 }) Y7 ]7 L/ l p/ ?; B
- ErrorAcc += nError*Kd ;5 X4 A# K- ]! P; i0 V7 H
- ErrorDec -= nError*Ki ;8 _7 n7 L+ l/ b5 l$ o
- Error = int(nError*Kp+ErrorAcc+ErrorDec);
/ B; y+ w) S; g! v - if(nError < 80 && nError > -80){" m: C: V% I7 r/ N
- if(nPower < MaxPower){9 V% F1 d9 H$ L8 g
- nPower += 3;
1 f$ s9 h& y4 s; L0 d- y7 a - }
! s% u/ z( S2 `9 F U$ `, o - } else{
5 d" |! \& d: r' K6 ^5 e - if(nPower > MinPower){. \8 R8 M m$ n# H3 ~: R9 w
- nPower -= 2;
0 U# i+ K" Y& t1 R' ? - }+ T1 c4 B1 H4 L
- } / x5 l: ]1 p5 \/ P* l" Y
- MotoL(nPower-Error);
9 D4 e$ B D; B# G4 E: H - MotoR(nPower+Error); 3 H; J+ Y! i+ |. `0 U3 R( {
- }else{
4 A8 O1 E9 P2 J' t% B) I: l - motor_9.run(0);# H3 u+ p! D _0 o( V
- motor_10.run(0);
; d" N- V" X6 o( C) Q! O) M - }* r; o) }! P0 v
- do{}while(millis() - StartTime < interval);
( u2 ~" l @# e' P' R! Y+ \% I. K - }
0 [- g% l/ {, [* A f
5 u0 ^( R9 B7 h1 W- S/ j) q- void MotoL(int Power){
3 S+ j' y$ ? f4 t- R - if (Power > MaxSpeed){
/ n! O/ ^1 G( ^3 r$ R0 o - Power = MaxSpeed;4 q* o6 N/ Y* x: i
- } " w& o+ u2 b0 ]5 H
- if (Power < -MaxSpeed){8 \3 D: o! N6 I6 z4 [2 k9 T' Y
- Power = -MaxSpeed;
' f& s! R- H4 ]; B( G3 G3 S; z - } 5 G- H4 x/ }7 u" y9 Y
- motor_9.run(Power);! g* Q" \- g: `* J) a* b9 Y
- } 8 g9 P1 E' u h$ K/ E
" X; _! Z; \) Y5 O6 e/ t- void MotoR(int Power){
. @3 h* C% y* W- p" d. V: Q# b - if (Power > MaxSpeed){
. h6 _+ o; @# H - Power = MaxSpeed;) N$ c3 B, A& M- f- d8 u
- }
8 _. i# L5 m, a" K+ m' { - if (Power < -MaxSpeed){
" o! c$ W0 ^7 Z4 U - Power = -MaxSpeed;
, `: |7 J0 F- p: p5 n4 \. v - } 4 m% m2 M# l9 w7 H8 C) j, t t
- motor_10.run(Power);$ q# z* u8 t5 }* z
- }
複製代碼 d0 ?0 m6 ]- g% M/ K" G9 \
; ^' W9 Y! C5 D' o
|
|