Self Balancing Triangle Robot | Balancing Inverted Triangle

1#include <Servo.h>
2Servo m;
3
4int t = 6;
5int e=  7;
6double   d,dp=0;
7
8void setup() {
9
10  pinMode(t, OUTPUT);  //trigger pin
11  pinMode(e,   INPUT);  //echo pin
12  m.attach(3);       // motor at pin 3
13
14}
15
16int   distance(){
17  
18  long duration, cm;
19  
20  digitalWrite(t, LOW);
21  delayMicroseconds(2);
22   digitalWrite(t, HIGH);
23  delayMicroseconds(5);
24  digitalWrite(t, LOW);  
25   duration = pulseIn(e, HIGH);
26
27
28  cm = duration/ 29 / 2;
29  
30  return   cm; //returns distance in centimeters
31}
32
33void loop() {
34
35  d = distance();
36   
37  if(d<18 && d>0){
38    m.write(180 - ((dp*0.95 + d*0.05)*10));
39    
40   }
41  
42  dp=d;
43  delay(25);
44}

1#include <Servo.h>
2Servo m;
3
4int t = 6;
5int e=  7;
6double
7  d,dp=0;
8
9void setup() {
10
11  pinMode(t, OUTPUT);  //trigger pin
12  pinMode(e,
13  INPUT);  //echo pin
14  m.attach(3);       // motor at pin 3
15
16}
17
18int
19  distance(){
20  
21  long duration, cm;
22  
23  digitalWrite(t, LOW);
24  delayMicroseconds(2);
25
26  digitalWrite(t, HIGH);
27  delayMicroseconds(5);
28  digitalWrite(t, LOW);  
29
30  duration = pulseIn(e, HIGH);
31
32
33  cm = duration/ 29 / 2;
34  
35  return
36  cm; //returns distance in centimeters
37}
38
39void loop() {
40
41  d = distance();
42
43  
44  if(d<18 && d>0){
45    m.write(180 - ((dp*0.95 + d*0.05)*10));
46    
47
48  }
49  
50  dp=d;
51  delay(25);
52}