|
|
- #include <Arduino.h>
4 m5 ~% ]$ {2 Z/ c - #include <Wire.h>
1 f5 W( f! k# U1 A$ z$ W _- c - #include <Servo.h>
, S0 i$ Y# T7 k( o% A
1 \* {0 g! T/ g7 i- #include "MePort.h"- V. {) e+ A! J/ C
- #include "MeUltrasonic.h"
% K* S+ X! U; g% @2 Z - #include "MeDCMotor.h"
- ]4 d; u4 D `7 M
# ] F- v7 E5 k# b4 E( U. G- //double Input, Output ;
! M$ v$ U) V5 Q% q- V4 R - float MaxSpeed = 255;9 V8 f: c9 \) n& a2 Z. Z) u
- float MaxPower = 180;
1 N/ i' F0 B( ^! s - float MinPower = 120;/ S3 k, J G: j
- float Error,ErrorAcc,ErrorDec;
8 u6 @7 p+ C6 |) b: p4 T! L
# w: R7 B" |$ \- float Kp=0.14;' U2 B- T. P- @
- float Kd=0.00020;//23;
8 P; A( B8 O+ d6 Q9 S' I, n1 y - float Ki=0.000201;
1 k/ F1 c# z! Y8 \ - / f3 ~8 @1 Z: |- R" S. o
- float nPower;% y0 v8 h M3 r0 f/ }
- MePort lightsensor_6(6);; V& y6 t) F9 a. @# N
- MePort lightsensor_8(8);
6 r0 M7 \, H* v7 @8 x. ] - MeDCMotor motor_9(9);4 ]* s2 a6 z! Z$ L; T& S! J% T
- MeDCMotor motor_10(10);7 B% d4 P" C- S3 o) G1 @
- unsigned long previousMillis = 0;
8 j, t9 e7 c+ R0 l& ?4 W - const long interval = 1;
. _& G; G+ K# H& L8 ]# B4 }
" F1 N7 s. p# t& N& Y9 y! M: y- void setup(){
1 P( N: k) l; \ - lightsensor_6.dWrite1(1);
; D% g$ F( u/ [# \# @* R% }, [6 D/ H - nPower = 160;
6 _* [* r1 X8 I, ^+ m - Error=0;
$ e& B3 Y$ g3 J& h" R - ErrorAcc=0;
# r$ |' G) k* m# s ` m8 \ - }# k" v; ]- P6 b0 s9 y" N
! R* C' Q' `) D' a6 P$ y- void loop(){
) Y+ w% C' `5 K3 Q3 ]5 K- l - unsigned long StartTime = millis();6 K U0 ~% G" k' V6 `( s( @3 C, q
- if(ErrorAcc < 18000 && ErrorAcc > -18000){
/ B: y O7 s- l& ~& T: r9 _; r. w - float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
2 |, U/ X0 F; c5 N - ErrorAcc += nError*Kd ;7 N. w: b) y# b F
- ErrorDec -= nError*Ki ;/ @! Z% k% s! L/ n1 M4 `3 |
- Error = int(nError*Kp+ErrorAcc+ErrorDec);. I' r x6 W3 M& h' h8 l/ V& @4 y
- if(nError < 80 && nError > -80){3 X$ g1 e& X: K- b
- if(nPower < MaxPower){, E$ D" Y" X' E! i% l+ q5 K
- nPower += 3;
4 g1 Z: W! h" N }, W( s7 f5 k! } - }9 {) o; n# G3 G' J
- } else{
& m- Z$ ]4 Z, V/ N4 _- y - if(nPower > MinPower){
2 g5 T; f: |: t' o - nPower -= 2;! F( M4 H4 p% }% M1 t( G: b& S2 p1 e
- }
: y. ^) n. R# F4 n; i - }
% |7 R. j, n8 Z. z - MotoL(nPower-Error);+ i o# k; e* |* l
- MotoR(nPower+Error); , ~6 J0 i7 O: ?8 Y9 o
- }else{3 C; h# R3 u2 X3 Q" Q" V
- motor_9.run(0);
+ k0 Q" m5 t L - motor_10.run(0);
7 |6 c- w- y# \- ]) F - } o: y: ~) I$ |* c6 J
- do{}while(millis() - StartTime < interval);
A4 `' l# i" @ - }0 P k! G( z" ]8 {' c
- - z0 p0 i# m- G5 l% Q
- void MotoL(int Power){
7 p [' X; r/ F' H4 P - if (Power > MaxSpeed){' B9 n6 A9 y+ A+ v4 e( O
- Power = MaxSpeed;2 U! o% Q# ?- M+ J1 ]/ ^
- }
! G$ W& p; a# P# |9 ?8 u - if (Power < -MaxSpeed){
7 |* W& j# y6 ~+ R - Power = -MaxSpeed;
; _* a. `4 t' l' |4 O7 j - }
9 m/ C7 e) H6 I. [ - motor_9.run(Power); \7 R8 v+ r. e7 {6 O. }1 f% R
- } B. ^; V3 g# }- ~# W% S0 y
+ V; v+ u B8 D* k: a$ F1 ^* L- void MotoR(int Power){
) C {5 r# u* r! e6 h; l+ C - if (Power > MaxSpeed){+ @1 x/ H7 Q7 S0 ^
- Power = MaxSpeed;/ z4 }6 L; f0 O3 p3 W( M$ Y# j
- } _( t& I, k; F5 _
- if (Power < -MaxSpeed){
% M: Y1 z) v8 m7 Y, y4 b. y) v9 h4 ^ - Power = -MaxSpeed; c! h* g; ^( t
- } - P% \$ \9 C6 |9 _% w4 U0 u( y
- motor_10.run(Power);# X6 l2 e( K, z+ [! d0 k
- }
複製代碼 . h' b5 w2 ^8 T( g% j) }
) y4 O7 M/ g- a2 I" j |
|