|
|
- #include <Arduino.h>3 e9 @0 c& Y$ p4 {5 P2 i9 R
- #include <Wire.h>
+ l* _3 N" _8 ?: d# H9 F - #include <Servo.h>
1 S4 ]* i9 P/ }
0 m; D0 u4 ?6 [5 v# D+ c- #include "MePort.h"
( e% G9 P. K' Y- p7 U - #include "MeUltrasonic.h"/ @- d3 T4 T6 I7 w) G7 i& C5 H
- #include "MeDCMotor.h"
- N: ]% f9 ?' q- H+ G0 E
3 x! E1 F1 x" o! Y" G5 O- k- //double Input, Output ;$ D9 i6 S: G, {; B0 s
- float MaxSpeed = 255;
3 m! G- r3 f, C. p0 z - float MaxPower = 180;
" @ Y( a2 ?9 |7 o+ y - float MinPower = 120;& w9 {4 x$ u$ Y9 g, [0 j7 k- H
- float Error,ErrorAcc,ErrorDec;
2 A4 c- @9 f3 B4 l* c
0 F8 _8 d" {- Z- float Kp=0.14;
; ~$ z, o$ J( ]* Y' a9 } - float Kd=0.00020;//23;! B, g4 X) ]: l" v6 W ^( S! B
- float Ki=0.000201; O8 z; s# ~& \6 T1 B+ u
' [2 p( l6 Q; [- E* B- float nPower;0 D( R+ X9 S- j8 ]; G; O
- MePort lightsensor_6(6);
( J5 x' b: ~, _: p/ `' K* z0 f - MePort lightsensor_8(8);8 }1 c6 ~8 K( [. _6 S+ ]. V
- MeDCMotor motor_9(9);" x% @0 I5 b! x1 f, M3 E! o0 {' T
- MeDCMotor motor_10(10);+ k% s4 I0 W- U* E4 M4 a. t1 p
- unsigned long previousMillis = 0;- d1 ~$ I# V& D4 N( J S
- const long interval = 1;$ n, t `( ^4 u8 N
- ) O. f. w2 S% l0 ^
- void setup(){
' t4 S; v% Y1 w9 Y3 } - lightsensor_6.dWrite1(1);
) b# `, j( A. I5 _- C/ l - nPower = 160;$ B5 S3 q, k4 @
- Error=0;
( F9 S7 ?0 u; [* }/ I - ErrorAcc=0;
2 M$ d c0 l0 H, P - }
7 I* v/ H3 O6 R; O8 K8 K8 T+ |$ r. S5 V - 3 q' @; F; {# B3 R" m
- void loop(){" W% Z. r* J) E- j" k. O8 n, d
- unsigned long StartTime = millis();
8 l, r4 V X8 u, \5 [ - if(ErrorAcc < 18000 && ErrorAcc > -18000){
+ X2 }# G9 m/ U @ - float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
& r+ i, r; L2 I0 W) x - ErrorAcc += nError*Kd ;
k! r4 X( @; i - ErrorDec -= nError*Ki ;
. O. _+ l7 p# h1 Q4 |/ S% f6 [ - Error = int(nError*Kp+ErrorAcc+ErrorDec);! M- m+ ^- l) x+ @! Y
- if(nError < 80 && nError > -80){
! m3 J% l G" S& F - if(nPower < MaxPower){
2 A0 W3 j5 b& ^, X+ _* P - nPower += 3;4 D2 O! z% ]8 ?! ~2 j4 T2 O
- }7 [1 [9 g5 F8 H) ~
- } else{+ i: m' J% ~3 G0 I# ]/ A
- if(nPower > MinPower){# l* k, [, g, B: A; }7 C1 X: @
- nPower -= 2;
j% d0 S/ w7 i* R+ j; S: I - }" C9 F) k5 j h: T; _8 o
- }
! X. f* Y$ ~7 T4 c6 g - MotoL(nPower-Error);0 W; _7 `9 X# J- H7 ]) `
- MotoR(nPower+Error); . v# [$ K/ I, d& D
- }else{
# q' s! M/ h! S - motor_9.run(0);
$ B, r9 |; k& ~ - motor_10.run(0);
9 y0 Z, h U+ f6 w) O - }+ T, K- `" }+ X6 J8 j8 l! R5 a/ b
- do{}while(millis() - StartTime < interval);
0 C) G0 i, x1 k - }, j8 Y+ i6 s, u
- - O* G0 B& C& b% Q: Q) c
- void MotoL(int Power){# J7 Y, v7 \9 U& ^6 n6 o7 M
- if (Power > MaxSpeed){# a4 ^* C& O5 [
- Power = MaxSpeed;2 d+ z, R: d4 q! a
- } ) N J- g0 D5 J* j" w( N" J
- if (Power < -MaxSpeed){
, y4 a3 Y+ n' W - Power = -MaxSpeed;
" ?1 k( B4 o; d2 k8 P' z/ O - } * w3 h% J( G" [: ]" e, c& ~
- motor_9.run(Power);2 D3 W2 h( B2 V
- }
( J. o: b/ k* j! g# h6 g% ] - $ X5 K) o# i( S+ U. ]/ _8 W
- void MotoR(int Power){
4 \! ~. F: ]: s' d8 | - if (Power > MaxSpeed){$ z7 p. N/ d, D# t1 |9 h- W
- Power = MaxSpeed;2 R1 K/ W! w6 E! M/ x
- } . t. E; ?$ Z1 W2 o: e. k) p4 m! h
- if (Power < -MaxSpeed){
3 f3 a a# h$ B; D' T% k- T1 }! d7 h - Power = -MaxSpeed;+ U% f0 z; F9 i( P2 X
- }
# G' q& ?/ u( E( o - motor_10.run(Power);
: a0 R- a% G* j9 l$ f9 k - }
複製代碼 * Z( J3 h6 c9 p2 }
% F0 Q7 p4 [$ Q y: F: `3 }4 ^: T
|
|