|
|
- #include <Arduino.h>- ]+ }$ P0 Q7 M
- #include <Wire.h>
: o( s9 [! A% p' I2 X" u/ w - #include <Servo.h>3 U" O. g7 }- h4 o" x/ c: z2 O" \- ^
* }( e; `; I, L. y2 O: B1 n; `# @- #include "MePort.h"2 }, t; z; `% J, f, k% V5 H) ]/ M
- #include "MeUltrasonic.h"
. P% K8 [# c/ T8 Z: s% U9 {& ` - #include "MeDCMotor.h"
]1 K6 U; K6 j+ n6 S- A - 8 R6 D7 e& `# c# g
- //double Input, Output ; |# G0 q" Q1 I5 R; M* |; T8 t5 ~( P
- float MaxSpeed = 255;; B: P( H2 ~* y8 R+ i; Q
- float MaxPower = 180;1 _! }: F; e# R% i2 \* a r
- float MinPower = 120;
, b8 T5 _& b# G% C7 y - float Error,ErrorAcc,ErrorDec;* p# E! v3 C! H1 W
5 o0 _; O v# Y+ m- float Kp=0.14;
& {3 k8 T: H1 G9 h0 X | - float Kd=0.00020;//23;4 f e2 E) a; C r4 U+ | G+ Y
- float Ki=0.000201;. R) U2 a2 z1 v' p8 c: [; [
- # C8 x- c% i( F3 Z8 F
- float nPower;$ K2 } D& X5 T8 s
- MePort lightsensor_6(6);/ m- ~- m6 _+ W/ [( Y
- MePort lightsensor_8(8);# h9 f) a+ ^- H. q% [' a, f
- MeDCMotor motor_9(9);$ ]- M$ w+ c) R& {1 E3 U
- MeDCMotor motor_10(10);# r' h3 F, p* H' v
- unsigned long previousMillis = 0;
0 R: k" f* m5 a5 e - const long interval = 1;
r& W9 H% M' ^* P# C - ! b- }! Q0 K& V; n, {+ y. g o
- void setup(){
7 ~) X# `( L# s0 t& Y& K& P" a% g* C - lightsensor_6.dWrite1(1);' o$ L" C, v' L# ], P6 H2 d3 H y
- nPower = 160;4 ~: w( |9 c% O5 T! `* D2 J
- Error=0;; D' Q8 p, t( v
- ErrorAcc=0;
2 D1 ~3 \$ r) Z5 @& m( | - }) L% X6 ^4 I! i# a" U1 ? C
- ; L; _- y/ n% v w2 {
- void loop(){
' @, k7 V( U1 \* M6 l4 d! w - unsigned long StartTime = millis();0 B( ]: D6 [# k x! ^ c
- if(ErrorAcc < 18000 && ErrorAcc > -18000){+ B& q! Z6 I# a9 [# m) h" y
- float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
8 n- `* N# ?2 Z- e - ErrorAcc += nError*Kd ;" ]0 o1 x( k; s
- ErrorDec -= nError*Ki ;
, y/ X2 a! I! G6 k7 ` - Error = int(nError*Kp+ErrorAcc+ErrorDec);& U& U0 ?2 H1 {8 O% o) t
- if(nError < 80 && nError > -80){
4 e, s8 F2 p$ c- X. F4 W - if(nPower < MaxPower){
+ g- H/ V. h/ w, [4 `8 t - nPower += 3;
?/ A9 ]. G0 y. D - } v U) v) ?) x& f) H9 O) x1 o1 G
- } else{0 L) h: e3 V: H& b! g2 I
- if(nPower > MinPower){6 X8 |8 f j) n( m7 M
- nPower -= 2;0 B/ F% j% N5 H" L3 Y2 q9 \: L
- }
5 r, M! L7 s2 t0 f& ^5 e, i - } 7 t8 R8 O6 q8 q! U9 i
- MotoL(nPower-Error);& [3 ^- e x6 {1 Z8 H
- MotoR(nPower+Error); ; w3 e1 i6 W6 Y( Q4 [4 s7 ?: s' A |
- }else{6 q9 @1 \+ H! `
- motor_9.run(0);
4 J; D5 F) W$ m; B7 E, ^/ V% z - motor_10.run(0);
+ J. P3 L0 z, j# T - }
( [7 {2 X/ }/ e/ \ - do{}while(millis() - StartTime < interval);
" @ |, I( i' d* ~3 B* ^3 X; M4 h - }2 e) m$ G5 V3 m+ |: u0 S
# t8 D6 e! R; R- void MotoL(int Power){
" ^ I8 T: D+ i - if (Power > MaxSpeed){
1 |: y$ _! {/ U4 V4 j, O5 }, L - Power = MaxSpeed;
9 ]2 d3 X! M: O0 b3 z - }
! ~( @# \8 M2 i3 b3 S. y - if (Power < -MaxSpeed){
% v; L8 E# S9 V - Power = -MaxSpeed;- d' U! i E( g
- } + {6 ^% I! W7 B6 R' Q3 q
- motor_9.run(Power);
2 o" U% Z, O9 s0 A: D - } * n; s- h2 Q) M$ I
- - e$ @; l- z- o% l
- void MotoR(int Power){
. L N% w& Q" L1 Z1 O9 Q( @ - if (Power > MaxSpeed){
5 N, d* _& c: r; w: @0 z [ - Power = MaxSpeed;
) b! }0 O7 {/ ] C - } & [ a8 q( L4 E' Q
- if (Power < -MaxSpeed){
+ c# x# m/ m) z - Power = -MaxSpeed;
5 I% b, Z7 ^: `8 r0 z2 z4 { - } ) H5 p! r K+ h8 k7 ]: P5 ~( ^5 G q
- motor_10.run(Power);5 W. }( b N7 ?& u
- }
複製代碼
: t- r8 Q+ F! y0 w
/ s% Y( S; t4 z* U# ~1 M |
|