|
|
- #include <Arduino.h>
4 `% m/ P) O7 z1 `( b) m. E% z - #include <Wire.h>
% n! U) t$ J2 j( n- W" ^ - #include <Servo.h>1 z" i3 p% L7 i* i7 c
~* r8 l* d6 a& G3 l3 v$ J- #include "MePort.h"% b' l% K9 q- w4 o+ n6 x, J1 G) c/ O9 G
- #include "MeUltrasonic.h"
! Z, z9 A: E/ G, a' l2 u3 E - #include "MeDCMotor.h"$ n. p* y# l, }4 @9 w2 d- S% r
) i4 [0 x0 K, S6 k9 S0 y6 i$ o! Z- //double Input, Output ;! s5 O, e, z" ?; O
- float MaxSpeed = 255;/ }# E3 `1 U% Y9 r- s
- float MaxPower = 180;9 V8 H! L0 H7 i& Z4 N8 K2 f
- float MinPower = 120;2 F" r! P, B9 P. D: @( X
- float Error,ErrorAcc,ErrorDec;
3 d6 M6 ~5 G3 W9 i1 o
4 ~) y4 r9 K+ v) o. }9 r7 {- float Kp=0.14;( ]8 S! F9 P1 R
- float Kd=0.00020;//23;( e2 ^( E1 {6 C: |4 N$ S
- float Ki=0.000201;$ `/ g0 x7 M: `% o& `0 R' v) l
; Q) R, Z% W' ]6 {) j9 o2 C$ C- float nPower;( T. c. `+ v' @8 L
- MePort lightsensor_6(6);/ T& i* O" d7 |, p X
- MePort lightsensor_8(8);
; X* K3 T( _1 S) i1 e! ? - MeDCMotor motor_9(9);
+ H6 R3 M! ]9 w - MeDCMotor motor_10(10);
9 s% ^: y0 E, z$ D - unsigned long previousMillis = 0;2 w4 @4 l: N( p% H
- const long interval = 1;7 n$ r1 L4 o2 K: Y! c9 p, ?
- + R& |/ E; ~# Y* A( |- ?; u
- void setup(){; E! ^, `! R1 o8 W
- lightsensor_6.dWrite1(1);& z3 B3 V& c9 w5 f5 F" }' r
- nPower = 160;7 Q$ z' j! @! w# _1 o ~9 ]9 }
- Error=0;6 D3 p5 U0 T; \8 r+ ^* u2 r) Y* g
- ErrorAcc=0;) F- s6 }9 B0 G o& v/ ~! q
- }2 ~* c* O. @7 B" ]5 |
% U6 T! E x6 L, I8 g ?, u" e6 ^# f- Q- void loop(){
8 T Q5 t. B0 V4 t - unsigned long StartTime = millis();% R+ x, ]+ M. X: A" G
- if(ErrorAcc < 18000 && ErrorAcc > -18000){$ U+ D( m# ?9 N
- float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
- ^" \* z3 O8 ?1 a. p) @ - ErrorAcc += nError*Kd ;
! k- s0 d6 k; m2 L' R1 o - ErrorDec -= nError*Ki ;
$ |0 M- k7 C0 B2 ~/ w - Error = int(nError*Kp+ErrorAcc+ErrorDec);8 f! Z! w* p* E; h* T, {, e
- if(nError < 80 && nError > -80){$ W, R. v, a6 d# f: W
- if(nPower < MaxPower){
9 R7 J' l. b3 F3 `+ _ - nPower += 3;
6 i' p6 j: B4 b/ E/ p - }
& `2 W6 j+ T0 S% p2 e. ?! `0 B - } else{
* v7 I' u ^* G- S; | - if(nPower > MinPower){7 B; P* u- X! e: c
- nPower -= 2;
, ^6 t. W% f- H+ [* n f - }
5 k7 u( J" x) o" A8 ^ - } ; C( C0 S2 Y F8 q) I
- MotoL(nPower-Error);0 l3 T/ a8 b, x: T3 ?
- MotoR(nPower+Error); " Z* {6 g6 F7 h% A0 J
- }else{+ M/ X. R2 X: Y) U$ E
- motor_9.run(0);
; E" k* l+ G4 Y' K - motor_10.run(0);
9 A* A, R. u) B$ \, d D) ? - }) P' a2 b+ W5 p. z) T
- do{}while(millis() - StartTime < interval);8 |6 R' h7 }8 N4 `9 q
- }
d5 Y' i* ^9 U# ]7 y0 I
# ~8 M% W5 |. a' q, h( P- void MotoL(int Power){
& d3 b7 b4 z7 I - if (Power > MaxSpeed){" E1 e; u5 r+ l& t9 u7 F
- Power = MaxSpeed;
- ?0 {2 I6 T- T/ g' p5 s - } # j$ h: b y$ ~* d
- if (Power < -MaxSpeed){! }# o6 W& m9 T% S$ [
- Power = -MaxSpeed;9 z6 _; H( S+ \9 E1 ^# v: h* K! M8 ]
- }
6 J, N- w5 Z+ m6 p3 i) [3 o - motor_9.run(Power);
- X2 q% Z3 V7 {, w4 a - } 0 x0 N$ o- ~" j- j
- - L T3 r/ ?/ j1 {
- void MotoR(int Power){" n5 s: @# k1 m, }2 {/ _
- if (Power > MaxSpeed){
0 c4 V+ T( V! f, \* o - Power = MaxSpeed;8 I& u" b: K3 [# J8 {9 G8 \
- } . a. A( @1 Z6 Q: S3 B
- if (Power < -MaxSpeed){* t# } n4 K$ {* T- C5 G
- Power = -MaxSpeed;
2 L: F- E' ]5 @0 G' z0 U - }
, C& r% N6 y7 l k) v' g6 p - motor_10.run(Power);6 }0 I. m1 E, x) y5 Y# ]
- }
複製代碼
4 e; J; H1 y& l3 Y% n" s8 L2 g* v( s$ J$ ?) d( c# E8 Q2 ?
|
|