|
|
- #include <Arduino.h>+ \0 j/ b/ v9 d) i% r$ S# g
- #include <Wire.h>
1 M" o$ Z: u' r$ ]5 L" o5 K - #include <Servo.h>! k& o3 ^6 I9 O/ {
9 Q8 b8 H' |& G n, i- #include "MePort.h"8 K! G3 M) K8 I6 G, c/ d
- #include "MeUltrasonic.h"* n# [0 ?' b3 T' w# P9 K3 Y
- #include "MeDCMotor.h"% N2 q+ v- K2 J( n% ~
- , Q$ V) l6 ]- r8 s. s- N- o
- //double Input, Output ;
* W9 R4 _+ d p$ O - float MaxSpeed = 255;% z7 @( }' d$ h+ j' U% R; z8 B! K
- float MaxPower = 180;7 p: a: D8 S$ W" M( J+ _
- float MinPower = 120;
+ G9 I4 Z3 w- M; a - float Error,ErrorAcc,ErrorDec;
# m0 ~) o3 k% \6 _$ G6 S( p* R1 l
8 @0 I) j1 U* u9 `& Y! p- float Kp=0.14;- ]8 R4 V; e2 c: {& T. G
- float Kd=0.00020;//23;! ]2 O, S$ ~: Y# e6 a
- float Ki=0.000201;1 V, n- [4 p' W! s0 r
* K7 E" o( ]( Q f) [ u- float nPower;) {, V3 r/ p3 M" N* [" T2 O
- MePort lightsensor_6(6);
" u( S& { h$ i - MePort lightsensor_8(8);5 E1 c, Q' G& T- Z' j+ `0 ~; Q
- MeDCMotor motor_9(9);4 |$ l; I; D) I4 n( Z8 g
- MeDCMotor motor_10(10);5 t) c2 m9 Y( p( X# A m. q
- unsigned long previousMillis = 0;
' `; T5 B1 w; b - const long interval = 1;
+ f' D _; n* @; z; s0 j. T$ i0 B, ` - + Y9 N+ p# |6 o$ i8 ?9 x t9 x9 Y- f
- void setup(){
& ?7 w {1 f/ f- Q; Q$ v' C - lightsensor_6.dWrite1(1);
( s& p; {6 ?. `4 @9 J) n9 G3 v - nPower = 160;
+ ?2 d7 m* ?! {! ~2 p- X - Error=0;
* T% v% V% V. s: T4 } - ErrorAcc=0;
+ S- c) G* f2 L9 f) R1 P x - }
( V b0 M8 s2 R! ?0 ]
4 ~0 R1 d! B8 b& t- void loop(){
# t q! j. U* E0 F x0 H! f7 U - unsigned long StartTime = millis();/ x6 J; a5 z% l; T, g0 P
- if(ErrorAcc < 18000 && ErrorAcc > -18000){+ X) T6 Y- n) }, H4 y9 Q5 S2 K
- float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();; k( W6 {1 E9 ]0 I: b1 @
- ErrorAcc += nError*Kd ;
# {4 s, e9 k- a* u+ r: g( `5 [( S - ErrorDec -= nError*Ki ;( Q. t( X1 X2 }+ m1 P( h3 B) x: Y
- Error = int(nError*Kp+ErrorAcc+ErrorDec);5 s, k! J/ q+ R c# s3 I6 r/ Z
- if(nError < 80 && nError > -80){
+ |; c5 i1 Y7 g9 g) e4 g - if(nPower < MaxPower){
# O' o3 O- c0 S$ C2 b |2 N - nPower += 3;
6 ?3 r$ ~! T" Q - }
, X7 C! Z' d) N$ n7 v9 P - } else{# [: c c; z6 g7 j
- if(nPower > MinPower){
8 Q( O# s8 P; B6 r/ G1 | - nPower -= 2;
) v2 v9 ?( p* M6 x! G - }
# h; U' ^; |, O0 \/ {+ R, Z1 t - } 1 |1 }, l3 O6 o+ G
- MotoL(nPower-Error);$ M6 f" z; W0 Z9 l- U
- MotoR(nPower+Error);
* h6 C% O$ [) |0 b' O7 |& ?9 ? - }else{
$ q- O7 [/ |2 ^& t$ ]- v - motor_9.run(0);6 z8 q; _; n6 x8 r# N" s" ]
- motor_10.run(0);
1 T5 Z- q6 g8 T$ Q - }0 d ?3 i3 S$ t
- do{}while(millis() - StartTime < interval); B, g0 V4 {8 ~+ L$ `% |
- }
6 N& `" \9 c3 T* T2 F0 K! J
( S2 N3 a" F" ~( i1 ^- void MotoL(int Power){
9 B, B; u; M' W* z - if (Power > MaxSpeed){
+ z) U, [- g6 i3 d4 P4 s/ z+ y - Power = MaxSpeed;" }- Y4 }0 y' e7 V6 C
- } 4 N0 h% ?8 I2 _, {; a. j3 x+ a) ?1 ^
- if (Power < -MaxSpeed){5 }7 j# K5 M- }3 K
- Power = -MaxSpeed;7 l9 B: G5 d- s4 V/ c8 w! |
- } 1 _5 P2 M% u3 g3 S, @, a
- motor_9.run(Power);
- X# |& w" f+ X( O - }
( e# |% n# c3 F$ p - % ~5 c V4 [( T/ c) K- T
- void MotoR(int Power){
" c/ ^3 N+ g( c6 p" w - if (Power > MaxSpeed){
& n6 \0 O" D+ Q3 L+ m - Power = MaxSpeed;, }: [: g F3 H3 O5 H, P# D0 ]! F
- } " l1 J# w2 v% ?; h! @% O
- if (Power < -MaxSpeed){, m: p b/ E) N# u0 ^- D; B. h
- Power = -MaxSpeed;6 t2 ?% k' f6 ~0 u
- } 7 v/ N4 U: J% _. T l
- motor_10.run(Power);
9 D) L1 S8 N( q% }& K - }
複製代碼 4 ~6 ^3 R' X$ m3 s- @8 s; k9 V' W7 v
: u* A2 D$ v+ Q! \) q a+ |3 u
|
|