|
|
- #include <Arduino.h>6 E( e) _* i* u. _3 n {6 G
- #include <Wire.h>6 `' j; L, @0 Y9 W4 q/ d- K
- #include <Servo.h>
4 i% i0 U- o0 D, C - & L7 y0 `4 e% l9 L* N; F# K
- #include "MePort.h"
1 i/ `+ O* ?" u5 U8 d: J+ a7 u - #include "MeUltrasonic.h"
' z; `5 e, D8 P, J - #include "MeDCMotor.h"2 ?- I/ J1 ?1 F* A- M* `
$ E1 q& @* ]) U* ~7 `7 r- //double Input, Output ;5 l7 b. V" f, _- \7 l
- float MaxSpeed = 255;
- T7 J5 c% `; F7 F# T& g* h! c - float MaxPower = 180;
2 {1 {! }* Y4 p8 I - float MinPower = 120;) R* o+ H2 E: L- H* a
- float Error,ErrorAcc,ErrorDec;
- d' u5 W& T8 R* r$ A, S - $ K' ?2 u& q9 ]0 G4 O3 M
- float Kp=0.14;" s1 R, ~: f: ~+ x% E' Q* e
- float Kd=0.00020;//23;' g8 c1 ^5 ]' w J f: G( P
- float Ki=0.000201;
6 i! X N& F! f2 s - 5 K- P. ^% x, } L. E; h. F* v" Q
- float nPower;1 ~& R7 O0 |: j+ ~1 h
- MePort lightsensor_6(6);$ T6 v( Y! p: @' R7 O+ r
- MePort lightsensor_8(8);1 R9 B* B; I. j- C" m) e
- MeDCMotor motor_9(9);/ W) B, j1 {+ I( b" j* \, R2 S
- MeDCMotor motor_10(10);
8 @6 {' u$ |2 C3 H8 t - unsigned long previousMillis = 0;4 W; T4 I4 D0 V
- const long interval = 1;
2 o, p8 G6 Q# a3 Q8 @ - ; ]! V. g3 m3 \ y
- void setup(){
4 S: |4 ?9 ^9 V/ s' b) j2 G) ]+ ? - lightsensor_6.dWrite1(1);
0 t. Q+ r8 k& [- O& Q* l0 P - nPower = 160;3 `# O/ S' k, Q; H2 Q
- Error=0;
9 Z/ Q* c: ?$ B5 `4 I - ErrorAcc=0;: b- K& ]: }( u" t8 b# e% a* o! b1 V
- }
, b/ ~/ l6 y! p8 _% g5 A - ; l' a! c; D2 C+ J" x
- void loop(){0 E" a& C) `- m' ^9 j# W9 U) M8 r
- unsigned long StartTime = millis();7 M, M9 ^& C0 X
- if(ErrorAcc < 18000 && ErrorAcc > -18000){
1 S8 T7 t/ C3 X c' m$ ^ - float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
! H0 x( D# ]% L2 W! T - ErrorAcc += nError*Kd ;# x9 N* p" F0 C, P' ~' O
- ErrorDec -= nError*Ki ;. V' ]* t! t' s$ @: U
- Error = int(nError*Kp+ErrorAcc+ErrorDec);" ^( K2 ?3 e. M s% W1 T9 X
- if(nError < 80 && nError > -80){( U5 H6 z6 I: ]
- if(nPower < MaxPower){5 [1 m9 ^5 B3 M- G1 r3 ]
- nPower += 3;7 m1 N3 p; v# D* Y3 @% e6 l0 Z
- }
% B0 o! x) m0 f - } else{
+ ^' x* N/ p# W& Y: g - if(nPower > MinPower){
5 k* c3 P8 i3 X- s) b# E - nPower -= 2;
, F8 a% I1 X: Y! m& Z6 g& W - }
4 T4 `0 U7 {, j( G - }
6 q+ `: k; i# Q+ o, c) a. D - MotoL(nPower-Error);5 ?& D% e8 v5 H% P& A) a }+ P
- MotoR(nPower+Error); * E& }/ N" ?% O1 E1 l4 V/ o" b2 G
- }else{; e% j& x) e( o' ]+ r6 f
- motor_9.run(0);3 s6 x3 Y4 p9 Z! @( ~/ d2 p
- motor_10.run(0);. }0 {6 o' i9 k: C8 N
- }. s# v# q* `) V8 K7 l) l. Q
- do{}while(millis() - StartTime < interval);' ~6 G5 n9 w* K9 y
- }( y8 Y. N2 `& ]; ?
/ L! J( M* `" ~; ]4 U- void MotoL(int Power){
1 r, y8 e) b6 _ - if (Power > MaxSpeed){( y4 G0 B2 |4 F( _; }% g: `& ?
- Power = MaxSpeed;- Y1 [/ t/ r3 f, l; L
- } ; h3 T6 L- c. I$ Q2 b+ M2 _* B6 L
- if (Power < -MaxSpeed){ \, }- \' ]8 e% \. ^5 X4 i
- Power = -MaxSpeed;3 n# m `) v0 Q+ ], J4 V
- }
5 R4 H6 l9 e+ I9 W; N- C - motor_9.run(Power);
& z/ V1 E6 i& Q2 C# Z1 C - }
* E* P; ^" y5 W1 k* |" x - " W3 {( }6 y# b- t0 H2 L# x
- void MotoR(int Power){
/ f" p8 q7 |' p8 M6 M - if (Power > MaxSpeed){
' d0 i% u2 A* B/ p3 b - Power = MaxSpeed;( ]+ d2 ?4 ^2 a' ?4 t
- } & n, n- t4 T7 I0 c( @; U* t
- if (Power < -MaxSpeed){
* z+ D+ n6 a3 I$ k" m5 T% k- G' v - Power = -MaxSpeed;3 f' @$ H, x0 H3 t4 }; R5 q# R, s
- }
5 W- _+ e& g8 u1 o - motor_10.run(Power);
) l# @( o `8 l. m' ^; o - }
複製代碼
% ?. C& u$ ]# W. N, I1 f8 N
7 S( l l/ @$ x- f3 h6 W2 A |
|