|
|
- #include <Arduino.h>( a7 U z1 P5 l5 x* `* w
- #include <Wire.h>
* q4 d$ J& o/ s, T3 k( W - #include <Servo.h>
h+ {0 l+ a! a2 T
" `9 o Y6 s; h2 c3 ^- #include "MePort.h"0 J$ P' V4 R& s4 a- K
- #include "MeUltrasonic.h"5 |, ^& I+ w- C. f* `
- #include "MeDCMotor.h"; p* ?. S/ \1 u! t! l
; _+ ?' [& S+ _" _" t T- //double Input, Output ;
v4 D) o5 N- y, l% c; S5 [( l - float MaxSpeed = 255;
. m9 s# Q2 D+ Y - float MaxPower = 180;
* Z1 x) [( c, f2 u1 v - float MinPower = 120;/ N: u6 [8 T% E3 n9 C; _+ x0 I6 n
- float Error,ErrorAcc,ErrorDec;
" }" [6 ^3 u3 }, R: n5 j
2 j1 q( O6 m% e- float Kp=0.14;
& W6 T' J5 g B$ k3 K - float Kd=0.00020;//23;( f& t3 c' |3 ?; H2 V, B8 X3 X
- float Ki=0.000201;
0 v( ?" H/ M5 \" b% w- Q* t8 {; b - 2 D$ n: m! e$ v& K. T
- float nPower;
7 a' j/ L* E$ b* ~ - MePort lightsensor_6(6);
* h3 p; ]: f, t/ p/ a. u4 s - MePort lightsensor_8(8);3 S4 u% J! ^1 B) i( L
- MeDCMotor motor_9(9);+ A' M: r9 }' q9 ?7 B: J' {
- MeDCMotor motor_10(10);
: J4 _# o+ G) z0 T$ ?" V - unsigned long previousMillis = 0;
9 }2 O2 P3 m. J% I4 h - const long interval = 1;: @7 B4 h, D3 {( _( L8 U
) `: Y% B5 d3 l$ g- void setup(){
5 o( n( D# e9 c+ W0 v - lightsensor_6.dWrite1(1);* A1 x$ v7 {& L
- nPower = 160;$ R' k n1 t$ w* i5 I0 ]
- Error=0;: W1 g$ J" H9 J: T; d3 Y
- ErrorAcc=0;
+ h# r; R1 g# t+ V( A6 G% e - }( X! \7 A# O, G# d0 N
8 j- ~" U- k+ X- void loop(){
9 E1 D O2 Y/ F% k - unsigned long StartTime = millis();
. \/ o1 B! ~, z# W( t' l - if(ErrorAcc < 18000 && ErrorAcc > -18000){; P1 j' d' ?/ ^! i
- float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
. B4 h3 H! u& \" L( ^ - ErrorAcc += nError*Kd ;" H3 p9 D/ U7 f+ M
- ErrorDec -= nError*Ki ;; D7 E6 _$ N( t$ Y( {
- Error = int(nError*Kp+ErrorAcc+ErrorDec);
* W @& Z# b& ]# P5 p - if(nError < 80 && nError > -80){
0 q6 \" U) O) G4 j- Z5 z: Q: ] - if(nPower < MaxPower){ i* E! B$ t- g
- nPower += 3;
8 q9 H, b4 r% m( T/ ] - }
# B8 z6 f9 ]- V# V9 C# T - } else{+ n9 x" b9 N; k9 i0 t
- if(nPower > MinPower){
/ T8 f+ t5 o0 } |8 e b# K1 K - nPower -= 2;: ~* e) h, r3 s- F
- }: @! ^7 q5 ]- e
- } ( \) O1 u$ ` z! a1 D
- MotoL(nPower-Error);4 p# W( X" f4 D6 s# j+ e
- MotoR(nPower+Error); , t& ` V! i+ R' S* _
- }else{
7 V* J" Y& v2 L, j' Z `( R f - motor_9.run(0);( B) @, Y6 Y$ ^
- motor_10.run(0);
3 i- o% b# V: }- V7 c - }0 H: E7 H& H2 T8 a
- do{}while(millis() - StartTime < interval);( s) H8 f$ W6 O8 G( i# }8 o
- }
! x8 }' T7 V/ g$ x2 A# ~) X
$ P t0 K: }, ?2 D1 `/ X- void MotoL(int Power){) f7 ` Y* t2 n; _) |
- if (Power > MaxSpeed){
. a" L% o4 V! W1 }" ?3 \ - Power = MaxSpeed;' n/ @/ \/ q) s4 h7 j, D) ~( C9 S3 R
- }
6 S/ A5 A" k* ~2 V7 A - if (Power < -MaxSpeed){" J9 z) @# Z7 G5 l
- Power = -MaxSpeed;& Q+ n, u& A( ] L; o; I- C
- }
% _( T0 \- U) E& Y - motor_9.run(Power);
0 z* d/ r7 d+ I6 ~ - } # {2 l/ [$ S3 R5 }" f" N
- , R. b; L) H$ ^
- void MotoR(int Power){
( E1 p$ w2 S0 T7 s9 m- {- o2 m* o8 i - if (Power > MaxSpeed){
4 A: s" {3 G* c7 h% L4 h - Power = MaxSpeed;# v( ?8 b- G( q8 k
- }
, {/ ~0 V2 L: |- N, }: E5 a9 i - if (Power < -MaxSpeed){9 z7 L8 `& y) }/ s- A
- Power = -MaxSpeed;! P: N6 K5 A9 w6 z+ Q1 }- f* d8 X
- } 2 _8 u" G* p2 h- h. v: M1 w' f
- motor_10.run(Power);( h+ @* ? I( a+ f% [
- }
複製代碼
1 C3 s9 {/ R7 _1 x" _- N
: I- n& J, w. [ |
|