|
|
- #include <Arduino.h>9 ^4 H. ?; I0 t3 r1 U
- #include <Wire.h>
$ c, x3 {9 [6 g( `3 w T* M - #include <Servo.h>
/ u* n X: X p; ]; Y1 p9 w
3 N3 ?& ?/ u8 y. H" ^5 @* X) v- #include "MePort.h"
+ }2 H( W/ D# m# Z - #include "MeUltrasonic.h"( T* [, J: P$ c% S8 X; V
- #include "MeDCMotor.h"
% E* {. J" t) d/ W! J x* L
' U4 Z2 B; n( Y X2 s) N- //double Input, Output ;
: J5 F3 q' q% _2 _ w - float MaxSpeed = 255;, E8 b: v- U6 h( \ D5 @3 N
- float MaxPower = 180;/ S! d2 [/ R; M
- float MinPower = 120;2 k( P4 z8 G# e; R
- float Error,ErrorAcc,ErrorDec;
$ l6 E3 o9 {$ _3 K7 E' I5 [
0 [% W ?4 V: o& T x. [. _, v- float Kp=0.14;
4 B, y9 K% L2 K+ f, b7 \4 m - float Kd=0.00020;//23;( Q& r7 U& S, d! E& D% h
- float Ki=0.000201;8 `0 Z0 R! R: z6 @1 d1 J* F' b
* M9 ]" _6 |& s+ Q* ?- float nPower;
' S4 U1 X5 O4 A* H V% n2 E - MePort lightsensor_6(6);
1 {$ K( w$ X% Z - MePort lightsensor_8(8);
: f5 ^: R- }5 A1 \6 E/ Q" [$ m - MeDCMotor motor_9(9);
# x% R8 r6 Y7 A; K% v( q - MeDCMotor motor_10(10);
' @8 g3 @( z2 B. u! r! n) E$ z. O - unsigned long previousMillis = 0;
9 [) v7 I6 Q7 U* m2 p - const long interval = 1;5 [; f8 c! |9 N
3 Y- T9 h# x+ D5 i- void setup(){
% L' ~( S7 W$ L/ ?7 j+ k5 Y1 i6 v - lightsensor_6.dWrite1(1);
) K) L! n. u- a8 O2 V - nPower = 160;9 U0 m, X+ c7 g
- Error=0;
+ ~7 t8 r3 y. n8 X3 @ - ErrorAcc=0;# n4 ^2 V- `6 p
- }
9 F6 \) E3 q/ I3 s7 R, T4 L4 j$ @ - $ V6 ]8 h% v) G6 v3 d% E _2 E
- void loop(){
/ N5 o3 W7 Z5 c$ w; ? - unsigned long StartTime = millis();
$ E. L- _; ~! I8 y) p" O3 ~. v - if(ErrorAcc < 18000 && ErrorAcc > -18000){% U1 q# O; f8 m6 L
- float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();& F4 l5 }% _. w k. H" j
- ErrorAcc += nError*Kd ;
% C( j% w2 b5 ~ - ErrorDec -= nError*Ki ;- v2 N1 G$ h3 l* [6 ]: H
- Error = int(nError*Kp+ErrorAcc+ErrorDec);0 |6 S" X* ]% g1 ^; z+ q# S
- if(nError < 80 && nError > -80){0 j2 @, u0 r& i3 W, R
- if(nPower < MaxPower){6 e# g% m, T+ L: ^
- nPower += 3;- I3 Z! i3 a# p `0 L$ _1 s) u' |6 ~% H
- }( R8 n2 ?6 O" m% ?% U, L3 q
- } else{
$ z: n% \3 m U7 A9 m - if(nPower > MinPower){ ?* ? p7 J) v0 E6 W
- nPower -= 2;& v$ b: z" S# s- a8 H& g. X
- }
8 p/ s8 O: J( [* F, p) Z - } 3 c: [) P% s2 i2 u
- MotoL(nPower-Error);
X# R3 V) `5 p - MotoR(nPower+Error); # F3 D+ A1 g- C- j! J; v {9 v, T$ O
- }else{3 L* O( F# b4 w9 ]1 s$ G2 V9 P
- motor_9.run(0);5 t2 h/ i6 m$ d. v
- motor_10.run(0);! U8 c! k& ~0 X
- }! a9 D5 C# C _/ N( `- T1 b9 }
- do{}while(millis() - StartTime < interval);
n- z# w8 y, l0 H$ M6 ~, V - }# Q; v* z- Y/ [4 C+ r
9 j1 ^# y( y) `4 w3 R- void MotoL(int Power){0 M5 t0 R5 b g L
- if (Power > MaxSpeed){
# c5 Q$ L* W: e' ~1 S - Power = MaxSpeed;9 | g/ N, B1 O: J. G
- }
4 A; b3 E$ E+ I- ~ - if (Power < -MaxSpeed){
: M+ o4 I1 P8 _9 o0 L) a. E# b+ N* t - Power = -MaxSpeed;
9 d {4 }5 `2 r - } # C$ S/ l3 O/ J* Y! M `
- motor_9.run(Power);/ G- r, m r$ ?1 s0 W5 j
- }
! r/ @( p. ^ _2 e/ m7 ?1 `8 X9 ~
^' `; l* e( O! K% L- void MotoR(int Power){
! t; P9 M% i& v% N0 s, H! _; q( ~ - if (Power > MaxSpeed){
# X- ?7 E8 b3 n/ ^, v - Power = MaxSpeed;
$ L! a$ ]5 n: T' i4 } - } * K& t" @# [7 ~5 _3 L
- if (Power < -MaxSpeed){0 c- y! A' l( S( F/ O7 j
- Power = -MaxSpeed;
$ ~7 x" X8 y6 [! h! J/ y- F - }
0 W4 ~# }* B. k' U" y% C+ r! P) a$ X - motor_10.run(Power);' C2 v: {" v* n5 q5 n
- }
複製代碼
2 S6 M; {* L. h1 g" V) }* P/ T( W" R, D( h& B7 `
|
|