|
|
- #include <Arduino.h>9 S' J. f% T- e4 D- r
- #include <Wire.h>; h1 q' e% B5 a# B
- #include <Servo.h>7 o8 s) O& h; d
3 N+ e [* T, Y1 N. s- #include "MePort.h"
6 E" j. a4 j7 W _8 a* n% L% M - #include "MeUltrasonic.h"
. Z" {( \! s- a& {* r9 E: K5 q - #include "MeDCMotor.h"" K: n' X" g* |6 b- J0 t. o
6 U6 W' f) @: ]- //double Input, Output ;. Y4 ]6 G" Q5 N
- float MaxSpeed = 255;
7 q) ~, ^% b. c8 \( w2 T' ^ - float MaxPower = 180;2 f- X" q/ I6 M% W8 j+ _
- float MinPower = 120;
6 W O0 O7 T' U% V1 B5 a9 ]- r - float Error,ErrorAcc,ErrorDec;
/ N2 V# k M' _8 K& ^ - & ]1 M! _8 t# a& n0 f8 c
- float Kp=0.14;
( b }# ~. b: z& w% ]3 Q - float Kd=0.00020;//23;6 i% }) ?2 [% }$ ]9 c
- float Ki=0.000201;4 {) b8 S0 ?* J+ C9 X5 i% B
- 9 W# j, c3 ^, X5 x( H. [% B
- float nPower;
4 z: d& V! R. V+ w( F - MePort lightsensor_6(6);
6 B1 D; A; l" @ ? - MePort lightsensor_8(8);7 A& D$ q4 _. m
- MeDCMotor motor_9(9);
7 H U% k9 ~4 |2 @ - MeDCMotor motor_10(10);" Z: E6 P# S; F8 Y2 ?& ]
- unsigned long previousMillis = 0;
+ O* r% {! ~1 C3 E/ R - const long interval = 1;* ?; |( `9 k' S' `7 v" T
- % b% S# l7 f$ j0 I1 z$ n
- void setup(){
* |, x" s$ {( u. D - lightsensor_6.dWrite1(1);
2 `" r7 V {- d - nPower = 160;$ Q4 p1 G8 y2 Q& [# h6 q
- Error=0;
, s+ m0 a# o$ v - ErrorAcc=0;
. l4 P( {4 k" F* A: j - }
) a8 L- S$ }- C$ {, y- F& v - + A2 Y2 O! L: M/ {: S) M k
- void loop(){9 E& @( D9 X( G% a3 k; A
- unsigned long StartTime = millis();* K9 Z: k; Q7 Y U4 \/ \8 A( k
- if(ErrorAcc < 18000 && ErrorAcc > -18000){
4 y8 D" I% i, u' t7 G - float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();) f; P* o6 i* @3 n4 F* L
- ErrorAcc += nError*Kd ;
6 J" l6 @* }8 B8 B4 P2 v/ G - ErrorDec -= nError*Ki ;: \0 Q# c6 N5 i
- Error = int(nError*Kp+ErrorAcc+ErrorDec);7 @( N+ c& D4 z
- if(nError < 80 && nError > -80){4 y6 U# l% O# ?: r$ K; x$ d
- if(nPower < MaxPower){$ W* T/ @& \ K
- nPower += 3;
c. v; b. F: f6 P) C1 w1 S1 y$ |% y - }
0 g. ^# ^) R5 s( U: \( C' d# f7 x0 \7 q - } else{3 s9 L3 R0 V$ ^$ @- |* u
- if(nPower > MinPower){
+ p; F$ p$ ]: b1 |9 G9 | - nPower -= 2;/ t" H' E7 N) l2 d% S# p
- }
; \5 U. h6 ]0 G+ K }, P! g - }
" X5 |' g* ?6 ]: B - MotoL(nPower-Error);
u/ z8 Q; E% ~, B# r - MotoR(nPower+Error); ! L# c, u- T4 Q; S) H( L
- }else{2 K2 b% ~* r% I, w9 E
- motor_9.run(0);/ C1 S( T, S% S/ w5 l
- motor_10.run(0);, [2 M2 b S& G H" r
- }
/ T# f% d) h+ w) D! K' G - do{}while(millis() - StartTime < interval);
# |3 j; ]; f& I4 V& i: [4 f- r" B - }
" k- ]4 }7 [9 Z
) H( c6 E- x/ S" I. M* w# N# }$ E- void MotoL(int Power){
* w2 f: U( Q. _2 s* _# t - if (Power > MaxSpeed){/ h7 b: [- k$ f4 \- o0 }4 n* b3 D# `
- Power = MaxSpeed;
% N4 _( ?) R. U, j. b# Q+ l - }
% P/ t/ f1 H/ F/ Y - if (Power < -MaxSpeed){4 r% h$ E# c( Y* S7 E
- Power = -MaxSpeed;
4 E! y8 {9 ]8 O! D - }
. @3 ]5 z* }4 ^4 `( H - motor_9.run(Power);
~- x; H; E/ [ O3 B - }
3 Y8 e2 ]7 Z' U" Y* Q- e! }/ A - , l! e% \5 F/ A& f; D+ n7 D
- void MotoR(int Power){
! ]( h( y p8 I) ]- O4 x0 h/ T6 I0 V - if (Power > MaxSpeed){
7 j: J1 b& |+ K% _* K - Power = MaxSpeed;7 V0 s5 f3 [1 f: t8 A% a6 N
- }
( o7 c, X2 L; s& m - if (Power < -MaxSpeed){
- ^& J* n+ T% K1 Y9 D9 b q - Power = -MaxSpeed;% M- X3 b7 M& K3 E) M( Q
- }
$ }4 k `% C; L: i4 H! n) n9 r - motor_10.run(Power);
. {0 Z( h7 ~9 k5 r5 \ }) f$ o; I5 U1 e - }
複製代碼 9 d& t9 s# d* ~4 Z8 h" @- b
5 F5 R) ?3 J5 [8 J2 d5 ` |
|