ball_and_beam

# Ball and Beam

## Motivation

The ball and beam system is a classic control theory problem. This simple system is an easily built testbed for control algorithms. Like the inverted pendulum, this system is open-loop unstable and therefore cannot be controlled without sensor feedback. Goal: Using a beam able to rotate via a servo, the position of a ball rolling on the beam should be maintained at a set point on the beam. This system should be robust to disturbance of the ball and and position the ball in a reasonable amount of time with little positional overshoot. The controller will be tested with static input and by tracking different functions. I will use a LEGO NXT, motor and light sensors.

## Examples

<br>

Note: If the videos do not load, just refresh the page.<br>

## Model

After drawing a free body diagram of a ball on a tilted beam, it can be seen that the acceleration due to gravity is proportional to sin(theta) where theta is the angle of the beam with 0 degrees being the horizontal orientation.

`m*g*sin(theta) = m*x''`

Based on the small angle theorem, this relation can be approximated by in the form:

`g*theta = x''`

Diagram courtesy of control-systems-principles.co.uk

## Control

As seen in the approximated equation of motion above, acceleration of the ball on the beam is proportional to the angle of the beam. This angle of the beam (theta), along with the position of the ball (x) and the velocity of the ball (x') can be either measured directly or obtained analytically from the accessible data.

Two controllers must be developed to control this system: am angular position controller for the motor controlling beam angle and a controller for the ball position and velocity on the beam. For this project, a stock controller available through the NXC programing language was used to control the motor position. This controller uses Proportional-Integral-Derivative (PID) control to set a motor position with an appropriate rise time and minimal overshoot of the set value. This controller can be accessed in two ways. One way allows the PID gains to be set manually. The second way (used in the following controller code) allows the user to control the maximum motor velocity and acceleration effectively dampening the system.

### Motor Position Control

For PID control of the NXT motor on Port A:

```OnFwdRegPID(byte outputs,
char 	pwr,
byte 	regmode,
byte 	p,
byte 	i,
byte 	d
)```

For Velocity and Acceleration control of the NXT motor on Port A:

```PosRegEnable(OUT_A);

PosRegSetMax(byte output,
byte 	max_speed,
byte 	max_acceleration
)

PosRegSetAngle (OUT_A, angle);```

The angular position of the motor is accessed via a built in encoder using the following command:

`curAngleInDegrees = MotorRotationCount(OUT_A);`

### Ball Position and Velocity Control

To control the ball on the beam, a PD controller was employed to control the ball position on the beam and its positional derivative, velocity. Control of position allows the ball to be maintained at a set point on the beam while control of the velocity slows the ball as it heads toward the set point.

To control these parameters, the ball's position on the beam must be measured on the beam. This was done using two infrared range finders positioned at either end of the beam. The difference between the values obtained from each sensor was used to determine the ball position on the beam.

#### Sensors

The sensor used was the High Precision Medium Range Infrared distance sensor for NXT (DIST-Nx-Medium-v3) from Mindsensors.com

Documentation and examples for using this sensor can be found on their website.

```SetSensorLowspeed(S1);

#### Calibration

Before the control loop started, the sensors were calibrated to provide a difference of zero (0) when the ball was in the center of the beam. This was done by taking the average of many readings to calculate an offset necessary for each sensor to return a zero at the origin. This offset was then applied to every subsequent reading used for control.

```int calibNum = 30;
int i;

...

for (i = 0; i < calibNum; i++)
{
Wait(100);

TextOut(10, LCD_LINE3, "Calibrating...");
}

DL_offset = DL_offset/calibNum;
DR_offset = DR_offset/calibNum;

#### Data Smoothing

A very simple data averaging filter was employed to help smooth the very noisy range sensor data. This filter, before releasing a data point, took multiple readings from each sensors and provided an average reading for each sensor every iteration. Although simple, this filter helped reduce the impact of outlying data points and slowed the control loop down minimally.

```int inLoopAvg = 3;
int j;

...

DL = 0;
DR = 0;

for (j = 0; j < inLoopAvg; j++)
{
}

DL = DL/inLoopAvg;
DR = DR/inLoopAvg;```

#### Tuning

The P and D gains were chosen by trial, error and observation. First, P was chosen to react to the ball's existence on either side of the zero position (center of the beam). With just a P controller, the beam would tilt clockwise with the ball on the left and counter-clockwise with the ball on the right. This controller provides the ability to react to a non-central ball location, tilting to move the ball in the direction of the center of the beam.

The position of the ball is proportional to the calibrated difference between the two range sensors.

The D gain was chosen to dampen the velocity of the ball as it rolled past the set point. When tuning the value, it is beneficial to attempt to control the ball manually (tilting the beam by hand). This trial will give insight into what type of motion is necessary to slow the ball down before it passes the set point (zero). The derivative (velocity) controller should be tuned to prevent overshoot while still achieving quick rising and settling times.

Velocity is calculated as the difference between the ball position from the last position and the current measured position divided by the time elapsed during the iteration.<br>

`(Current - Previous) / deltaT`

For the ball and beam system, built with a LEGO NXT motor geared down 1:5, the values used were:

```KP = 0.15
KD = 0.04
MotorAngle = KP*Position + KD*Velocity```

## Video

Note: If the videos do not load, just refresh the page.

## Code

BallandBeam.nxc
```// Program: BallAndBeam.nxc
// Author:  Alex Alspach ([email protected])
// Date:    March 2009
// Status:  Working but needs Tuning and redesign of the syste

#include "dist-nx-lib.nxc"

#define   Kp 0.15
#define   Kd 0.04

float DL_offset;
float DR_offset;

float DL;
float DR;
float X;
long curAngleInDegrees;

float DL_old;
float DR_old;

float errorX;

float errorX_old;

float DerX;

float angle;

float dT = 0.1;

float alpha = 0.5;

int calibNum = 30;
int inLoopAvg = 3;
int i;
int j;

SetSensorLowspeed(S1);
SetSensorLowspeed(S2);

PosRegEnable(OUT_A);
PosRegSetMax (OUT_A, 80, 50);

int count = 0;

for (i = 0; i < calibNum; i++)
{
Wait(100);
TextOut(10, LCD_LINE3, "Calibrating...");
}

DL_offset = DL_offset/calibNum;
DR_offset = DR_offset/calibNum;

while (1) {

DL = 0;
DR = 0;

for (j = 0; j < inLoopAvg; j++)
{
}

DL = DL/inLoopAvg;
DR = DR/inLoopAvg;
X = (DR-DL);

curAngleInDegrees = MotorRotationCount(OUT_A);

ClearScreen();
NumOut(0, LCD_LINE1, DL, false);
NumOut(0, LCD_LINE2, DR, false);
NumOut(0, LCD_LINE4, X, false);
NumOut(0, LCD_LINE6, dT, false);
NumOut(0, LCD_LINE7, DerX, false);
NumOut(0, LCD_LINE8, curAngleInDegrees, false);

errorX = X;

DerX = (errorX-errorX_old)/dT;

//PD control
angle = (Kp*X) + (Kd*DerX);

// Limit the beam angle range
if (angle > 200)  angle = 200;
if (angle < -200) angle = -200;

PosRegSetAngle (OUT_A, angle);

DR_old = DR;
DL_old = DL;

errorX_old = errorX;

count = count + 1;
}

}```

## Future Work

The ball and beam system shown in the video is a work in progress and performs poorly at this point. In this first go at the ball and beam system design and control, many lessons were learned that can be used to create a much more stable system and controller.

### Mechanical Design

The beam built, along with the frame on which it rotates, is flimsy. This beam is so flimsy that it actually arcs so that either end, when horizontal, is lower than the center. This became noticeable when attempting to tune a gentle proportional controller. Although the beam would tilt, the ball would remain in a low point at either end of the beam, not reacting to the corrective motions.

The frame is also not nearly rigid enough. It allows not only the intended degree of freedom but fast motion and offset weight of the range sensors cause rotation about an upward pointing axis. This was reduced by tightening a zip-tie around the frame but ultimately, these motions still interfered with the dynamics to be controlled and, therefore, the stability of the system.

### Backlash

Backlash in the NXT motor amounts to almost five degrees of free motion while the motor is meant to be rotationally static. This “slop” was reduced by gearing the system down 5:1 but this was not enough. This error, along with even more rotational error due to beam driving axle torsional deformation, causes hardly precise measurements of the motor angle and poor ability to reach a specified angular position. This slop could be reduced by gearing down further or using more high precision servos.

### Motor Response

The motor position control needs more attention. Currently, the acceleration is too high, causing a very jerky system. These jerky motion causes quick changes in velocity and therefore quick over-compensations from the velocity controllers. This motion and reactionary motion causes the system to approach instability.

### Position Sensing

The ball position data is extremely noisy, inconsistent and, therefore, unreliable. This is for many reasons. The sensors are not mounted securely enough. Sudden motion causes the mounts to flex and the range data, even if only slightly, to change. Even with a data filter, this system property causes errors from the very beginning. Secondly, the sensors are not focused perfectly on the ball as it moves and the relation between sensor output and ball location is not exactly linear.

### Filtering

The filtering method used (averaging multiple data points each iteration) is very basic and slows the loop more as more data is averaged. A more advanced filtering algorithm may be necessary if sensor data stays this noisy.