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  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}