|
|
- #include <Arduino.h>
2 @/ h% ^% N" _+ J - #include <Wire.h>
6 o, G9 J$ Q1 L - #include <Servo.h>; n' i7 [* p) a& t. D. _
- - @5 v: B6 x {9 _; t2 V g0 U% F
- #include "MePort.h"
% {0 z( k! }! N+ u' ^! z - #include "MeUltrasonic.h"
8 r+ D% m! n, a, D. l - #include "MeDCMotor.h": m0 C8 `! @- i+ r
9 ~7 E- A+ ~9 U1 |* t" n2 P- D- //double Input, Output ;
5 U4 B4 D& T% |1 j - float MaxSpeed = 255;
/ R% v* y1 j N# {/ @0 k- m3 _ - float MaxPower = 180;; d: Q: V9 ]. J& l; P/ ?0 A1 j
- float MinPower = 120;8 l: G* \5 v, E4 W# ]9 t7 r2 c7 a
- float Error,ErrorAcc,ErrorDec;
, r% [5 s( A4 F8 R- @; t g$ v3 i
$ J7 G3 _: S/ D9 T3 {* o0 \4 F( t- float Kp=0.14;7 N9 w" ?: \' H" b
- float Kd=0.00020;//23;
3 I' H+ Y7 J; X3 I& Q E - float Ki=0.000201;
8 V5 W+ o2 q# m - - N1 M( D& q- q) c2 x
- float nPower;5 m! g3 }( ]+ H7 u0 r
- MePort lightsensor_6(6);8 u* K" w' R# c5 J$ d( X( N6 u
- MePort lightsensor_8(8);
/ a' A* i, p% M; k; u6 D6 L* Z8 B0 ` - MeDCMotor motor_9(9);$ {# [5 j' ~/ S" Z0 @: W4 S
- MeDCMotor motor_10(10);' P, F- c b- J+ Z" ~# ?# x9 P
- unsigned long previousMillis = 0;, l, x6 L% M2 \( ?) a0 F
- const long interval = 1;5 e# p6 \4 |( Q, w. I ]
- G, Y: y/ L4 i# A' Z+ t- void setup(){
& T% X+ N2 Q d3 k9 H( u% | - lightsensor_6.dWrite1(1);% b4 P e) }7 ^7 p0 Y+ T, Q8 F" x* s
- nPower = 160;
. P1 L3 g, N8 w - Error=0;
, C. @- Q( W; y7 j - ErrorAcc=0;
: K% E( V6 }5 N# m( F5 s) n! Q+ E - }
' {* n- I9 e# H# P
' d" R6 o t- ?$ S3 G$ q- void loop(){" p7 ^! p" J/ ^. A/ a6 }
- unsigned long StartTime = millis();
4 d1 h: `/ }* X! {6 [- B - if(ErrorAcc < 18000 && ErrorAcc > -18000){: y3 L6 ]8 n' E4 D* M4 b
- float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
+ W' Q ]) M% i( L$ v% t# G# ]7 d - ErrorAcc += nError*Kd ;
+ P+ ]) b# a1 t2 w+ j- {7 L - ErrorDec -= nError*Ki ;. T# H& {# ^' m$ E8 S5 o
- Error = int(nError*Kp+ErrorAcc+ErrorDec);+ K; q* I: j4 O$ G
- if(nError < 80 && nError > -80){. _! N7 k- U% ^ f$ [# l" o
- if(nPower < MaxPower){
$ x+ _ \7 K, P( n3 F; y$ ] - nPower += 3;, N# B* _8 r. s6 G
- }
9 V3 m% e' |3 @0 O5 l - } else{
, o$ z T$ d' W' N; {, C - if(nPower > MinPower){( |) G& Y# S1 x" N4 x" O
- nPower -= 2;" J2 }1 Y4 t. t' [2 G+ O; B
- }
; U+ T8 b# a0 M: V/ l% m$ b - } $ m9 }* z4 G, {2 C; [
- MotoL(nPower-Error);( ] k$ |) X8 }, l0 d; T+ I1 F: x# ~4 |
- MotoR(nPower+Error);
. G+ I( l! q- P; N% O - }else{3 A' N) ~% [7 G7 s( U, H+ E
- motor_9.run(0);
% N" y7 E6 V: ~' w$ t& v - motor_10.run(0);
, t; o1 H' D5 } - }, b3 w) m* q9 D5 L0 y' y
- do{}while(millis() - StartTime < interval);! \" _' _4 N& q( q, c
- }! ]) ^. I2 U" @9 k# a9 t" D5 S
. [9 q! T/ P3 k- void MotoL(int Power){
. q. p( N3 g2 k1 M3 R: Q - if (Power > MaxSpeed){. |9 k) f1 h% D2 h$ o: F! A
- Power = MaxSpeed;
: a- P' t( l9 I - }
( M) T2 `1 }$ E& m3 G - if (Power < -MaxSpeed){
7 y3 \9 M5 h. p, H - Power = -MaxSpeed;; _( C' A( D7 }" `
- }
2 `0 r7 z! K( l F4 F( Z - motor_9.run(Power);; w9 i H! t5 F( {8 q% [
- }
: l [9 y8 v' `) m; T- y
0 r3 C; \# u' D( w6 ^2 t% H& C% S- void MotoR(int Power){
) j$ `7 j" O2 G5 q- ~: o9 T( J - if (Power > MaxSpeed){
, G1 I/ Y. S7 h$ {: g - Power = MaxSpeed;. A, y. C, g8 Y3 ^$ E, B: N$ A8 Y5 F
- }
5 G6 Y4 g" v5 q$ a0 |* P3 {+ C4 P) f# w - if (Power < -MaxSpeed){
- v3 l- {7 Y; |4 H2 V( @ - Power = -MaxSpeed;
/ S) F% f# u) J/ v2 u - } ! R2 \ e. E, I
- motor_10.run(Power);0 r8 w6 L: H7 a0 K5 x* N
- }
複製代碼 ( [9 R/ A7 i3 ?' d
* t6 A5 e# x/ J+ b
|
|