|
|
- #include <Arduino.h>( C X" ]8 b: A
- #include <Wire.h>- `* s M. e: w$ q8 s" b
- #include <Servo.h>
1 ?& l8 h0 J. F1 R0 F x - ) r8 ^; O$ q8 ^! e
- #include "MePort.h"
7 g0 O$ E, m( Y - #include "MeUltrasonic.h"
" P0 l& o: n2 \' o# Q( c - #include "MeDCMotor.h"
0 ]1 m, V: q4 M1 G" m0 W8 ~ - 5 B# B, P6 |% v- i8 F
- //double Input, Output ;) x' y( h( `. I' u( t$ I; ?
- float MaxSpeed = 255;
8 C4 }: _+ L$ ]& {) x( e0 F - float MaxPower = 180;+ ^7 P, G( j6 Z1 t% M$ Y% [$ @
- float MinPower = 120;
! ~6 n) d5 o$ z8 ^ - float Error,ErrorAcc,ErrorDec;
4 ~* U! {( s& v" p! f, h0 x - 1 Q0 C! [* o+ j. M
- float Kp=0.14;4 A7 a& g- e8 g( B1 p$ P
- float Kd=0.00020;//23;* I% @7 ^( m0 F0 v% `: g( B0 O
- float Ki=0.000201;
4 G. w: m1 Q7 s* N
8 u( J. {) X6 v- float nPower;
9 ~- b* C6 z) ?: |0 L& b0 M. n2 b- p - MePort lightsensor_6(6);
8 l2 Y( r& p. ~: n/ k) F# t3 z - MePort lightsensor_8(8);: a1 I) [- W) V+ J' {
- MeDCMotor motor_9(9);
2 t$ X _2 o1 _' |7 v - MeDCMotor motor_10(10);% l: `7 P) M. R% r
- unsigned long previousMillis = 0;3 T, s8 ^7 Y* n
- const long interval = 1;
/ q. p* c) R- L
) V& x1 f" u0 q- X* B1 Z- void setup(){
" R4 F, S- o8 f+ }/ T% w2 r - lightsensor_6.dWrite1(1);
* M% r8 \: x5 X5 j" V - nPower = 160;' L6 v. D' n3 B1 ^, J
- Error=0;
" Z- R Q, C/ S( S: Q- s: o8 o - ErrorAcc=0;
0 `3 ?9 [: k8 I1 K+ ^ - }2 v5 S+ `) X( u$ ]
- ) C; B9 J4 u1 h4 A5 u
- void loop(){
K6 D6 I( f- ? O2 k4 b4 e! U( z - unsigned long StartTime = millis();
, Y( {. r# v9 P2 W - if(ErrorAcc < 18000 && ErrorAcc > -18000){
$ s5 _4 ?2 r+ p7 q - float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();. C3 {' V/ l8 D
- ErrorAcc += nError*Kd ;
7 o$ w1 T P. e d7 [2 G - ErrorDec -= nError*Ki ;$ w2 P; u# I! ]
- Error = int(nError*Kp+ErrorAcc+ErrorDec);. h9 i6 k8 Z5 R( E- U E
- if(nError < 80 && nError > -80){ I% R8 V- b% T3 B- F
- if(nPower < MaxPower){1 L4 I% |; L; u7 I& A/ [% Y
- nPower += 3;& b; t1 Q, }. w. |' k8 G
- }. H; L; ^4 z d' |
- } else{
9 ?3 I; v; E1 v! E5 F5 w - if(nPower > MinPower){4 V; U9 u' H9 K; m- N* x. l
- nPower -= 2;0 _8 Y) f! v- `, n1 j3 g3 ~& K
- }
?' D$ ^" B4 I7 ~ - } o; Y: W+ U/ B( A/ Q
- MotoL(nPower-Error);# C+ b4 J. H( E" B
- MotoR(nPower+Error);
! ~& I J5 r# r5 |4 @ - }else{5 }- t2 F c7 E9 z6 s
- motor_9.run(0);
+ p8 u& y5 [. G9 m - motor_10.run(0);
0 M+ g( H8 a& v3 z - }7 W4 G4 |6 @. j" e, S
- do{}while(millis() - StartTime < interval);; y: U3 y- V/ G( Q
- }
0 A( ^/ C- ^; }) C, u9 }* p
- j* w$ _. J& E. c" M/ _4 ?# X) X% G- void MotoL(int Power){
3 N% l- c8 c$ k5 q5 H& R - if (Power > MaxSpeed){
1 W) U8 p- V; H2 @, c' D8 q# F - Power = MaxSpeed;5 B: L- G: f8 I( w# m
- }
1 n" ?0 P6 n$ ~: Q% D9 \ - if (Power < -MaxSpeed){
$ ~) k4 ]; H& C5 c$ F8 x" h - Power = -MaxSpeed;
% r: c( B1 Y5 X* c6 d - } ! V- a- F% n4 f; I2 e# B
- motor_9.run(Power);
' D6 [. Z; K' z0 s - } . T8 E4 o* M M# r. C1 c! @( }
- 5 U9 j7 a9 ~" G& l8 O+ {2 M/ ^
- void MotoR(int Power){
5 @. ?0 ]3 v. d$ ?- `# A4 s, H - if (Power > MaxSpeed){
& @. Q5 ]: H/ H/ s - Power = MaxSpeed;
- w1 ]- H% i% ]. o l; {- t - } 3 \2 y B# A& G8 K
- if (Power < -MaxSpeed){
$ E# @* M: d8 @+ M/ w2 @ - Power = -MaxSpeed;" Q1 w& {! N0 {; u
- }
' H6 B5 U' W1 A% Z0 ^: n4 i0 h) _ - motor_10.run(Power);7 i4 o* H0 B: u2 g- n6 a
- }
複製代碼 5 }7 Q6 h% e% j- K6 U5 e. w; F3 m2 t" p
; K) n$ s- a6 w, E+ U, m
|
|