|
|
- #include <Arduino.h>
, W v' \8 [8 Q - #include <Wire.h>( ~" B1 X9 G4 }
- #include <Servo.h>
/ o2 l, b# t0 S: f: K& |8 m) |3 W - 0 _: i( R+ ~3 ^. n
- #include "MePort.h": \0 k& W. T1 t& Z' Z; P4 o1 i
- #include "MeUltrasonic.h"
" L4 C7 C' l1 i ` - #include "MeDCMotor.h"
) G3 h& E( I& x. |) k9 r& V) _* ]. h/ V - # h) z: D6 _8 Y" n
- //double Input, Output ;
0 D$ ^2 R: r( T% N - float MaxSpeed = 255;* Z/ V7 M3 r9 h
- float MaxPower = 180;
3 C0 b) V. X" q9 g' |3 P: W - float MinPower = 120;
( C6 j# W% l' g - float Error,ErrorAcc,ErrorDec;
9 \, Z- N8 G2 F( O6 }7 k* f6 c
# q# N- G* T7 W5 i- float Kp=0.14; K$ K: O( c" @3 y. }
- float Kd=0.00020;//23;, I* j! a7 K) `6 m* P: Q1 @& ~, A
- float Ki=0.000201;9 M; J$ M% R# u! |% C7 J
4 H7 q4 l1 B6 F. g8 L+ G7 f- float nPower;7 a. [4 m/ x) D- M6 S
- MePort lightsensor_6(6);
3 a0 T8 a8 T. D4 T - MePort lightsensor_8(8);
3 |, W! @- Z% K5 q# I7 f - MeDCMotor motor_9(9);
|3 H3 _: Z# | - MeDCMotor motor_10(10);2 U3 k4 L5 V7 ~' B' [( ]4 V+ a
- unsigned long previousMillis = 0;4 x% ^. q8 X6 d( U# u2 z
- const long interval = 1;$ g% P6 X) X0 m6 H6 ~5 k5 t7 Q
- * K$ k1 y" o9 b2 c
- void setup(){# ~, J: V6 o" I' {# B. M3 e8 G
- lightsensor_6.dWrite1(1);
6 ]* b6 S1 ]1 u. }; J - nPower = 160;& m( K. h; ~% ]- A
- Error=0;/ r. y# } {- |0 T' M' t
- ErrorAcc=0; J3 [$ D, Q s! L) x- o
- }
/ Q. a: u+ m) C4 C
$ f) x0 u) h% n" v3 t- void loop(){
; G7 R, O5 Z! z) Y - unsigned long StartTime = millis();
7 A- c9 o6 W$ d; P6 g - if(ErrorAcc < 18000 && ErrorAcc > -18000){2 E# A, Y: A+ Q7 _) O% k3 z( ~& G
- float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
& i* ~& h0 ]! K. k6 Y0 w) { - ErrorAcc += nError*Kd ;
6 U+ G' B6 T# ^( p0 n - ErrorDec -= nError*Ki ;
; L! F! k7 s( f, f3 q7 }4 n% n9 m - Error = int(nError*Kp+ErrorAcc+ErrorDec);! z* s+ ~ j! p8 f6 m4 z: q% r
- if(nError < 80 && nError > -80){
9 G- t0 O( { S! j6 |& \' _ - if(nPower < MaxPower){
! A1 `4 p5 }' |, N - nPower += 3;9 R/ U$ X3 p4 W, Y+ |; l/ v7 W
- }
- ]6 d+ Q* B7 B& Z& F0 W* E/ r - } else{
7 d) Z( J8 Z& A - if(nPower > MinPower){
$ Q( i# i% ?2 _4 J - nPower -= 2;# b- x( e1 D9 g. k& S
- }( ^' i5 }# v/ O, J/ K: ]
- }
% ~4 e2 B" k$ _1 c' m - MotoL(nPower-Error);' [3 l V/ a3 p* ]- J1 Q
- MotoR(nPower+Error);
& f5 @% R1 }7 q: F% m5 B - }else{
% A! L7 {0 t' e$ X8 A6 J - motor_9.run(0);
1 z& Z3 Q, w) R6 M - motor_10.run(0); m+ I* v( j/ F% H$ z R7 \, A7 |
- }3 ]% c& _1 y9 r4 z. G1 b2 j
- do{}while(millis() - StartTime < interval);8 K' `( ~0 d! B/ t, f' I( R% m
- }
2 Z0 @+ C9 S( ^- T& Z5 Q8 K
% N6 f; `$ V3 k$ A2 u- void MotoL(int Power){. c/ s% H# n6 G
- if (Power > MaxSpeed){8 o6 q1 m3 T b3 u
- Power = MaxSpeed;% o, u8 T1 y q- e2 s! Z3 q
- } " m) [; e/ Y3 {. J. E5 c
- if (Power < -MaxSpeed){4 ?7 ~6 n; |- r0 d- q
- Power = -MaxSpeed;1 F* p' l2 q F4 e( I
- }
) e3 u* D7 E3 p/ R, U. T) y - motor_9.run(Power);0 j" C4 o; X! ] u0 j
- } & X- j8 C0 G5 D( Q _- b1 W1 o
- 5 e8 M- m, _7 |- S5 t
- void MotoR(int Power){. H: g9 }( }8 M8 n1 n5 B3 Y
- if (Power > MaxSpeed){
5 ^& ?* K6 }" v& T: x - Power = MaxSpeed;
0 q' ?& |/ \( w5 p; S - }
: [! ?4 C; [( [' I3 U& B1 M - if (Power < -MaxSpeed){
1 Z6 @# H. m4 v* [! l+ F - Power = -MaxSpeed;
# N0 V7 l" i8 `" r/ v) ~/ {: T+ j - }
: g" ]! R) @+ W1 Q0 {2 G2 W# U1 } - motor_10.run(Power);0 f4 q4 B$ i" K( m' k
- }
複製代碼
+ {% G j1 f8 a0 j L
% q" [' c! `0 @3 f" M3 s. y0 D- ~. I |
|