|
|
- #include <Arduino.h>9 u) b- ~ y# c6 f/ x
- #include <Wire.h>8 u9 j) o; C F1 z! C
- #include <Servo.h>
% |% g' K# f* g+ S# m5 u- e - ) Z8 O) q1 H: O% n( o* ` A
- #include "MePort.h"
. y+ Q% C1 [8 A+ e( z9 Y - #include "MeUltrasonic.h"
- y) e, ~( T+ ~5 C4 I( r - #include "MeDCMotor.h"% S) n5 ~+ q J6 I: @0 s3 n
- 3 r( I2 d4 g* z
- //double Input, Output ;5 l- S0 s$ w6 }: w( E5 q
- float MaxSpeed = 255;
2 j/ [( A& Y7 m. S- G9 v - float MaxPower = 180;
! f6 V3 e/ W/ U; r1 ? - float MinPower = 120;4 q+ k+ x& ^' c1 b0 M- |% R
- float Error,ErrorAcc,ErrorDec;
2 B' O4 f5 k7 W; N d- D! o9 i! l
0 p' U l; \6 d7 y7 [, T- float Kp=0.14;% Z0 Z8 p. {, {4 J! t9 ~: c9 ? L
- float Kd=0.00020;//23;
! f$ B/ V, L* Y5 A+ \( _ - float Ki=0.000201;
9 s7 X* u8 A5 b Q2 Q0 O/ l
9 S! L! a( f! ~7 c# q+ y! [- float nPower;
' e$ ]' Y \& f/ ~" {$ l' @7 `* W - MePort lightsensor_6(6);
% H0 ^! }' b2 ?9 a& Y8 e - MePort lightsensor_8(8);, O; o7 q6 i8 |; ]9 q( ^
- MeDCMotor motor_9(9);
Y1 G! }7 o2 y1 e& U( { V% O - MeDCMotor motor_10(10);
0 w) W3 y3 L4 H4 X; q - unsigned long previousMillis = 0;
1 q+ b/ d( {) [0 q - const long interval = 1;- P/ T% v. t% F2 v
- , M8 p, K4 e, v; l$ L3 M( b$ v
- void setup(){
9 Q0 l: y! g( Q3 ~! ] - lightsensor_6.dWrite1(1);. v- N& G/ y' ~5 T9 V' Z
- nPower = 160;, R% E ~. E. g
- Error=0;
V1 h! R% P6 u - ErrorAcc=0;9 [4 q, r, d) P/ o9 {; f/ [
- }
6 B) R5 l% n! a& L/ Z8 ] - 3 _, N- {, T) p6 k
- void loop(){4 X- W$ `$ S: E
- unsigned long StartTime = millis();, z7 U. ?4 P% n5 E
- if(ErrorAcc < 18000 && ErrorAcc > -18000){
8 s( G$ O+ j. O3 I0 b - float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
) y P7 Y) ]. {* J - ErrorAcc += nError*Kd ;
/ u6 x( t, @# o3 Z - ErrorDec -= nError*Ki ;% D: R0 ]4 l( Q, e9 l; b
- Error = int(nError*Kp+ErrorAcc+ErrorDec);
! R) t' z1 S' ~! J6 E/ ]$ q - if(nError < 80 && nError > -80){2 a7 Q% f5 A( o7 V. D
- if(nPower < MaxPower){
% z- A5 ?1 f& I, R0 A4 U$ J - nPower += 3;
4 G' F# L* a# [5 t2 R - }1 D7 s4 A2 q E+ c9 y7 [( [7 k' h9 T% X
- } else{
3 ?$ Y& h* g7 ~1 o6 N - if(nPower > MinPower){' P3 ?, T* k% I2 _3 w- ?. g
- nPower -= 2;' F7 i1 }& O0 ^2 Y
- }
3 g( X6 p h1 f/ i+ f - } 4 c1 Y, ^8 `+ A( q7 p- V- }! I
- MotoL(nPower-Error);
$ }" n S* R/ g! D* D - MotoR(nPower+Error); + U: \; e' x9 u: n- u5 p9 }
- }else{
: Y0 i4 G% Q' L1 j# e+ T - motor_9.run(0);( q! U' Q( }" O! q
- motor_10.run(0);* u# J+ I! H% f* P- A! d
- } z3 e7 R* t9 ?- k9 t" L7 C. P6 x( k
- do{}while(millis() - StartTime < interval);8 z) {5 ]6 G% R0 k
- }/ _4 ]' E7 l: i9 F! U6 ?
" G6 _ F3 h6 p4 _# j- void MotoL(int Power){: c& \- E: w. b
- if (Power > MaxSpeed){
% z5 u1 [! N9 j: c. }* | - Power = MaxSpeed;
`" T% [9 ~6 [5 g. d+ B$ ^ Z - } $ x- a2 X$ \. b1 a) [2 o# S
- if (Power < -MaxSpeed){
7 U2 l! J0 u4 M9 G. s - Power = -MaxSpeed;/ W3 d6 b! M% n2 {" `
- }
, Q% _) w1 `4 R* l/ q - motor_9.run(Power);- q7 K) x5 \) U- @( U' H7 S
- } 6 @9 V+ l) {' {
- / ^0 K1 ?) d4 j" ~* S( G( ]
- void MotoR(int Power){
) a; y+ z) R! m0 X5 F! ~" N - if (Power > MaxSpeed){/ F F' n9 O& U' S
- Power = MaxSpeed;3 `% P/ L5 w. U1 c+ a
- } 9 t% z0 J; u+ W$ N; |
- if (Power < -MaxSpeed){. A6 j. v& }/ f) O6 G2 R6 e
- Power = -MaxSpeed;
6 @3 C' t/ [: @7 }7 G8 E# m - } % s! S- h& T+ H4 l. W: w+ c* p
- motor_10.run(Power);
* T7 u% B( C1 R+ ^ - }
複製代碼 ) S- }+ @5 T9 V9 ?+ \7 ~5 {% E
5 y7 Y2 h! M! Z; i( n |
|