-
Notifications
You must be signed in to change notification settings - Fork 0
/
BALL_TRACK.ino
59 lines (45 loc) · 895 Bytes
/
BALL_TRACK.ino
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
#include <Servo.h>
#include <PID_v1.h>
int led = 4;
int led1 = 3;
Servo y;
int turnlr_time = 10;
double Kp=2, Ki=1, Kd=0.5;
double Setpoint, Input, Output;
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);
void setup() {
// put your setup code here, to run once:
Serial.begin(9600);
pinMode(led,OUTPUT);
pinMode(led1,OUTPUT);
y.attach(10);
y.writeMicroseconds(1600);
Setpoint = 86.00;
//myPID.SetOutputLimits(-255,255);
myPID.SetMode(AUTOMATIC);
}
void loop()
{
if(Serial.available() >0)
{
int val = Serial.read();
double err = Setpoint-Input;
myPID.Compute();
if(err > 0)
{
digitalWrite(led,1);
digitalWrite(led1,0);
y.writeMicroseconds(1600+Output);
delay(turnlr_time);
y.writeMicroseconds(1600);
}
else if(err < 0)
{
digitalWrite(led,0);
digitalWrite(led1,1);
y.writeMicroseconds(1600 - Output);
delay(turnlr_time);
y.writeMicroseconds(1600);
}
}
}