|
- #include <Arduino.h>
$ N, O+ C Z& }# m7 ? O8 G" S' L - #include <Wire.h>
$ s: ]7 Q) A- u& ^% P( F% F+ k - #include <Servo.h>
% ]3 R' i v2 M6 Y - 9 [8 }, T: r% r5 |
- #include "MePort.h"2 z2 P* ?7 Z+ H% d3 r" X
- #include "MeUltrasonic.h"
# T8 u2 y7 r5 |: k - #include "MeDCMotor.h"
3 s' C0 d( H) p5 ]( A+ @) h3 b: z
. l* ^( |3 X# T- //double Input, Output ;* i/ ~+ S4 }' G6 t" Y
- float MaxSpeed = 255;
* Z; V4 ~& G; V" Q9 m! `7 P - float MaxPower = 180;
, ], P K8 g5 `$ y - float MinPower = 120;
. s5 P2 [- n9 u& a, l- ^ - float Error,ErrorAcc,ErrorDec;) {* W/ m& m* J0 I/ I5 c
! f7 r1 w' }" d, H. ]7 \" l+ i- float Kp=0.14;) @% S0 j+ x3 V' |/ X4 d% K
- float Kd=0.00020;//23;: N5 Q8 k; P3 @. n7 @
- float Ki=0.000201;
) o2 ]5 R. P/ {5 m' G' R - ) O8 t' }3 y, F; B4 I6 F
- float nPower;3 T7 s+ t8 W% z! e0 o
- MePort lightsensor_6(6);! I$ v: X# J% X+ C% S- C
- MePort lightsensor_8(8);
5 H! Q8 e. ^& I4 `) B6 |+ m& R - MeDCMotor motor_9(9);
' A0 C! H9 M. K2 \3 c/ ^& O' ? - MeDCMotor motor_10(10);
$ Y8 h$ S0 I: t& c - unsigned long previousMillis = 0;
, g; d% _+ f" d - const long interval = 1;) Q& H2 D) H8 g; X! J
- + T& n7 q% I9 y5 e3 F2 H5 {
- void setup(){
- H y. K! \# E7 z6 q6 ?; R/ a# | - lightsensor_6.dWrite1(1);
1 h6 P8 C- s5 |/ M - nPower = 160;
) D( O' ]4 u4 d# k8 d& F - Error=0;
' D# ?' D' }, o0 N( t" ^! ` - ErrorAcc=0;/ g1 d+ j$ U' a1 `! k* Q
- }
! s9 w8 s6 z3 E! W - . _8 ~! n2 V, _9 d' H0 B6 i: {- j
- void loop(){
) f+ n0 P- w0 j+ \5 [" s1 a" G - unsigned long StartTime = millis();$ J2 Y6 o- ~% \8 E4 [: W' _4 C
- if(ErrorAcc < 18000 && ErrorAcc > -18000){- T4 m* J( X6 j. B9 i- w
- float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();8 A6 L3 c' ~3 W( F
- ErrorAcc += nError*Kd ;" O: a/ _- n/ o, u# b' m( t
- ErrorDec -= nError*Ki ;
3 x1 d( ? i, ?# V1 e) }2 p - Error = int(nError*Kp+ErrorAcc+ErrorDec);
1 e- F. y8 g- q) k$ l+ m - if(nError < 80 && nError > -80){
, T) Q9 D# e# g9 F- {8 z - if(nPower < MaxPower){4 k# X( q9 S3 s7 |' l2 s
- nPower += 3;
. g2 A) ^9 V% O' ? - }
4 M' g9 y. {, f* w; y - } else{ Y2 C. i1 x+ q$ p+ \0 i
- if(nPower > MinPower){9 c0 J- x- _" R% A) d$ q
- nPower -= 2;" q! Y/ d# A1 i1 x3 E
- }4 g3 Y' R0 {6 I& K2 e- G. R
- } 6 }+ e+ J/ g7 T* u0 E8 `
- MotoL(nPower-Error);3 \: J' ^7 M9 c+ Q$ D( \# F
- MotoR(nPower+Error); ( D/ I) b4 \4 k' U- o
- }else{8 A: j; A- ?9 d( ]5 d
- motor_9.run(0);
! N5 ?! h6 X5 c D0 T - motor_10.run(0);
& @# v. G" F* w! v o# T - }
/ S1 `4 _1 k* p" X' a5 A0 R - do{}while(millis() - StartTime < interval);
- h: M! p/ i: w `- y5 Q - }
" C% [: _0 `2 E8 D" D$ S' S) e
, _% B0 j# x( [4 f- void MotoL(int Power){
% T/ T! m" |4 Z x - if (Power > MaxSpeed){
. [" ~: r6 [2 d: a - Power = MaxSpeed; T: S8 _ O4 q
- } * i9 c/ R. b# X# Q
- if (Power < -MaxSpeed){0 z& n8 Y: B# s* v% u' i* s2 Z
- Power = -MaxSpeed;; ^, o1 G# O; O' z
- }
( R$ Y# q4 Z$ i - motor_9.run(Power);
) a& @% ^& X! R% V - } 0 f) h+ X Q; K" K6 w
- 6 A. d8 O; A! {) f! m
- void MotoR(int Power){% |; j: l4 K w [( c o9 r2 g
- if (Power > MaxSpeed){
$ F2 Y" F# L# m( L0 c - Power = MaxSpeed;
) h2 Z+ P/ A" }# J; d. }/ c - } 5 z0 d, K1 Q5 X+ n
- if (Power < -MaxSpeed){! f0 l. k3 Z% f
- Power = -MaxSpeed;
4 m. F- A+ z! }; E$ X3 G - }
) t3 q( Z8 X0 [2 ? - motor_10.run(Power);- }$ L+ Y6 l1 e8 A+ A. J
- }
複製代碼
/ G& y* X0 @; p8 x: |& p0 H0 [5 {, `2 J7 R
|
|