Sei sulla pagina 1di 19

Ball and Beam Control System Project

Mu Lin
Electrical and Computer Engineering
New Jersey Institute of Technology
Abstract
The ball and beam control system is open loop unstable. Therefore, a feedback
control must be employed to maintain the ball in a desired position on the beam.
A full state feedback and full order observer was designed. The mathematical
model for this system is nonlinear but may be linearized. In this project, the
system includes an aluminum beam, a DC motor with optical encoder, a
HCTL2016 quadrature decoder, a ball, a L298N motor driver, a Mega 2560
microcontroller, and two infrared sensors.

Figure 1. The ball and beam system


System Overview

Figure 2. Sharp GP2Y0A21YK0F analog distance sensor

Figure 3. L298N motor driver


Figure 4. Power supply

Figure 5. Litton motor with optical encoder


Figure 6. HCTL 2016 quadrature decoder

Figure 7. Arduino mega 2560.


Figure 8. Overview of ball and beam system.
System Parameters

Parameter Symbol Unit Value

Beam tilt angle rad (-/6, /6)

Mass of ball _ Kg 0.05

DC motor resistance R Ohms 5

Electromotive force K Nm/A 4.91


constant

Ball radius M 0.02

Control voltage V volts (-12,12)

Position of ball x m (-0.3,0.3)

Length of the beam L m 0.6

moment of inertia of the J_bm kg.m2 0.065


beam about the motor
shaft
Mathematical Model

2
(1 + 2 ) + =
5 _
2
( )

=
_

State Space Model

1 = , 2 = , 3 = , 4 =

1 = 1

2 = 2 3
(1+ 5 2 )

3 = 4
2 /
4 = 1 4 +
_ _ _

1 0 0 0
=[ ]
0 1 0 0
MATLAB Simulation
Full state feedback full order observer design
The feedback control gain calculated by LQR method and the full state observer
gain calculated by pole placement method.
The following closed loop poles were chosen for the simulation.
Desired closed loop poles: [-20+10j,-20-10j,-40+15j,-40-15j]

Figure 9. Simulation result of the motor voltage


Figure 10. The simulation result of the beam tilt angle (in radian)

Figure 11. The simulation result of the position of the ball (in meter)
Appendix A
Matlab M-file for simulation
clear all
close all
clc

%Ball and Beam Control System Simulation

syms Rb m_ball m_beam L_beam R Kc Ke J_beam J_motor J_bm b g a1 x1 x2


x3 x4 u K s x

%--------- System parameters--------------------


Rb = 0.02; % ball's radius (m)
m_ball= 0.05; % mass of the ball (Kg)
m_beam= 0.7; % mass of the beam (Kg)
L_beam= 0.6; % length of the beam (m)
R = 5; % Dc motor armuature resistance (ohm)
Kc= 4.91; % Motor torque constant (Nm/A)
Ke=0.5/(2*pi/60); % Motor back electromotive-force constant
(Volt/(rad/s))
J_beam=0.012; % moment of inertia of the beam by measurement
(Kg.m^2)
J_motor= 0.043; % moment of inertia of the motor (Kg.m^2)
J_bm=J_beam+J_motor ; % moment of inertia of the beam & motor
(Kg.m^2)
b=0.16/(2*pi/60); % DC motor damping constant (Nm/(rad/s))
g=9.81 ; % Acceleration of gravity (m/s^2)

%-----------State Space Model----------------


xdot1 = x3;
xdot2 = x4;
xdot3 = x2*g/(1+2/5*(Rb)^2);
xdot4 = -x1*m_ball*g/J_bm-x4*(Kc*Ke/R+b)/J_bm+u*Kc/(R*J_bm);

A=[0 0 1 0; 0 0 0 1;0 g/(1+2/5*(Rb)^2) 0 0;-m_ball*g/J_bm 0 0 -


(b+Kc*Ke/R)/J_bm];
B=[0;0;0;Kc/(R*J_bm)];
C=[1 0 0 0;0 1 0 0];
D=0;
sys = ss(A,B,C,D);
H = tf(sys);
%-------------numericial data--------------
%A=[0 0 1 0; 0 0 0 1; 0 9.8084 0 0;-8.9182 0 0 -113.0289];
%B=[0;0;0;17.8545];
%C=[1 0 0 0;0 1 0 0];
%D=0;

% Check controllability and the condition number


Cm=ctrb(A,B);
Rank_c=rank(Cm);
cond(Cm);
% Check observability and condition number
Om=obsv(A,C);
Rank_o=rank(Om);
cond(Om);

%--------------LQ design------------------------
q=10000;
Q=[q 0 0 0;0 100 0 0;0 0 10 0; 0 0 0 10];
RR=1;
[G,M,ecl]=lqr(A,B,Q,RR);

%--------------FOO------------------------------
%Assign closed loop poles and calculate the estimate gain matrix
Kt = place(A',C',[-20+10j,-20-10j,-40+15j,-40-15j]);
K=Kt';
Ahat = A - K*C;
Appendix B
Arduino code for ball and beam control system
#include <FlexiTimer2.h>
//============ Setup Variable for motor =====================
#define motorDirF 10
#define motorDirB 11
#define motorPWM 9
//============= Setup Variable for the IR Sensor =============
#define IRpinLT A8
#define IRpinRT A14
float distanceLT = 0.00;
float distanceRT = 0.00;
float dis = 0.00;
//============ Control Gains ================================
const float G1 = 99.5018;
const float G2 = 77.3276;
const float G3 = 39.8344;
const float G4 = 1.3335;
//observer gain
const float K1 = 43.8;
const float K2 = 7.2;
const float K3 = 50.2;
const float K4 = 5803.8;
//============ Static Variables ============================
static float t = 0.0; // Time in sec
static long Ts = 10; // Ts in millise (change as appropriate)
static float dt = Ts / 1000; //sample time in sec
static float x3 = 0.0; //x1 = x, x2 = theta, x3 = xdot, x4 = thetadot
static float x4 = 0.0;
//estimated state variables
static float x1hat = 0.0;
static float x2hat = 0.0;
static float x3hat = 0.0;
static float x4hat = 0.0;
static float dx1hat = 0.0;
static float dx2hat = 0.0;
static float dx3hat = 0.0;
static float dx4hat = 0.0;
//residual
static float r1 = 0.0;
static float r2 = 0.0;
//============ Ordinary Variables ==========================
unsigned long time;
const float pi = 3.141592;
int dutycycle;
int PWM_val;
float count;
volatile float u;
volatile float v;
volatile float x1;
volatile float x2;
//============= Read Counter ===============================
/* Variable Decleration*/
//Counters Output Pins
int counterInputs[8] = {53, 51, 49, 47, 45, 43, 41, 39}; // this is the standard but it can be connected
differently depending on the rover.
//Counter select Pins
int counterSEL = 36;
//Counter enable pins
int counterOE = 34;
//Counter reset Pins
int counterRST = 32;
float counterRead(int inputs[8], int _OE, int _SEL) {
float value = 0;
//Enable the counter
digitalWrite(_OE, LOW);
//Select High Byte
digitalWrite(_SEL, LOW);
//Read High Byte from Counter
for (int i = 8 ; i <= 15; i++ ) {
value = value - digitalRead(inputs[i - 8]) * pow(2, i - 8);
}
//Select Low Byte
digitalWrite(_SEL, HIGH);
//Read Low Byte
for (int i = 0; i < 8; i++) {
value = value + digitalRead(inputs[i]) * pow(2, i);
}
digitalWrite(_OE, HIGH);
return value;
}
//=============== Read IR Sensors =========================
//This method return a distance value in cm. negative value represents ball is on the left hand side,
positive value represents ball is on the right hand side
float Sensors_read() {
float position;
float voltsLT = analogRead(IRpinLT) * 0.0048828125;
distanceLT = 29, 988 * pow(voltsLT, -1.1173);
float voltsRT = analogRead(IRpinRT) * 0.0048828125;
distanceRT = 32.75 * pow(voltsRT, -1.10);
position = (distanceLT - distanceRT) / 100;
return position;
}
//=============== Saturation function ======================
float sat(float v) {
float u;
if (abs(v) <= 12) {
u = v;
}
else if (abs(v) > 12 && v > 0) {
u = 12;
}
else {
u = - 12;
}
return u;
}
//============ Ball and Beam Control Algorithm =============
void BallAndBeamControl() {
//Read ball position from sensors in meter.
x1 = Sensors_read();
//Read counts from HCTL2016
count = counterRead(counterInputs, counterOE, counterSEL);
//Calculate the rotation angle of the motor
x2 = count * 2 * pi / 8000;
//Compute control signal
v = -G1 * x1 - G2 * x2 - G3 * x3 - G4 * x4;
//saturation function
u = sat(v);
//output control signal to PWM
PWM_val = map(u, -12, 12, -255, 255);
dutycycle = abs(PWM_val);
if (PWM_val < 0) {
digitalWrite(motorDirF, LOW);
digitalWrite(motorDirB, HIGH);
analogWrite(motorPWM, dutycycle);
}
else if (PWM_val > 0) {
digitalWrite(motorDirF, HIGH);
digitalWrite(motorDirB, LOW);
analogWrite(motorPWM, dutycycle);
}
else {
analogWrite(motorPWM, 0);
}
//=========== UPDATE ALL STATE VARIABLE ==============
//update time
t += dt;
//updat state variables
r1 = x1 - x1hat;
r2 = x2 - x2hat;
dx1hat = (x3hat + K1 * r1) * dt;
dx2hat = (x4hat + K2 * r1) * dt;
dx3hat = (x2hat * 9.8084 + K3 * r2) * dt;
dx4hat = (-x1hat * 8.9182 - x4hat * 113.0289 + u * 17.8545 + K4 * r2) * dt;
x1hat += dx1hat;
x2hat += dx2hat;
x3hat += dx3hat;
x4hat += dx4hat;
//update state variable
x1 = x1hat;
x2 = x2hat;
x3 = x3hat;
x4 = x4hat;
}
void setup() {
//============= Initializing Counter's pins ================
pinMode(counterSEL, OUTPUT);
pinMode(counterOE, OUTPUT);
pinMode(counterRST, OUTPUT);
for (int i = 0; i < 8; i++) {
pinMode(counterInputs[i], INPUT);
}
//Reset the counter.
digitalWrite(counterRST, LOW);
digitalWrite(counterRST, HIGH);
//Setup IR sensor pins
pinMode(IRpinLT, INPUT);
pinMode(IRpinRT, INPUT);
//Setup Motor pins
pinMode(motorDirF, OUTPUT);
pinMode(motorDirB, OUTPUT);
pinMode(motorPWM, OUTPUT);
//============ This is the interrupt service routine =========
FlexiTimer2::set(Ts, BallAndBeamControl);
FlexiTimer2::start();
Serial.begin(9600);
}
void loop() {}
Appendix c
System connection and pin assignment

Potrebbero piacerti anche