|
|
- #include <Arduino.h>
; `2 U9 G4 O; N2 [ - #include <Wire.h>. @8 s- {; [; N3 u" d1 C
- #include <Servo.h>% I8 H/ O; r1 y: ]. u5 ^( J; Q
: y$ ~( |2 M3 M/ D* v6 J7 e+ Z- #include "MePort.h"9 ]$ }" R R8 k* }# o
- #include "MeUltrasonic.h"
% Y a5 y5 _- @# ~+ \- ] - #include "MeDCMotor.h"9 f8 }4 i; s [4 r1 b
* Z8 S9 v, L- P' c1 I( `2 k- //double Input, Output ;
{/ |- ~) }. `! W" `5 F - float MaxSpeed = 255;
: X+ B/ w5 z5 l1 N - float MaxPower = 180;
/ e0 \1 C% h; g z* H - float MinPower = 120;* R4 c4 j% O$ j r2 `
- float Error,ErrorAcc,ErrorDec;$ N2 A5 u5 ?! h% |/ ~$ T5 r1 R" N
" B( I7 s+ W6 Y L& J+ ]- float Kp=0.14;
) J$ Y- q9 M! Z* Z k1 S/ c - float Kd=0.00020;//23;
" _9 ?7 e& e4 V: W* m7 Z - float Ki=0.000201;
8 X5 |' L( y8 F, h& q: V
$ m; R5 B' c- c5 j( H- float nPower;
' D3 e3 L# h& _. g* Y, ~ - MePort lightsensor_6(6);3 Z) ?9 j* I" G% q6 e: p
- MePort lightsensor_8(8);& @, s0 X R' @, {6 x3 n
- MeDCMotor motor_9(9);
/ G# m8 J& h" Y3 y' F; W; Y - MeDCMotor motor_10(10);
2 k/ `2 g; f* Q# j0 j! n6 }- S - unsigned long previousMillis = 0;2 s: h! ~8 f; X2 Y( Y
- const long interval = 1;2 S: c7 B; A( ~$ a! [4 \
6 j1 w, V( Y; I/ v0 O- void setup(){/ d9 L* a% A* m1 ~
- lightsensor_6.dWrite1(1);- f5 M1 v* y7 }6 D3 ?
- nPower = 160;2 i7 X! C7 _: q1 |) A W/ y
- Error=0;' R$ I5 M$ g4 G+ K
- ErrorAcc=0;
# R& R8 R, _6 i1 k7 N/ d* R - }
; s$ i3 J" r' [' ~; L - , L. J+ ]! U6 {& A( D$ B
- void loop(){, B/ L+ e9 n" U& S x0 w" y* }7 P
- unsigned long StartTime = millis();! B0 v( ?% V7 V$ W" A2 Z
- if(ErrorAcc < 18000 && ErrorAcc > -18000){# K0 t4 E M5 x; Y/ j
- float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();8 @& \2 B# p& P9 ]( X U7 w, R
- ErrorAcc += nError*Kd ;1 H; b. e, N8 v! R/ s
- ErrorDec -= nError*Ki ;
9 h [+ G9 S, @& o" c - Error = int(nError*Kp+ErrorAcc+ErrorDec);5 F* J7 N5 d5 E; ~( {
- if(nError < 80 && nError > -80){, f8 d; w* S9 F+ w
- if(nPower < MaxPower){
, I4 n4 O( z7 [; z8 @& F - nPower += 3;
( R# i! _. L$ z0 \' S% Z3 e( B - }
0 x1 R3 z5 n1 \4 j( j7 P - } else{4 G8 w. n8 q9 E& ~" u8 S6 h
- if(nPower > MinPower){( {' M6 r7 ^; E" N
- nPower -= 2;# J3 {% f( z2 [0 d
- }6 M! U+ @2 n l' d P. y
- }
2 D+ k7 B1 x" A2 Z( c) P6 I - MotoL(nPower-Error);
& ?" d, C! M7 K5 ?$ R; S/ W/ Y - MotoR(nPower+Error);
7 R) q `; t- ?, Q6 Y" [. v3 ] - }else{1 v( U4 q; J ]& F! N' w: z6 G. }
- motor_9.run(0);' g {4 l5 g" h5 P/ l) v
- motor_10.run(0);7 f3 X2 E, t) t
- }
C6 g. b" U5 x - do{}while(millis() - StartTime < interval);
7 Q9 g/ F7 v/ A; I; i" L% M4 t: { - }0 E1 C6 D& n- z) o" p' l4 a
- 3 s- Q+ W9 B8 z% x! v
- void MotoL(int Power){+ d1 P7 G2 g! J; q- z2 x6 E2 V0 L; J
- if (Power > MaxSpeed){( ?8 v& _7 v) t+ R2 O% J7 p
- Power = MaxSpeed;
4 q+ s( m- s+ h - } . D" \, x' O' V/ ^- t1 I
- if (Power < -MaxSpeed){ m: k7 _/ i0 x4 v2 L
- Power = -MaxSpeed;- g4 I/ t9 f. W; ]- f* P7 b
- }
$ R4 ]: q7 E0 C! J* {5 d - motor_9.run(Power);/ _; c9 W& D( W/ t1 I4 I. g# t
- } + H2 @8 Y5 Q$ D
0 H0 |% N" O# C& v$ I+ N5 k- void MotoR(int Power){- K4 {9 x* z7 n6 V5 K; |
- if (Power > MaxSpeed){& H! Q a; T7 f9 [' Q
- Power = MaxSpeed;
2 Y& `) B9 { D5 h9 o& A1 v - } ) E3 |, J' M( `: p. ^
- if (Power < -MaxSpeed){1 h y. c1 z8 _* v
- Power = -MaxSpeed;
& W# y! A. f/ @# x" p4 \ - }
! i9 H! }+ W- @- M+ ]# | ]( F5 N; ?( g - motor_10.run(Power);
1 o" H6 y: W0 \8 m; j! D1 k - }
複製代碼 ! v& j0 k' h: U+ `
{! I' k E! w) f& t |
|