|
|
- #include <Arduino.h>
7 Y! Y) Y* i5 x; K+ P V" R( T; X - #include <Wire.h>7 f- p& d% ~3 L7 R* d
- #include <Servo.h>
2 N, m5 k% }( ]7 j
8 K2 Q- A+ S! h3 F5 l* j; F! Z- #include "MePort.h"3 _: O2 ?6 }4 H2 {
- #include "MeUltrasonic.h"! ~3 Q$ n$ u( U) k6 v
- #include "MeDCMotor.h"* T& F6 U+ f! k6 ?% B3 ^4 [
% u; c/ y) i) d M/ ^- //double Input, Output ;$ y4 H, l2 e& _- F
- float MaxSpeed = 255;
! b: M2 W4 _7 k' W! \* _ - float MaxPower = 180;
0 O) m: W7 x/ E% ]& P0 }) Q - float MinPower = 120;. L) D, V+ A) F; Z( r: K
- float Error,ErrorAcc,ErrorDec;
' a6 g+ k R' [ - 6 G: F( S- V9 N- Q; M7 m
- float Kp=0.14;% d1 I9 l, A, V' p0 M
- float Kd=0.00020;//23;
5 f9 a; c* ?/ V2 @+ y1 Q - float Ki=0.000201;: _, ^( r- X! x/ K0 l# _
+ }/ R2 `6 x+ y- M m- float nPower;/ S0 J& R- ]$ O( @6 d
- MePort lightsensor_6(6);
: |* [1 K1 o# s8 y% a: v - MePort lightsensor_8(8); M9 n$ K3 u: O, k0 U& f
- MeDCMotor motor_9(9);
% J, p5 Y3 M; C8 `; u - MeDCMotor motor_10(10);
# o m/ F9 H7 V$ n" v - unsigned long previousMillis = 0;6 U$ L8 s- ]; ?4 F8 G' Y) \/ e9 b
- const long interval = 1;
4 \" g, C! }3 A' W/ H$ p- ~2 v: ?
+ @" g5 ^, b5 l' Y j" y G! j, u- void setup(){
" |: r5 t8 h5 x! |% T - lightsensor_6.dWrite1(1);
2 q' Z T8 }8 \" ^$ l3 Z- Z - nPower = 160;1 ^. Q# a" e0 G* v9 K0 q7 A
- Error=0;1 u& Q3 `0 d8 X% |4 v
- ErrorAcc=0;
7 L2 T: `. [$ v" _. Q, r - }; F$ @& b& b* K" Z. A! Q
- ! g% m- M% T- Y9 p2 p- s5 H, B2 G4 S
- void loop(){
! r7 M! C! y2 l7 `! ^4 S - unsigned long StartTime = millis();: i3 r/ { L; X6 j+ A
- if(ErrorAcc < 18000 && ErrorAcc > -18000){* c# v) Y9 A; D/ ?! E; @7 v4 Z$ _
- float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();& j0 S, e B, R9 v. r
- ErrorAcc += nError*Kd ;+ C+ E! |, W! w4 x1 T$ M
- ErrorDec -= nError*Ki ;
8 |6 s, X ]) R7 O" K- }! _7 S5 J$ J - Error = int(nError*Kp+ErrorAcc+ErrorDec);
$ I, }" t7 y8 p3 E# m% G - if(nError < 80 && nError > -80){4 ~* e7 ^0 B/ z. a* h$ b$ v0 H: B: s
- if(nPower < MaxPower){% J2 Q9 ]) }% T1 L! k" {$ v
- nPower += 3;* A8 f2 a: H* b) u9 k6 x( U
- }2 s" E, n, O4 g8 y( ]0 y: Q1 K2 [8 T
- } else{
9 K1 F# g3 L$ P$ W - if(nPower > MinPower){4 f" l, L; l2 M4 H2 ]+ V* T: \+ q5 w
- nPower -= 2;
" I3 P4 e2 u% W. ?5 h" T - }
+ W0 @) a* r4 o, n9 H" C, d - }
# |1 a$ \# }" G. c7 B - MotoL(nPower-Error);/ [5 K5 ]9 ~9 m+ F4 G& V
- MotoR(nPower+Error); 1 V0 U8 c% G) D, o: u) H
- }else{# p; L0 b/ r6 D- ^ }, a
- motor_9.run(0); p$ S a6 I! Z/ i( o
- motor_10.run(0);& s; J2 L6 U$ m3 P/ L- o8 |+ @
- }9 A3 p' i, e$ V% H
- do{}while(millis() - StartTime < interval); ]( ?' C; [1 b& r/ H9 W, B9 J
- }+ |9 K/ f' G" N" f: O7 i
- 2 [6 Q# O4 X8 C( ^# x! M! }& Q
- void MotoL(int Power){
, {9 b8 |( S, Z- R5 ]* z* Z0 \ - if (Power > MaxSpeed){
0 S$ r0 T0 k9 h - Power = MaxSpeed;3 t) ]0 b; x/ y! g$ b) |3 B7 W
- } 8 r3 G3 U6 B+ B) N; V2 L6 {
- if (Power < -MaxSpeed){: H7 l/ k b s P
- Power = -MaxSpeed;
* Z, E q, X; @$ H/ I" H - } ( H! {$ H2 t7 b5 |
- motor_9.run(Power);
& V& t5 w3 }8 M5 A. q( r' r. ^ - }
5 @$ D& q. }$ } ~6 w
0 A/ {0 s0 m: v! L, E- void MotoR(int Power){
, p2 ^3 U/ I2 l/ d4 ~ - if (Power > MaxSpeed){
1 [ [6 }0 ^6 x5 \" ?4 C - Power = MaxSpeed;6 d, c: Y* ], n8 ~& g2 }9 O7 I) G
- }
# D' A5 S8 v- D& F/ e. k D; a9 L - if (Power < -MaxSpeed){
& y, k; m( t( c# J u, {9 f* c - Power = -MaxSpeed;
9 n9 b K8 e2 M8 G7 D! v9 n - } 4 y! }3 t5 T/ }( l
- motor_10.run(Power);4 ]! q: l6 U( J
- }
複製代碼 4 t% Y) ?# }' F) k( ~8 g9 G
5 I. J" W7 O$ {9 B4 { |
|