|
|
- #include <Arduino.h>* H! a& K- }( ?
- #include <Wire.h>1 F$ m% r: X, q* f0 p* n
- #include <Servo.h>; |& t- F+ X: O; [' ^" t# v
- V, o: f' g; R2 K" g* R N8 m# X
- #include "MePort.h"
$ A& O' B' G0 \4 a( x, d" Q - #include "MeUltrasonic.h"' ~2 r/ B# t# ?$ t$ G$ C6 H
- #include "MeDCMotor.h"- v) f3 [9 n- b: D0 J
- }) ~, a/ ]7 k! S- //double Input, Output ;4 @; y; h- Y2 q, {
- float MaxSpeed = 255;
. u. u+ V) I+ G1 Z; I& I0 q. ^4 g4 C - float MaxPower = 180;
2 j. @. Q8 g) Q2 g - float MinPower = 120;
* S% c+ g) s' p" G3 D - float Error,ErrorAcc,ErrorDec;+ j- @* ]" Z$ M' h5 M
6 }. n q4 |) G9 T% j( f3 `- float Kp=0.14;3 L( l. r/ g% J1 f3 z
- float Kd=0.00020;//23;
9 L( g8 N5 j6 R( i2 f& W - float Ki=0.000201;
! {# i: d, M0 C, P' h3 \ - + Y3 U$ g+ U, Y1 ?6 ]$ H" V
- float nPower;' I/ |9 A" Z; Q0 \6 N# R5 U
- MePort lightsensor_6(6);
5 {1 C) j/ c& M' ^ L - MePort lightsensor_8(8);' r% {9 a9 S4 o' S7 T0 Q. |, { q7 b
- MeDCMotor motor_9(9);
4 J* Y: E1 y/ J - MeDCMotor motor_10(10);
: p/ a3 ?; r4 R+ B/ J( { - unsigned long previousMillis = 0;0 o0 B" f0 o& q
- const long interval = 1;% q2 K% l) {) J, }
- & t9 h4 m s: D; `
- void setup(){
6 k5 z( N8 }! t3 r1 B - lightsensor_6.dWrite1(1);# c4 O5 s0 L! v, ?# t. S
- nPower = 160;
, B6 [( z6 r4 M9 B - Error=0;
0 R0 x! g8 X6 w) e; ^ - ErrorAcc=0;
: {4 j% f- ~" M2 G) z - }
$ J' m/ v& ?- F; s) F0 ?2 c7 O5 q
9 z' d% y/ x2 c' I; }1 Z) j5 ~- void loop(){
* i( q" J) |1 m) x w - unsigned long StartTime = millis();2 J0 ]% w3 U( i' U& t! Y4 c
- if(ErrorAcc < 18000 && ErrorAcc > -18000){
5 M8 z; S7 D% I4 w* M. ` - float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
% A! {" n# u) O( E& i9 Q. G - ErrorAcc += nError*Kd ;
% k0 G& a: a2 ^# V! M9 ?2 I7 u5 D; @ - ErrorDec -= nError*Ki ;4 [0 P9 \2 u0 i% m# _
- Error = int(nError*Kp+ErrorAcc+ErrorDec);. V; b! d1 |0 D* J6 u8 _6 m- e
- if(nError < 80 && nError > -80){
; ^9 D6 [1 H+ }* W - if(nPower < MaxPower){
& |( f7 }1 a# x - nPower += 3;! Q) x9 K3 D E# G$ h
- }& ?, G: I4 |; I( h* V
- } else{
( b" l( O) ]1 a$ m* j( ^+ w - if(nPower > MinPower){- Z8 J7 ~4 H2 q; T' Q% L7 k
- nPower -= 2;4 V1 G; Q" x# E S; m8 q' p, `3 M
- }
! a! w' C+ F! V - } 2 c8 w8 c% O) ^7 G
- MotoL(nPower-Error);
& {) h& ^- w7 U8 m& }% _! a: D - MotoR(nPower+Error); ! d4 b5 y& R6 f( E! s$ p
- }else{6 e& r% ]$ ^: b
- motor_9.run(0);
2 p8 _6 u' I# u7 _3 c/ n - motor_10.run(0);) s0 {: {$ g. d/ f7 O1 H
- }
5 U* @% ^* ~+ E" ~( @ - do{}while(millis() - StartTime < interval);
8 R% g$ P b3 {1 B) _& t - }# j- ]1 x: l8 X# Y
- 9 _1 A1 `0 t; m" ~
- void MotoL(int Power){$ |1 P9 R0 y4 H5 s' t
- if (Power > MaxSpeed){! @+ `0 R) H* E
- Power = MaxSpeed; M( |) W& _. }3 U& E
- }
& \* n, P; ~" Z4 I - if (Power < -MaxSpeed){
* t" C8 C. M% S' S: ?' D0 p - Power = -MaxSpeed;
3 i, d# p9 _. U - } " H& X9 \4 U! z+ F/ M* e: p% }
- motor_9.run(Power);
: m. s, H1 O- s, I& n+ G$ m - } 6 m" G i: g, ]) w& a. e0 V* J
& M/ W$ ?! _) f5 ^) N6 i( d, y- void MotoR(int Power){
3 A0 C; _: x I: d: O - if (Power > MaxSpeed){
* K* x- |. P G0 Y- y( f - Power = MaxSpeed;, j6 k! Y `' B, \
- } 1 j0 V% O8 q. w& }
- if (Power < -MaxSpeed){
0 v! g9 R8 r. x4 \, k" t - Power = -MaxSpeed;+ h+ {( P) f6 z8 |- L2 F+ ^
- } 3 ^4 j. A( |% A+ S
- motor_10.run(Power);6 k5 Y! D' W' C3 ~+ G
- }
複製代碼 6 w/ L+ l* {0 X7 @- d8 \
9 x! J' E5 D2 N( s |
|