|
|
- #include <Arduino.h>
4 S9 `% B6 l3 W2 k& w7 m+ U - #include <Wire.h>
' }. U2 C: c3 @+ C - #include <Servo.h>' l8 B7 _) c" r/ M B1 ~
- + T9 U! e7 b, _( t- Q
- #include "MePort.h"% l2 L- P9 T6 L6 x
- #include "MeUltrasonic.h") A+ i' J* M& \3 X, w% R! W8 H
- #include "MeDCMotor.h"
6 G! f7 d7 Z6 U+ h - d- p+ t& _+ w+ L1 {8 e2 b6 Q/ O) p
- //double Input, Output ;
, n4 s/ G; E6 k% s - float MaxSpeed = 255;! l$ G% {) s. E" V0 y* D
- float MaxPower = 180;; }$ @& a i3 J
- float MinPower = 120;
2 N, r: r1 T: c: v+ S - float Error,ErrorAcc,ErrorDec; K1 t; t# l/ b% f5 S: x
- 7 I) F: x$ |; [
- float Kp=0.14;
/ z$ l! [) }' d" f0 v. B- V3 G0 n1 q - float Kd=0.00020;//23;
! h7 r. R2 n% [3 _( o - float Ki=0.000201;
/ ?' r8 L7 r+ z3 g Y* U - & `& o1 @ n/ K, n& ^) K
- float nPower;$ T/ A. {$ C R9 c& L) t
- MePort lightsensor_6(6);
0 }' j. |- R2 X' C - MePort lightsensor_8(8);
1 ~( }0 g) @, |6 k. I - MeDCMotor motor_9(9);
6 i$ Y6 \/ p* y% |7 x - MeDCMotor motor_10(10);. m2 q+ D/ f0 Z: `+ N: y# E
- unsigned long previousMillis = 0;! T S0 n0 J" F
- const long interval = 1;( [$ z( |5 I. f* W( T! t- Z" J
$ M+ X2 o9 E( c5 } a0 e8 D- void setup(){( C {' u+ ~# R% j5 w
- lightsensor_6.dWrite1(1);
, w, W0 c5 Y: k# { N - nPower = 160;
; M3 G3 t+ [+ L6 r! `( t6 u0 I - Error=0;
2 S; [7 N/ G7 E) ~. F Z - ErrorAcc=0;1 K3 L8 [2 |9 G) c W
- }
* Z1 C, `0 w0 y5 ~7 W0 _& J4 n5 J
; m P2 N; I1 t- void loop(){
* k1 J& r2 u# f7 r - unsigned long StartTime = millis();
\3 x0 i# Y$ d1 m% z - if(ErrorAcc < 18000 && ErrorAcc > -18000){
8 b1 u5 A% v) K+ j - float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();9 ^0 i0 P5 e9 n/ x: C V
- ErrorAcc += nError*Kd ;0 ]- _. ~/ B6 O* L& H" R
- ErrorDec -= nError*Ki ;+ C" o1 H4 Z. X! ~* N
- Error = int(nError*Kp+ErrorAcc+ErrorDec);7 Y; j3 m" v% [1 Q9 M, E
- if(nError < 80 && nError > -80){6 H- k |* B- R9 _
- if(nPower < MaxPower){. U$ m7 g u8 {( E1 _
- nPower += 3; Y% D6 Z0 P+ J( N: M. X
- }
& s: }6 E: _ ]* H# p6 k2 l - } else{9 b# g0 d6 q- I) i9 g5 Q7 W
- if(nPower > MinPower){
+ ~1 L0 x+ x% U* a3 x - nPower -= 2;4 ?4 L; i1 h% V" d( Q' l& ?' b
- }
3 ?) A" e5 Q9 F) l5 o, u - } # E8 K& [3 U) d3 }0 w' ]
- MotoL(nPower-Error);
- z) G1 q) Z6 X# p, R. F - MotoR(nPower+Error);
$ n5 T I; x* p) M2 I - }else{
* Y, a' l8 N0 _ - motor_9.run(0);
. b0 W+ p; e% v6 r u2 F3 Q - motor_10.run(0);8 y6 h: ^/ X- J2 x* [
- }
) r9 j9 L+ z/ @- Z - do{}while(millis() - StartTime < interval);/ \% ~8 i* w; Z0 T' [
- }
+ s+ w2 {! \. f
- \# i' k0 m3 F8 c3 F- void MotoL(int Power){
4 e- c, I, L6 [ - if (Power > MaxSpeed){" q" v7 V! M9 K9 @0 E3 C
- Power = MaxSpeed;. L& X9 Y L+ I; \- b+ \
- } ! B4 L5 c7 f" | l% X
- if (Power < -MaxSpeed){ I) V7 D% Q I# U
- Power = -MaxSpeed;
# K$ s& H9 ?/ A/ k, d" ~/ R - }
0 g% ^: R1 h# x2 w' a* [ - motor_9.run(Power);3 C$ {% {* K, y8 F* @% a/ `
- } , }9 u3 N+ i( u$ j
8 O# g2 C$ O/ H$ N- void MotoR(int Power){
$ }: o* \7 R9 U4 ]7 b7 G4 T* H - if (Power > MaxSpeed){
: e( o+ o4 U3 k+ N - Power = MaxSpeed;. P+ \7 \& { n. v2 C
- }
" t0 l5 ]; `3 s' K - if (Power < -MaxSpeed){! v- o9 ~& v8 b/ P
- Power = -MaxSpeed;
9 V( Z# S8 `3 |) U1 w/ L; F - }
' y' l Z) q3 c) k* f4 p' [ - motor_10.run(Power);
8 _$ J& g! Y+ l: ` - }
複製代碼
E: T, B* \, U4 J6 I
. p/ d2 Q$ @; `& D% i* b, k |
|