|
|
- #include <Arduino.h>: P3 R' ?3 M3 N# o* Y
- #include <Wire.h>
+ ~4 Y! x9 r! c. } - #include <Servo.h>
8 F* l" Y7 _* p8 l/ I - % v" n+ K& i# O" Q
- #include "MePort.h"3 _9 Y& E7 z" G+ E* }+ U
- #include "MeUltrasonic.h", J( z; @/ _3 m& m
- #include "MeDCMotor.h"1 q8 K4 s) T. z* \; V# O- _
$ I, O8 g6 r# I$ r% h* j. E6 d4 v) G' ]* L- //double Input, Output ;
% L, L9 w9 B7 g+ p& S% z: ] - float MaxSpeed = 255;
6 V9 R, U3 G7 u3 E) l - float MaxPower = 180;
4 T0 }7 I2 b# \- a" ] - float MinPower = 120;
) @' L" f+ ~8 q% X- Y' g - float Error,ErrorAcc,ErrorDec;
- c; D4 q. A0 `* ^3 c* S - 9 _2 B: B9 N# C9 i
- float Kp=0.14;" z% A% Q! S1 z8 p
- float Kd=0.00020;//23;! l# X* O3 j' ]6 @5 w% G
- float Ki=0.000201;
& a1 u' m3 v- z, x) h) x3 e
; Z1 m) ?8 j5 i' `* I% U1 n- ?7 b- float nPower;
) o. ]! Z4 p9 |% j, \: _ - MePort lightsensor_6(6);0 d8 H/ y3 R/ J P k$ q
- MePort lightsensor_8(8);4 D* V0 o; Y& z' Q/ _- w- n
- MeDCMotor motor_9(9);
4 \" E+ B( c1 J y5 L9 R$ B - MeDCMotor motor_10(10);4 B) ^4 X$ K" P1 g1 @
- unsigned long previousMillis = 0;5 T/ i6 ^" ~' [. l$ ^" s
- const long interval = 1;$ O& m1 k: F8 l7 F+ B6 C
- * K O& y9 g/ m X2 |/ v) b2 x
- void setup(){
" F& i! i: ~/ _3 P - lightsensor_6.dWrite1(1);
& s0 G8 M8 ~5 `) B- y - nPower = 160;
3 a0 |. w$ A. I3 B, T1 u, Y5 Q - Error=0;
/ S+ j# D' p( k- s( b/ O( u6 S - ErrorAcc=0;
& {0 [3 L$ Q, y" ~( p' ? - }
, X7 ?- f) U# ]. m# F9 D! x/ w9 G- z - : Y+ @. h! M' A* y
- void loop(){
( J- v t) M4 U1 T) y - unsigned long StartTime = millis();
8 X3 g$ H |& g7 c, C - if(ErrorAcc < 18000 && ErrorAcc > -18000){& F2 y# S, {! @4 K2 v; x$ x4 D
- float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
g5 \; v7 C- S0 r# m, S - ErrorAcc += nError*Kd ;
* m+ m. T8 K: m4 [ - ErrorDec -= nError*Ki ;+ ]5 h' I$ p( o
- Error = int(nError*Kp+ErrorAcc+ErrorDec);& l- i5 t2 S0 e: V$ l; \+ j
- if(nError < 80 && nError > -80){# u) M$ H) {) E( Q' ^" \; S% r
- if(nPower < MaxPower){' B9 t1 D X# l2 l5 [
- nPower += 3; d3 I% S# \) H: I' B( q5 V8 f% Y4 J' K
- }
0 j+ W8 E2 ~; }: T! x - } else{, P8 A0 ?6 I8 V6 {2 t K I. _' j
- if(nPower > MinPower){5 I; E1 t! @" i4 o
- nPower -= 2;
9 G5 _! {. O2 L3 e3 ` ] - }
' `$ y8 C1 S. [( n' @ - }
0 H$ u2 a1 e" N+ c7 m7 T* Z7 C. i- B - MotoL(nPower-Error);
1 ~% I2 a8 ~0 u - MotoR(nPower+Error); D9 F; i `; c+ s& M! l
- }else{* Q4 p8 \$ W @5 F9 s& M d: u& p
- motor_9.run(0);
/ l, `2 e% ]: a9 J) j - motor_10.run(0);4 ?1 Z3 G4 W2 A8 G% X C, l- ^* C0 O- D
- }7 T/ A2 q5 M8 W3 ?0 u) u
- do{}while(millis() - StartTime < interval);
) O. F$ u7 ?! Z9 F: s% { - }
6 e% M; t) M+ Z - 6 c% X! w1 t, U% F) `( E: |: Z
- void MotoL(int Power){
) m& F+ m: u/ ~! c0 v" s E - if (Power > MaxSpeed){
# K( A8 w# e% q' I - Power = MaxSpeed;2 C5 k. X' C9 }; q
- } 4 F p/ s& O6 s
- if (Power < -MaxSpeed){
. J) x" G/ h0 s" h& |5 E9 w0 b5 o - Power = -MaxSpeed;
5 E n* E+ U/ I* P/ M - } ] b: e+ _! {6 `/ v5 {( U
- motor_9.run(Power);- I$ z4 k9 ]4 S* Q6 b4 ^
- } 7 ?' y; R) i8 h
- 3 u0 }, R- a* F0 n3 } t/ ^! M c
- void MotoR(int Power){
* G7 x* r, z$ N3 o. I# N; O - if (Power > MaxSpeed){
* k' p. S- B8 o1 \ S - Power = MaxSpeed;8 A& t- h- }9 [- @) _: s! r
- } % c X" ~2 w3 C' Q. h+ L
- if (Power < -MaxSpeed){3 M S) g4 a9 g$ I6 P" ]
- Power = -MaxSpeed;9 c- l5 K2 d/ o0 _
- } 1 q+ c7 ?: _8 D+ r5 F
- motor_10.run(Power);
, u: \7 T# P) f2 c4 q - }
複製代碼
/ m- U# ], ~% f; I# c
4 a% C! Y! T- d0 r+ f |
|