|
- #include <Arduino.h>/ n. _9 A2 {$ t# M5 L
- #include <Wire.h> T. B1 Z5 s- s( ^- {! U2 c' B
- #include <Servo.h>2 k$ D& }8 q+ S6 L) z; |
. R0 ]0 n# v% g& G; `- #include "MePort.h"1 x% o& m# Y$ A/ h# K6 O5 s
- #include "MeUltrasonic.h"
$ u% J7 }2 }- R" l - #include "MeDCMotor.h": r7 d# ^1 F8 p0 |# G$ P
8 L9 m) A4 \+ ^1 e4 f1 T0 s# }: D3 B- //double Input, Output ;
3 ?# F- u& ~) }5 }0 n! A1 O - float MaxSpeed = 255;
6 E' j( k7 `& G% V - float MaxPower = 180;( G0 K! N' ]1 Y; g/ L! d
- float MinPower = 120;
6 C. l1 K4 }* q* _/ O5 v3 \" t - float Error,ErrorAcc,ErrorDec;. k, f7 n) I# e$ u
- * ~- D( ]( ^3 I. ]6 x
- float Kp=0.14;
' l: u6 z+ n) K- |* b - float Kd=0.00020;//23;
1 y/ d4 }* g$ R: S - float Ki=0.000201;
" V( `" Q- `, Y; q! c( Y
& @% u) S& \4 P- float nPower;
- r+ D" b. H/ y& | - MePort lightsensor_6(6);
/ ~/ s% B1 M9 [4 O# b0 x - MePort lightsensor_8(8);
7 `( ^* U. ?: z& B - MeDCMotor motor_9(9);8 W1 f$ a( ^! c! {2 i5 C
- MeDCMotor motor_10(10);
; z, Y- N* l8 }$ k0 { - unsigned long previousMillis = 0;
" S% d% w' y7 a+ R! o9 I5 I( S - const long interval = 1;8 ?8 O1 a8 s8 ]
- " j W( |4 f1 z& B4 B
- void setup(){
" [/ d% q5 c+ { - lightsensor_6.dWrite1(1);- {: }) k( z3 K
- nPower = 160;
1 Z b9 q3 k( E0 k9 C( H - Error=0;
' Q9 k K5 `5 A. U# z - ErrorAcc=0;- W# }+ [) ?$ a$ h: B
- }
5 J9 j2 w! q$ E" s# H9 I - 6 _( v, p$ ~! c
- void loop(){
0 V8 j' D/ s6 W - unsigned long StartTime = millis();& o8 N8 ]) P0 c1 F. }) E1 P
- if(ErrorAcc < 18000 && ErrorAcc > -18000){
. K; u0 @' \3 ]% X* v- M, w - float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
" c+ c4 a ?& i7 { - ErrorAcc += nError*Kd ;; M- M) i7 K; \( t, [1 V
- ErrorDec -= nError*Ki ;( g6 A) m: t% j) W
- Error = int(nError*Kp+ErrorAcc+ErrorDec);
7 @2 _. ^% j$ p Y - if(nError < 80 && nError > -80){+ l4 x/ @$ m. x6 K) U& }
- if(nPower < MaxPower){8 h9 P1 `6 q; P3 M' L( n
- nPower += 3;! Z: g: X' ^5 _9 F5 J. {9 S' A
- }7 {/ r* s; f; s. N/ j4 C
- } else{" K5 f6 l6 e* R0 O! u) t
- if(nPower > MinPower){
9 z2 |, ^0 C& F7 R& [ - nPower -= 2;
5 B3 u# b8 k' p, P7 j% l3 F! y+ h - }
- C) r: p) I0 Z i" M - } : M+ v1 A' m) {7 z8 T. j1 }
- MotoL(nPower-Error);
% r0 q( j. m* q- E! T$ [ - MotoR(nPower+Error);
1 o1 _9 G% K4 T - }else{
) w" b' ^3 z$ W" G) o$ K3 S, A - motor_9.run(0);
0 p! R! d1 { {0 {3 N' V - motor_10.run(0);/ [$ @' F, L' |+ E; I- F/ g# V9 [
- }! G/ _8 _9 K" a1 G0 }0 W& k: c: |
- do{}while(millis() - StartTime < interval);
( g) _. h0 X7 q1 s% w& I - }* t- N; |* H5 X' A0 K+ g% r0 c
. V3 Q/ ^: D) H0 L1 T+ m& o- void MotoL(int Power){+ x! u( p5 ^+ H J
- if (Power > MaxSpeed){
5 l0 M M! _; t. V4 a4 i - Power = MaxSpeed;7 u4 P" e* V' ?5 n( @6 I; A( g
- } 3 w" G; Y( P7 o6 T$ t" x
- if (Power < -MaxSpeed){
9 C6 {0 d( g! L+ ~2 p - Power = -MaxSpeed;. k9 |2 m4 t' \" V) {( Z
- }
: h- \" J1 a7 S: b7 M$ H - motor_9.run(Power);! h' k' r y; ]" k. t+ {. F+ l
- } 0 U9 m+ @: c3 U) ^ r( W# Q# `
7 `+ Y, ?. z* T" d- void MotoR(int Power){# M' \1 J4 m: D
- if (Power > MaxSpeed){: F+ `0 C7 z! n( m8 i. p
- Power = MaxSpeed;* {+ c7 C2 R0 e, @- P3 @+ m0 s
- } : H$ ~; N* L; {$ J c' f
- if (Power < -MaxSpeed){; P: \* g2 ~/ E) Z) V0 Q- ^
- Power = -MaxSpeed;/ A: M* Y& t! z
- } 6 Y9 t$ t, h& e+ t. z
- motor_10.run(Power);
2 S( q% H% W! ], U9 r; y2 ] - }
複製代碼
0 P) ?7 \5 [' g& R
2 g8 x6 y; r/ O6 y3 r |
|