|
|
- #include <Arduino.h>, }/ [$ `: ]9 ?. V) t
- #include <Wire.h>
% y& }; {% n4 X/ h - #include <Servo.h>
! m/ H* Q7 {5 l$ h - 1 i) b( W) r" k) b- W6 N
- #include "MePort.h"! N0 G( e5 m) q# ]' C+ L
- #include "MeUltrasonic.h"
/ y$ N; Y1 i4 M% R& Q - #include "MeDCMotor.h"1 H. L& @" i$ W+ i+ H2 L
- , S& X5 A) T, [8 \# A' W
- //double Input, Output ;7 \6 h% O7 t9 m; {
- float MaxSpeed = 255;
) q, R4 T! Z1 I) {3 |3 ? - float MaxPower = 180;8 O/ B8 R M" O7 p5 X
- float MinPower = 120;, o% f" I9 w3 v4 P; h7 g1 D
- float Error,ErrorAcc,ErrorDec;7 Y4 b+ O9 z: \" H6 N; ]/ Q. x
6 a) S' j6 F0 m" z. ^! a- float Kp=0.14;
* V# U6 i4 |2 ` - float Kd=0.00020;//23;
: y% r4 r, r- @7 F - float Ki=0.000201;
z/ b$ H* v; y% d5 {' N9 m - * j9 H9 Y- b: A
- float nPower;
( h0 \' }- {0 s2 l, P( G* S - MePort lightsensor_6(6);
( L. q5 r3 t1 V0 G4 q7 \' [4 V - MePort lightsensor_8(8);) w% y/ N4 U' o% d. Z8 D
- MeDCMotor motor_9(9);
5 i( q: I, _- }3 S( [1 f* z - MeDCMotor motor_10(10);
: p/ p$ Y: Q+ k2 ]+ B - unsigned long previousMillis = 0;
7 f3 B, g# T" g# S1 a2 d, x - const long interval = 1;2 v% b0 [1 U9 l" g
3 {8 @5 v, X$ y( l; ]4 F- void setup(){; Z1 Q' v" t% O8 ]$ `2 q
- lightsensor_6.dWrite1(1);7 s. @5 z, j3 U# Q0 b, F* i
- nPower = 160;) @% m3 [ k1 R3 B
- Error=0;- a- Z! e/ X- s" S% o$ V
- ErrorAcc=0;' R- b3 P1 O+ r
- }
7 h2 \% A5 u2 ?1 q: {6 D# H
* e3 a4 X7 ?2 _- C0 D6 ]9 v$ k" D- void loop(){
, J5 C+ D E) O7 S9 |( J6 t - unsigned long StartTime = millis();
^* ]4 W( G. Q1 Y, h% E$ n - if(ErrorAcc < 18000 && ErrorAcc > -18000){
U6 H: T1 Y* B8 R/ Q8 Q$ C- z0 Y( |7 T - float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();. w" R9 e) W# M6 V- `4 v
- ErrorAcc += nError*Kd ;
% \5 G2 |1 J( |* g: J! d& ^ - ErrorDec -= nError*Ki ;1 e0 t8 p/ J8 V. y' w) ^
- Error = int(nError*Kp+ErrorAcc+ErrorDec);3 J- q9 I" g* S) i; A: q& r* z
- if(nError < 80 && nError > -80){
3 O3 i" ?$ L n7 r - if(nPower < MaxPower){
" K0 N9 C% N8 C, r - nPower += 3;
" N* S+ v* J% l4 @/ Y - }: l6 f0 l* w4 d F% b9 X1 F% K- X
- } else{+ b# p, h. a" R/ h8 d5 w6 E; E
- if(nPower > MinPower){& _3 z* t5 N' [% @, [
- nPower -= 2;: {. L$ H" J' s' j7 R
- }
& N3 j$ d6 |+ | - } ) I/ v+ Y0 n# }, [/ f. `% D
- MotoL(nPower-Error);6 l& L- x3 I% M
- MotoR(nPower+Error);
3 N+ ]* ~2 X2 E - }else{$ o, b, t6 E) Y$ i
- motor_9.run(0);
9 w: D+ h1 r* O7 W' @* Z" C - motor_10.run(0);
/ T7 S1 R) E0 p& c - }
& i" R- [- U/ x8 U* |5 w - do{}while(millis() - StartTime < interval);
5 l3 t! V1 O: N% Y - }
4 K7 q4 c1 C- v# R - . J5 z' R3 b7 q' q( X" U3 Q) ]
- void MotoL(int Power){4 m4 g% E5 L: q+ A% r
- if (Power > MaxSpeed){
: k% t2 R( g+ N3 {# l2 `! [ - Power = MaxSpeed;
1 p" j2 N5 Q* s6 h+ _4 `' R3 i) h - } & M# L/ s# p. z- @# s0 F, B
- if (Power < -MaxSpeed){' ~0 \' p2 X0 |+ R/ p& u) W
- Power = -MaxSpeed;
, P' l4 H1 r5 l0 |* R. Y" s - } ! A. W2 a6 s* E
- motor_9.run(Power);
1 p/ z, F& ]3 U) O - }
# o. i( t p, A% y1 j
6 V# C- B& B3 Y, B. L9 W0 H- void MotoR(int Power){6 I$ I- T- A4 p% s$ n+ K
- if (Power > MaxSpeed){
/ D6 K3 o2 N7 j* {; r - Power = MaxSpeed;
]7 v: `3 j* q - }
' q8 l6 D5 k* z- h9 B. h - if (Power < -MaxSpeed){
2 L* M e/ s$ ~# f% _4 D: h - Power = -MaxSpeed;$ N! ]- A% v1 w
- } 8 |( z( m: z- a) S
- motor_10.run(Power);
6 m8 k" f9 U7 m p# F- t - }
複製代碼 2 ~% _- P; B5 W6 r
O, d! S9 K1 E' M1 X/ d
|
|