|
|
- #include <Arduino.h>
' e2 m- ^* p3 Q1 ? - #include <Wire.h>
1 q6 | x! S- i1 p6 M# t' N+ I9 p; { - #include <Servo.h>
9 f4 _8 r1 H# V4 Y! b- G5 p
! Z w3 A! V# y2 i+ [- #include "MePort.h"
% A% W& l/ h6 L5 ]0 w - #include "MeUltrasonic.h"
5 ]1 o8 e, k, A5 H$ C" P% } - #include "MeDCMotor.h"
" J6 i5 a" i& g - , C# I; [0 D3 i3 }& ~: x. i2 e
- //double Input, Output ;
( ? D% H1 m" c: V, Y; M+ ^ - float MaxSpeed = 255; t- c7 t6 F" g
- float MaxPower = 180;
7 \" }9 C. f9 S, g! B - float MinPower = 120;/ {* j8 @* U& m
- float Error,ErrorAcc,ErrorDec;
2 t# K+ Z& ?* f% s3 ~ - 2 p$ V4 W( u2 T& ]' R
- float Kp=0.14;% Q0 ~9 [3 E( U) T3 k7 o# H
- float Kd=0.00020;//23;; m1 y/ m7 T7 \/ f+ t. J
- float Ki=0.000201;# O8 D( \5 U3 {% k$ `, A+ N
- ; @ O) L& M Z) o8 ^
- float nPower;
1 @5 X8 J+ X* Y& N - MePort lightsensor_6(6);( t9 Z* ]* t" x v9 j
- MePort lightsensor_8(8);- j! z* Y* J3 C
- MeDCMotor motor_9(9);
' _2 ^, @- M! V - MeDCMotor motor_10(10);/ v# G3 j' R6 M8 H( q# G z1 N5 c( e
- unsigned long previousMillis = 0;* j$ ]- D8 m2 l( c9 j2 `
- const long interval = 1;
# Q! k1 J. k! B - 5 u2 B' a: h/ I3 n8 Z a8 ^; F6 u
- void setup(){+ @& k# i, [# c- c; c
- lightsensor_6.dWrite1(1);
& x5 b8 H9 z4 ~8 ~' ` - nPower = 160;* K( }+ {$ [" `/ }% H2 `
- Error=0;
! B; M' p% n a6 i( H - ErrorAcc=0;$ M( R, q+ Z4 u% D) z% D$ w
- }
" c- s% l. Q0 k - . i$ R; M' s. `; h' A0 y2 _1 t% a
- void loop(){5 \% K2 C# m7 A- L$ P# h; t* ^
- unsigned long StartTime = millis();
' f$ T$ Z. T) V# h. s1 @5 g - if(ErrorAcc < 18000 && ErrorAcc > -18000){5 ]" ~8 L2 y6 g/ J1 a
- float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
) [% C3 Z0 D, q2 o4 ?. L - ErrorAcc += nError*Kd ;- o- k# [; D& a# M
- ErrorDec -= nError*Ki ;
# q) F7 J0 {: |$ M - Error = int(nError*Kp+ErrorAcc+ErrorDec);
- b# ^9 W! a+ s4 B - if(nError < 80 && nError > -80){0 n0 W3 a/ ~" G( O- E! J
- if(nPower < MaxPower){2 w9 ]" _ f7 F1 S
- nPower += 3;, l! m4 m6 O4 a7 n( j# n t
- }& f1 t7 L* J6 v0 Z. L
- } else{
* X& N2 F' h4 N6 a6 O - if(nPower > MinPower){& q7 R. y1 m$ `& D
- nPower -= 2;
# O2 G# I7 v T; [ - }2 T; K7 z& Z$ @. A! |" R
- } * F: P+ E, ^; |2 Y; R8 Z- P; A6 [
- MotoL(nPower-Error); {+ a+ t) X H) d" ]+ L, Y
- MotoR(nPower+Error); 5 C, Z& a- {2 Q( A
- }else{1 Q$ l4 H& t. E, D- a$ E+ ?
- motor_9.run(0); n: H R7 p0 ?
- motor_10.run(0);/ C- ^5 H& Y! g5 f* G# Q& _
- }) H! K- [3 r) H. M, t
- do{}while(millis() - StartTime < interval); i7 Y/ _0 g1 X2 s6 a8 Z& l
- }
5 M3 b/ L( q2 h Q, h - - c. g9 m! `6 s7 z/ o( @7 c
- void MotoL(int Power){% I. F+ P/ M4 U$ `2 N$ M8 V
- if (Power > MaxSpeed){
% X* }) e/ x( v5 `$ n- t - Power = MaxSpeed;7 @8 x! n# b+ N( [9 a0 I
- } 7 c' S. K2 m3 Z
- if (Power < -MaxSpeed){9 A `# X. s, @: Z( a
- Power = -MaxSpeed;1 _, H: R: K+ Y0 a
- }
9 n: L' w$ w8 M4 r) \ - motor_9.run(Power);
9 K& L4 `% K2 u7 \, A4 F - } 1 @. {- y- |& m- {/ B+ W9 q
. [' A" E# [1 c0 o. h5 s1 {( u- void MotoR(int Power){" T6 m* _ Y* v5 p
- if (Power > MaxSpeed){4 ]9 M+ X. s0 ]
- Power = MaxSpeed;
0 ?5 A; t5 W' e - } 8 Y# a, N4 D# _& L0 P) T. d; y2 w2 m
- if (Power < -MaxSpeed){& f5 j) g; _( F1 q- p O
- Power = -MaxSpeed;
- ~6 R0 }$ d$ P$ R! c1 L - } " s: Q2 ]& m- R
- motor_10.run(Power); p: b+ ?/ Q2 c7 e
- }
複製代碼 - B Q: X1 m5 B ~, j% B: M
- W- F7 Z; [+ Y! i
|
|