Ball Balancing Robot | Arduino Object Detection Via OpenCV

1#include <PID_v1.h>
2#include<Servo.h>
3
4Servo m;
5int t=8,e=9;
6int  d_mid = 0;
7
8double Setpoint, Input, Output;
9//double Kp=0.5, Ki=2.4, Kd=4;
10double  Kp=0, Ki=0, Kd=400;
11
12PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);
13
14void  setup() {
15  pinMode(t, OUTPUT);  //trigger pin
16  pinMode(e, INPUT);  //echo  pin
17  m.attach(7);
18  
19  Input = distance();
20  Setpoint = 10;
21
22  myPID.SetMode(AUTOMATIC);
23
24}
25
26int distance(){
27  
28  float duration,  cm;
29  
30  digitalWrite(t, LOW);
31  delayMicroseconds(2);
32  digitalWrite(t,  HIGH);
33  delayMicroseconds(5);
34  digitalWrite(t, LOW);
35  
36  duration  = pulseIn(e, HIGH);
37
38  cm = duration/ 29 / 2;
39  
40  return cm; //returns  distance in centimeters
41}
42
43void loop() {
44
45  int d = distance();
46
47  
48  if (d<22){
49    if (d<11){
50      myPID.SetOutputLimits(0,90);
51      Input  = d;
52      myPID.Compute();
53
54      m.write(90+Output);
55      
56    }else{
57      myPID.SetOutputLimits(-80,10);
58      Input = d-10;
59      myPID.Compute();
60
61      m.write(90+Output);
62      
63    }
64  }else{
65    m.write(110);  
66  }
67}