|
|
- #include <Arduino.h>( _" o( M) S0 j( F* s
- #include <Wire.h>
0 B* i6 L/ d% D, ]6 u' q9 e - #include <Servo.h>7 o, a! l9 l2 r9 E
- ( @3 i) I2 Y; M- Y4 Q$ ~
- #include "MePort.h"
# q, Z. a: _! g - #include "MeUltrasonic.h"; X& h e4 W& p1 Q& Y; g
- #include "MeDCMotor.h"
6 e- _ V9 L- E" O) s - 6 _' h, Q( k" Q( w% e/ v6 t
- //double Input, Output ;4 `# T' q0 P9 y# K( ?8 s3 q9 ?
- float MaxSpeed = 255;, g1 P3 Q1 i! u4 ^
- float MaxPower = 180;
' I1 X, M {, y - float MinPower = 120;8 n# a0 z, i3 \: Q/ R% c
- float Error,ErrorAcc,ErrorDec;# x/ k+ W6 k! F* R' L
- 8 f5 w4 F6 c4 L0 o w
- float Kp=0.14;% v5 L8 H5 J8 |, X# {/ k& M
- float Kd=0.00020;//23;. `+ q$ [; k/ \$ J$ Z9 @8 e
- float Ki=0.000201;/ w! n; U/ O0 b2 q
- 2 a; B( R# H t0 j& i. l* M5 W
- float nPower;- j& o3 P' L) L1 c* [
- MePort lightsensor_6(6);
% D7 p: Z# v# ~ - MePort lightsensor_8(8);. ^& p [1 K! {
- MeDCMotor motor_9(9);1 d7 i. o) ]+ Y# J
- MeDCMotor motor_10(10);0 s2 F8 \# u" A. K3 I
- unsigned long previousMillis = 0;
6 c1 c8 C l3 t/ o S' X! e - const long interval = 1;/ y; Z8 ^/ w/ e# g, D8 m$ A
- / F3 @0 {* N. j+ \/ a( [0 y7 j
- void setup(){
5 T& b: y9 G$ ^8 U! V - lightsensor_6.dWrite1(1);
5 A3 X0 \: c: U) z l+ T) d6 \ - nPower = 160;
8 W8 H9 r$ @+ u* q- D6 P8 P - Error=0;8 f, {! ?4 A. ~" o1 D+ Z+ ^
- ErrorAcc=0;; }; A* j) z; z; b1 B: `2 }' b% q
- }
' C6 e& l8 U; P; {1 g- {
4 w- h, d; w7 q$ T- void loop(){
: q7 L# U- A5 O; R; \$ Z - unsigned long StartTime = millis();3 w: r' ^9 G. v0 {& _
- if(ErrorAcc < 18000 && ErrorAcc > -18000){9 c" s# m2 a% Q, s3 y
- float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
1 L/ u: U1 D6 I" }4 } - ErrorAcc += nError*Kd ;
) j9 [2 n6 {; M2 ? - ErrorDec -= nError*Ki ;: `' k6 ~% @& j# e
- Error = int(nError*Kp+ErrorAcc+ErrorDec);
4 l+ r- |( j9 l' L - if(nError < 80 && nError > -80){
# T* u5 N( P& I' E3 d4 V6 t ] - if(nPower < MaxPower){9 G9 m: c w1 [7 p. h
- nPower += 3;
; S9 j/ p1 ^/ P; S - }
; n# c4 {* f1 i1 K! g& D - } else{. M3 Y5 T4 B+ \) u0 v( x' f
- if(nPower > MinPower){6 [1 I" ?0 B/ Y6 {5 W4 k
- nPower -= 2;
" G" i7 g4 n" w; a7 S# j - }
: Q( c* N8 |; E+ { - } + K" {' g. G5 S8 E T% j
- MotoL(nPower-Error);
* y& K9 ~* J r5 C - MotoR(nPower+Error);
0 ?) j: }9 n( G9 [ g8 W; E - }else{& p3 p6 K& E5 Q' [( [+ ?- l% g
- motor_9.run(0);
0 g" E9 _& e7 N! W - motor_10.run(0);
^* e5 F* m5 i! S. @( z6 \ - }5 G$ a3 `5 e1 `6 l" M* }+ i
- do{}while(millis() - StartTime < interval);
$ u2 J2 B% `! P u8 R/ T) ]& S - }
) ~) m5 O! J. f; |
/ e* C: m0 `# u2 h3 v/ g7 @7 S- void MotoL(int Power){
" r+ b ^% a: d, I' ` - if (Power > MaxSpeed){
7 c( Z! J3 Z2 M- v- m% t4 g/ n - Power = MaxSpeed;( c$ T$ @% e- c9 h/ m, M7 J
- }
0 d) F0 K; v9 B- H6 g - if (Power < -MaxSpeed){" X/ d2 i" X: o/ y
- Power = -MaxSpeed;
$ t3 ?; X9 F- Q# j - } - v5 P' D, o$ e" k* k9 c" O
- motor_9.run(Power);
& {5 C5 e g7 n8 q9 A! q - }
) T( |. p5 {% K1 q4 @ - 9 O1 L6 a3 L& e/ j `; O
- void MotoR(int Power){6 F" w8 Z( g$ b
- if (Power > MaxSpeed){. }9 x- _% n" I6 C
- Power = MaxSpeed;
# w# g% c/ S# h* c8 d3 A# D - } 7 E' W& m$ r4 p) \
- if (Power < -MaxSpeed){' d) p9 i' }# y
- Power = -MaxSpeed;9 }$ ], h7 ~8 [4 M7 S# Q0 ~
- }
: F1 C, `+ Y( s3 m' R - motor_10.run(Power);
( Q" n8 {. C8 l9 z - }
複製代碼
- g3 {" E6 G; @8 M# a3 \# P2 G* G0 l' R! i+ @
|
|