|
|
- #include <Arduino.h>
7 c( Z1 l( e' s( B" Q+ l; i; X+ X - #include <Wire.h>
' ^9 V0 K. p) O9 S1 g) v: r- @ - #include <Servo.h>9 l6 o3 F# ?: k8 j2 ~* Q* Z1 T
7 h6 B! c* O) K- #include "MePort.h"
0 [3 O* j- r- b: k( c - #include "MeUltrasonic.h". p( f" Y& C, ^0 G2 s8 H- n
- #include "MeDCMotor.h"
5 M5 R6 n C) L
% y3 R3 n t! W7 j. P- //double Input, Output ;
/ a N* }5 \2 \8 y$ H - float MaxSpeed = 255;4 v& C& c* E D
- float MaxPower = 180;0 a g. f/ |0 C+ Y6 O3 V
- float MinPower = 120;
" e# c- T1 A/ c* D6 G; O - float Error,ErrorAcc,ErrorDec;9 z' L3 |; Y6 t G3 y
& T0 e+ u# e1 f1 z4 ?. @4 ?5 q- float Kp=0.14;
( a3 g, w3 P+ l' V0 y - float Kd=0.00020;//23;
& {' R7 L& U6 q% m6 r% |$ M9 a - float Ki=0.000201;% `8 A4 z4 s& j5 ~
- 8 C o- w, |& q, V& h" F
- float nPower;5 N5 ^+ w" w q4 v
- MePort lightsensor_6(6);
, B9 u. @7 i- N - MePort lightsensor_8(8);& x" b+ b6 `- F, S) I( N
- MeDCMotor motor_9(9);- l% C& \, {) `, E0 M' u
- MeDCMotor motor_10(10);
& O9 N. v5 r; H - unsigned long previousMillis = 0;; t# L. H* D; k7 T3 g8 [
- const long interval = 1;
) [3 q* h! K& i l1 ^% c7 P
1 I* f+ h; n5 t) ]% L- void setup(){5 ^' A/ U6 M' r% q
- lightsensor_6.dWrite1(1);0 s4 q7 j- [$ y" J; a+ J# A
- nPower = 160;
* i$ W) R* l; l; r1 P - Error=0;
6 F; d. S+ ?* I& Q" i - ErrorAcc=0;; U l- T7 t5 I0 [! Q% t
- }: \ D+ X" a( \2 F
- , p* K: C q- k U1 Z1 F
- void loop(){/ ~' e( z6 w$ }% X9 s! R; v5 {/ [
- unsigned long StartTime = millis();; @2 q' ]2 Z2 U/ o- q' n
- if(ErrorAcc < 18000 && ErrorAcc > -18000){7 b" p- b y6 y
- float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
: w" U3 P u5 i3 G$ {/ b b - ErrorAcc += nError*Kd ;
2 m- S2 B8 b+ N3 _5 m - ErrorDec -= nError*Ki ;
8 T4 J' T) W! U% q7 m/ D - Error = int(nError*Kp+ErrorAcc+ErrorDec);
+ _4 ]3 \( k" e$ w) t - if(nError < 80 && nError > -80){0 Q" b2 {- \/ r
- if(nPower < MaxPower){
/ U! }7 F& g- ^& a4 P, `& P - nPower += 3;
* q+ c5 b4 ]" G p5 n/ j! O. s - }
; Q! x- J! ^9 u& j. w/ X - } else{* a2 s3 M6 N Q/ i
- if(nPower > MinPower){ x7 l: ^& @, Z/ g
- nPower -= 2;
7 U: I- C3 T: @7 T* } - }
2 L2 J# ]4 B2 r+ d( X, o- d4 ?; C9 E8 X' _ - }
, [( L; i( V! J- V$ [$ Q - MotoL(nPower-Error);
6 H* B s% E6 Q! `9 x) }2 ?. l - MotoR(nPower+Error); ! @, r. ~, W; F) O5 ?; _ R7 |
- }else{
* I$ O2 W5 T4 X4 w" n - motor_9.run(0);$ G, S3 t# w7 L5 ^; B
- motor_10.run(0);
# `+ q$ ~8 W7 X - }
O, s' W |1 Q( G0 M - do{}while(millis() - StartTime < interval);8 l& e# [- l# m* n5 c' u2 u
- }
8 A) R+ M3 E$ C8 g - - n: T7 t8 E6 B! w0 Y
- void MotoL(int Power){1 L& o: T6 A3 |: T1 v' R4 T5 d
- if (Power > MaxSpeed){
7 j. y/ P2 e( s6 f) M+ n o - Power = MaxSpeed;
: A" U. D8 \3 g- X+ C - }
) [! j2 C) E; Z1 Y, f% r - if (Power < -MaxSpeed){
' T- l* D1 D# x; C( Z5 ^) z7 D - Power = -MaxSpeed;
; C: ?8 i4 |" w8 w' i2 C - }
8 L/ C: X( C2 m% L7 f - motor_9.run(Power);
0 W* @- n: r7 D9 g, u - }
, Q" s" ]0 R2 J R$ _ - 4 D; R9 G, y" v7 w6 p+ {
- void MotoR(int Power){
" @9 } b$ W2 l6 }4 b - if (Power > MaxSpeed){
$ j; D5 N( K3 H' I9 b2 g - Power = MaxSpeed;
0 w7 J8 Y& }1 D( d - } & J: U. a! |# R
- if (Power < -MaxSpeed){9 `1 k3 F6 t3 h7 v7 w
- Power = -MaxSpeed;
: \/ ?6 ~- N Y% |; i6 I, \ - }
* d" o+ Q. o8 p" R% S - motor_10.run(Power);
, K w, g0 |* u- ^6 T' e - }
複製代碼 % b5 u& C* L4 \+ a/ r0 o$ h5 }
/ s9 M d/ b+ b8 @! K
|
|