|
|
- #include <Arduino.h>
# b$ T" D% }+ T0 v' r4 O- i - #include <Wire.h>& s# ^' M1 N/ B+ S5 v% M& X0 u) }9 M
- #include <Servo.h>" C8 k& }6 \* y: {
- 8 x4 q9 e6 T# ^7 x
- #include "MePort.h"& h x/ q. q2 b8 L
- #include "MeUltrasonic.h"
3 v9 y% z$ V0 Y" j6 S/ a - #include "MeDCMotor.h"- z' Z' B% p4 C* U
6 i0 u- H) R# A, l- //double Input, Output ;/ w* b W8 ^+ H9 B
- float MaxSpeed = 255;* a8 Q; G/ e) `9 c2 m
- float MaxPower = 180;
0 {6 L& V1 v8 ~* ^ - float MinPower = 120;8 d' @$ X9 n. q; [2 y* x( F; ^
- float Error,ErrorAcc,ErrorDec;+ a0 C6 b6 \' M7 n: B4 Q) w8 b) |
& f8 i9 [. p$ y- float Kp=0.14;& ?) v- m" z+ Z: }7 W
- float Kd=0.00020;//23;6 ?* ]) ^% a; [
- float Ki=0.000201;2 g* V4 {7 ^& j' k* m6 s0 h& E
- o8 X W+ b# A. M- g% r& U
- float nPower;
, G& B( C- B/ j! d- [ d - MePort lightsensor_6(6);! S1 [0 W% z0 J- C3 u2 n/ q6 K
- MePort lightsensor_8(8);9 q. z9 f1 E- E3 U" \
- MeDCMotor motor_9(9);, q# l, {3 Y. [9 P) |5 Q6 g2 Q" J7 A( f+ ^
- MeDCMotor motor_10(10); W- ?7 R5 f% ~* q
- unsigned long previousMillis = 0;
' ]4 s7 W( `# [! ^. { - const long interval = 1;
9 x$ Y+ @8 ]# [' r; v8 |
" _" d& u7 p0 y4 l- void setup(){8 d& A0 w1 e! |5 n2 Z% ^
- lightsensor_6.dWrite1(1);
, A7 U$ h9 x i" w - nPower = 160;! |. c7 D2 l2 o$ N: q
- Error=0;
+ N5 l/ \- k3 K2 ~; j' } - ErrorAcc=0;
7 L1 }0 c. L* A6 o) B! ^; S% x- e - }
! O$ s' o! d( ?# L" @ - 7 m! K6 }4 V! F$ e9 P7 l0 |6 }7 [- x
- void loop(){
$ [3 n. Y5 E/ N - unsigned long StartTime = millis();
$ h" J2 P; E1 o9 {0 r+ s - if(ErrorAcc < 18000 && ErrorAcc > -18000){
, P2 B( x5 u' N( p1 y3 h - float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
- N4 w( N1 \4 H: J9 ~ - ErrorAcc += nError*Kd ;4 T, m: C" ]' P1 j
- ErrorDec -= nError*Ki ;
" [% n( r; o8 t; `. Y: Y - Error = int(nError*Kp+ErrorAcc+ErrorDec);6 ?& T5 Q+ g0 I5 I
- if(nError < 80 && nError > -80){; w- D" O; l8 z
- if(nPower < MaxPower){9 `9 \& E' t3 U8 i0 R9 L& H2 c" V
- nPower += 3;" B; K2 J$ n% r6 W
- }
/ v- c2 u. j# K* P6 F$ a p; u - } else{
6 `' R+ \( k. x. y$ ]5 S - if(nPower > MinPower){
' B/ p9 `) ^$ i* w+ G- e' u - nPower -= 2;6 X; D. s4 j* g: L f( i
- }5 R' c5 P3 h& q! q& c' ~6 |# C) r
- } 2 d" O5 @0 n0 _# O. t
- MotoL(nPower-Error);
& V2 c$ X6 n5 u - MotoR(nPower+Error);
% G5 A* C; R1 S4 p, b9 h9 \ - }else{
. o e4 ]- p3 N* W7 c - motor_9.run(0);/ Z! S+ d) G, I' K q4 n$ L
- motor_10.run(0);0 r5 U+ d3 J1 z3 m, Z
- }5 M) m" e; r4 E7 E% K0 D
- do{}while(millis() - StartTime < interval);
2 K- N0 K j k( i6 V - }
9 d5 @: p" _0 G* R9 _/ R
# ~; y" v+ L9 O% c+ N* v- void MotoL(int Power){- ^9 G, w' p& O
- if (Power > MaxSpeed){; R: N! H1 A. U- y) V* |
- Power = MaxSpeed;
' F I; u- {3 B" Q" L - }
4 y& c( S1 ]0 x. s - if (Power < -MaxSpeed){
" |; X: N* Y- a m$ }: C0 ]$ A- U - Power = -MaxSpeed;
, Z; o& O1 w' M/ t8 S3 h T - }
" O# @* l# ^( g/ a - motor_9.run(Power);
4 l6 c' ?1 \' Y0 |" }) g - }
! J; r; A* k* O: F- l7 @) [ - 4 U9 d3 [. [ t4 d: C, P& _2 {
- void MotoR(int Power){/ J( Q6 ~; C4 u/ C, B1 W
- if (Power > MaxSpeed){% f9 h; ]: ]4 B9 x+ k4 n' s
- Power = MaxSpeed;6 z2 [+ [* s% G& J
- }
( v. V P+ E! Z# T G: h" j - if (Power < -MaxSpeed){1 z* c. I2 p. d" Y- G
- Power = -MaxSpeed;
# @* O! J' ~7 e - } + C9 q" C) E1 `1 s8 E" Q( f
- motor_10.run(Power);
: i& a) y/ ]7 |3 r7 @3 F3 L9 K4 I - }
複製代碼 3 O* J( h1 t) H7 e: D( V' \! L, G
7 R# J( a. U7 J, ]) | t* W" m |
|