|
|
- #include <Arduino.h>
- e/ ^% o2 J. t - #include <Wire.h>
1 a+ P9 w8 g; w0 c2 @- ?& Y - #include <Servo.h>
8 J* c8 {( T) i+ }) T
% L) I K) [! J( b5 ^- Y3 s- #include "MePort.h"
# v! _& N% }# X# T3 I) E& w - #include "MeUltrasonic.h", L$ C! J8 O( Y3 l. D: {
- #include "MeDCMotor.h"
3 z5 s) `- \8 @0 \ - v/ {- _( Y- l5 |
- //double Input, Output ;) i5 L* q# s; E( B
- float MaxSpeed = 255;6 S q( C2 V- w- m
- float MaxPower = 180;% b# s6 [1 `, H% a, B
- float MinPower = 120;
7 m) m1 y0 J- s" Z+ g: n2 q( c - float Error,ErrorAcc,ErrorDec;+ q- P) e! W- ~0 W) w
) ^& }! R, d a: c6 ^0 t& Q- float Kp=0.14;# I3 d. ]- e3 W# ]
- float Kd=0.00020;//23;9 P: n; W; i% s! r) \) D9 m# _
- float Ki=0.000201;! x" @ o# R8 L+ \4 h, y8 J
5 W/ m# [+ ^3 b. }1 Z+ U- float nPower;
- g5 ?1 g6 @) W# G. N - MePort lightsensor_6(6);
5 B& w8 T, c1 H - MePort lightsensor_8(8);
9 Z) H7 {* P) b4 u' W9 |; F- L - MeDCMotor motor_9(9);9 F8 q0 \0 r1 x/ o7 u4 y
- MeDCMotor motor_10(10);! Q0 \" t# k; T& k+ Z7 p2 p
- unsigned long previousMillis = 0;
2 N" V6 w" l$ V* J* T - const long interval = 1;* d0 r; n, w: s& f8 _
, Z% Z3 O ~8 }, P- void setup(){( Q3 [' T& b( E+ r0 B3 J- k
- lightsensor_6.dWrite1(1);! A* V/ b, W/ X( W Y
- nPower = 160;& g3 l' E0 R1 t1 c# W. {( P) G4 `9 F
- Error=0;
; k- |/ h; S* v$ i - ErrorAcc=0;- u+ g5 V u0 L# g
- }
( ]6 K; W: i/ U% b - 9 ]. G* e& h/ ?7 P6 |! }7 i6 J9 V
- void loop(){
/ }2 @- o- c3 X- K5 S) M: O - unsigned long StartTime = millis();
9 |& A$ F* \; w) K7 K k - if(ErrorAcc < 18000 && ErrorAcc > -18000){$ w8 r( e. K5 P! n0 T) V
- float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
( [2 s3 u, g# p+ t) W - ErrorAcc += nError*Kd ;
+ L7 R. V" [) F4 m# F- @! M - ErrorDec -= nError*Ki ;* {) d" z" o4 ?" P2 p. ?
- Error = int(nError*Kp+ErrorAcc+ErrorDec);" b; |$ k* h0 [2 j
- if(nError < 80 && nError > -80){
5 ~( a9 f. E" E/ ^ - if(nPower < MaxPower){
0 t# w$ F0 b% I) k8 v7 H - nPower += 3;
. B" R, u+ w! H n1 _( e - }$ B8 S6 Z: \8 L9 q5 ~) {
- } else{
" N6 x! t" E9 ?$ O - if(nPower > MinPower){+ i+ Q; h8 ^8 X5 b5 b, ^
- nPower -= 2;1 ^, K0 t( \, t# r( T
- }
& @. E! d6 N- _, |2 ^" f8 @: k - }
. y' T) E2 A& b/ g x* z% n( T$ L - MotoL(nPower-Error);
% p9 P, ^& s; I& R8 c1 R7 g3 a6 z9 i - MotoR(nPower+Error);
- F" ?; G: j9 X1 H2 }% @ - }else{2 C" t; G5 h% S; Y6 \/ [: Y* i
- motor_9.run(0);
) @4 Z$ ^) C( H2 {" h5 i( a - motor_10.run(0);
# V% M2 ~) @; t6 L& N& L2 G% l, s - }6 L; Q! ]& z. o1 ~( ?* v
- do{}while(millis() - StartTime < interval);$ n8 P1 }0 E+ p$ l. X4 C3 a
- }
9 l0 k Y/ e. Q9 Q - % A* x5 r0 O, L2 ]+ ^3 D* H5 T
- void MotoL(int Power){
/ v2 y. }, Y) r$ J1 ^$ _# k - if (Power > MaxSpeed){
9 Q4 c' e/ _6 S$ [) P8 N( W - Power = MaxSpeed;' P, W9 h3 {$ |
- } : L; t, [+ L( v6 M& J
- if (Power < -MaxSpeed){9 c7 ?' X# q+ o5 Z+ B
- Power = -MaxSpeed;! a8 M& e* `' h" o# ?# ?0 s3 C
- } $ z# v( F* Q# x! l6 _- w+ o
- motor_9.run(Power);+ w }0 X& V% l3 g
- }
; F% ~2 v* b0 R8 }. c% ~
3 }& r. i" F0 ?- void MotoR(int Power){
% W' m8 r: C. X- x D$ X - if (Power > MaxSpeed){
( b+ }! B$ w b$ A - Power = MaxSpeed;
0 g& h" k8 r+ P3 u - } / M6 x6 p8 P( y* W; e
- if (Power < -MaxSpeed){, q, C/ D; S. _. H6 E
- Power = -MaxSpeed;9 k8 j- k4 K0 H, s- D$ @$ s* G5 q
- }
; n9 c7 }4 Y/ h+ n( v. y2 v - motor_10.run(Power);1 F- f/ ?) V( K1 p
- }
複製代碼
) n' `) S% L# m' D6 z, M* Y% T+ l- ?% D
|
|