|
|
- #include <Arduino.h>- j* Z% e X6 h4 S
- #include <Wire.h>+ }" _* o+ I3 d
- #include <Servo.h>
7 J& T4 @+ O! F0 e6 {: }) K5 ~
' u+ ~* k/ @2 z8 }( f- O- #include "MePort.h"7 w" @0 S1 z7 }, g# K( y9 M6 e
- #include "MeUltrasonic.h"" ?+ [' _ J8 s) O# \
- #include "MeDCMotor.h" T# A( r4 @0 D
- # D, g/ U1 {( L, U4 L
- //double Input, Output ;, H# l; J0 Y r. r) Y3 r: r. `4 g
- float MaxSpeed = 255;
- g. y+ x' E. V$ w$ A - float MaxPower = 180;5 |# P9 Q) O3 X% D' I
- float MinPower = 120;# b3 Y" n9 _/ ?9 u( k9 ?
- float Error,ErrorAcc,ErrorDec;
! L5 `# }$ r5 b7 v7 Q
- u$ C; D1 _6 |2 W5 s4 s3 y. ^, g- float Kp=0.14;# s2 \8 y) k* P7 `0 h5 |+ o0 P+ s
- float Kd=0.00020;//23;
1 V" o% Z" k$ R: q - float Ki=0.000201;; }! s4 Z, `4 d- r) o5 k
- 5 j5 l" Q; a4 e. r
- float nPower;
+ Z3 m' G r0 p0 k: F8 D - MePort lightsensor_6(6);
& i- q& K- c5 ^; k - MePort lightsensor_8(8);9 e/ P4 R3 S) v/ W' a
- MeDCMotor motor_9(9);
! I9 i/ H* r1 i2 m' c - MeDCMotor motor_10(10);* q. j. R* C( F( I. T
- unsigned long previousMillis = 0;
+ T; O9 W( ]' E - const long interval = 1; I- i' L& l& ^9 }: `
- . ?7 L! g @' H% M# X8 _! t# H
- void setup(){
; Y9 B3 F$ D; @/ W+ \- P - lightsensor_6.dWrite1(1);- U$ R b* v) l: J" w6 \) v3 o
- nPower = 160;
5 Q/ u, |# f, t- B - Error=0;0 H; I. z; C0 {: b
- ErrorAcc=0;' ~5 p1 s) X& H$ ^7 \) w( z
- }
# \, }7 H, @! [; A. v, x: s - ) `0 c+ M* B! [* r
- void loop(){
# `' \+ r" ?4 S! c - unsigned long StartTime = millis();# z3 r; k0 Y& C7 J K/ O3 e7 C; o
- if(ErrorAcc < 18000 && ErrorAcc > -18000){- l7 E5 J/ t- N$ Z. }( s4 e1 o4 e
- float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();+ s% L. B5 U1 O( e; d
- ErrorAcc += nError*Kd ; F& O- C5 d6 X, g4 O
- ErrorDec -= nError*Ki ;0 D& v7 T3 A$ G3 B
- Error = int(nError*Kp+ErrorAcc+ErrorDec);
3 d1 H% C$ u0 a: F% M) M - if(nError < 80 && nError > -80){
: _7 m' w* G4 U4 ^ - if(nPower < MaxPower){/ G3 O4 \- y( C, `0 n
- nPower += 3;
2 I# x5 Q8 N8 B - }
t) Y% { h, y- x! I/ O% | - } else{
: ~ g# U# w1 s# a/ ?; w7 S - if(nPower > MinPower){
# }- }" B9 ?+ x* d, y - nPower -= 2;
3 N7 Q5 h4 A3 T) a - }" K% M3 r. s- N5 A& {* e V
- }
; e {7 i9 ]6 D* }. v$ f3 T& k# r - MotoL(nPower-Error);; N- J9 d' E" q$ f
- MotoR(nPower+Error); ; i) {6 P( F8 e+ z0 {( m/ l
- }else{
7 v/ n P0 f% t - motor_9.run(0);
/ d* w s3 t; z) I5 D j - motor_10.run(0);
! o4 L; P# [: i! P, F) p: V, @$ l - }
& C. J9 _6 b4 Q - do{}while(millis() - StartTime < interval);
& j- A# f! x1 B1 }6 c - } E: v' {3 E5 Z; Q5 [- l3 @) g
1 n% D. m1 b# z' _. X% m* w1 U- void MotoL(int Power){! A9 {' X- e9 G7 z
- if (Power > MaxSpeed){
O& K9 T4 S. K, @7 U) A0 o - Power = MaxSpeed;7 W' \9 J& Y' n ~# ^8 u
- } z* h5 \$ j/ s& N5 E8 d
- if (Power < -MaxSpeed){
; t& t8 f. r. v - Power = -MaxSpeed;4 G0 k$ v5 Y4 t% V8 v6 U/ H
- }
8 S9 |/ J" h+ n* B - motor_9.run(Power);
* {( O- ~; i2 f/ } - }
4 L, ?& O; n y# v" q% A - ! g% L; \! H7 U3 h
- void MotoR(int Power){4 r. P# t5 f! }: q1 m
- if (Power > MaxSpeed){8 _( x1 k, O; F D
- Power = MaxSpeed;( r7 @. f! G$ ^* o$ V
- } 4 Q, }) q1 X: I5 i0 e8 `9 L4 e
- if (Power < -MaxSpeed){/ b Y# n/ V7 H3 e2 S/ {% c! d v
- Power = -MaxSpeed;7 c% k% g5 w* }/ @
- }
0 e# g0 D; r; n1 i. @! S+ F8 K - motor_10.run(Power);
; {+ C1 Z% |: o, j: ~ - }
複製代碼 * t' N: l r2 O% }, e# x2 ^3 r
; w1 P/ t) O L T% V# h
|
|