Post

[Project] Ball & Beam | PID control with Arduino

Posted Updated
By Inseong Paik
2 min read

BallAndBeam.png

Arduino Code

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
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128

#include <Servo.h>
#include <PID_v1.h>

#define TRIG_PIN 20
#define ECHO_PIN 21

#define TRIG2_PIN 4
#define ECHO2_PIN 5

#define SERVO_PIN 6

double distance;

const int UPDATE_INTERVAL = 35;
unsigned long currentMillis = 0;
unsigned long previousMillis = 0;

int maxDist = 49;
int minDist = 5;
double targetDist = 30;
double input, output;
PID pid(&input, &output, &targetDist, 0, 0, 0, DIRECT);

Servo myServo;
int minServoPos = 40;
int maxServoPos = 200;
int homeServoPos = 105;

// Moving average filter parameters
const int BUFFER_SIZE = 10;
int distanceBuffer[BUFFER_SIZE];
int bufferIndex = 0;
int bufferSum = 0;
int bufferCount = 0;

/// Setup:
void setup() {
  Serial.begin(9600);

  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  pinMode(TRIG2_PIN, OUTPUT);
  pinMode(ECHO2_PIN, INPUT);

  myServo.attach(SERVO_PIN);
  myServo.write(homeServoPos);

  double Kp = 3.3;
  double Ki = 1;
  double Kd = 0.15;
  pid.SetTunings(Kp, Ki, Kd);
  pid.SetMode(AUTOMATIC);
  pid.SetSampleTime(30);
  pid.SetOutputLimits(minServoPos, maxServoPos);
}

void loop() {
  currentMillis = millis();

  if (currentMillis - previousMillis >= UPDATE_INTERVAL) {
    previousMillis = currentMillis;

    distance = getDistance();
    //targetDist = getTarget();
    targetDist = getFilteredDistance2();
    if (targetDist >= maxDist) targetDist = maxDist;
    else if (targetDist <= minDist) targetDist = minDist;

    // Use the direct distance as the input to the PID
    input = distance;
    pid.Compute();
    myServo.write(200 - output);

    // Print the direct distance and target distance
    Serial.print(distance);
    Serial.print(",");
    Serial.println(targetDist);
  }
}

double getDistance() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  long duration = pulseIn(ECHO_PIN, HIGH);
  double distance = (duration * 0.0343) / 2;

  return distance;
}

int getDistance2() {
  digitalWrite(TRIG2_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG2_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG2_PIN, LOW);

  long duration = pulseIn(ECHO2_PIN, HIGH);
  int distance = (int)(duration * 0.0343) / 2;

  return distance;
}



int getFilteredDistance2() {
  int newDistance = getDistance2();

  // Update the buffer
  if (bufferCount < BUFFER_SIZE) {
    bufferSum += newDistance;
    distanceBuffer[bufferIndex++] = newDistance;
    bufferCount++;
  } else {
    bufferIndex %= BUFFER_SIZE;
    bufferSum = bufferSum - distanceBuffer[bufferIndex] + newDistance;
    distanceBuffer[bufferIndex++] = newDistance;
  }

  // Calculate the moving average
  return bufferSum / bufferCount;
}
Project
Control mechanism Arduino
This post is licensed under CC BY 4.0 by the author.