linear inverted pendulum, Thesis of Control Systems

Download linear inverted pendulum and more Thesis Control Systems in PDF only on Docsity!

Project Title:

Linear Stepper based Inverted pendulum unit.

Components:

 Stepper motor (nema 17)

 GT2 Pully

 2mm Belt

 2x 8mm rods

 Stop button(Limit switch)

 Rotatory encoder

 8mm bearings

Solid works Model:

Methodology:

Encoder:

The encoder used is Rotary encoder KY-040. The Arduino code for the encoder is as

follow:

Motor Control Code:

Overview:

An inverted pendulum is a classical problem for those who study mechanical engineering and

feedback control theory. In this tutorial I will go through the steps of building an inverted

pendulum on a cart stabilized with a DC motor. I will describe how to measure motor’s

parameters and calculate coefficients for the feedback regulator.

Equations of motion:

Equations of motion can be obtained by differentiating of a Lagrangian which will give the

following system:

Stabilization with feedback:

In order to keep the pendulum in the upright position an external force should be applied

depending on the state of the system. In this project full state control is used meaning that the

control (u) equals to

where x is vector of state, K is a vector of coefficients obtained using LQR. I simulated

controlled pendulum with this script. In physical device we can measure motor's position and an

angle of the rod using incremental rotary encoder, the velocities are then calculated as a

derivative.

Motor control:

In order to stabilize pendulum we need to apply a certain force which we calculated in a previous

section. But the control signal coming out of the Arduino is the width of the PWM signal,

basically is the voltage applied to the DC motor. In order to calculate the voltage given the force

required we need to model the DC motor and estimate it’s parameters.

The DC motor is modeled by the following equation:

We don’t need to know the exact parameters, instead we can

simplify the equations with regards to friction as follows:

In order to determine parameters (a, b, c) we can record the relation of cart's velocity to time

depending on different voltages.

Arduino Code:

• == Inverted pendulum stabilisation with state control with DC motor ==
• == Hardware specification ==

const float THETA_THRESHOLD = PI / 12 ; const float PI2 = 2.0 * PI; volatile long encoderValue = 0L; volatile long lastEncoded = 0L; volatile long refEncoderValue = 0 ; volatile long lastRefEncoded = 0 ; unsigned long now = 0L; unsigned long lastTimeMicros = 0L; float x, last_x, v, dt; float theta, last_theta, w; float control, u; unsigned long log_prescaler = 0 ; void encoderHandler(); void refEncoderHandler(); void setup() { // setting PWD frequency on pin 10 to 31kHz TCCR2B = (TCCR2B & 0b11111000) | 0x01; pinMode(OUTPUT_A, INPUT_PULLUP); pinMode(OUTPUT_B, INPUT_PULLUP); pinMode(REF_OUT_A, INPUT_PULLUP); pinMode(REF_OUT_B, INPUT_PULLUP); attachInterrupt(digitalPinToInterrupt(OUTPUT_A), encoderHandler, CHANGE); attachInterrupt(digitalPinToInterrupt(OUTPUT_B), encoderHandler, CHANGE); attachInterrupt(digitalPinToInterrupt(REF_OUT_A), refEncoderHandler, CHANGE); attachInterrupt(digitalPinToInterrupt(REF_OUT_B), refEncoderHandler, CHANGE); pinMode(PWM_PIN, OUTPUT);

pinMode(DIR_PIN, OUTPUT); digitalWrite(DIR_PIN, LOW); Serial.begin( 9600 ); lastTimeMicros = 0L; } float saturate(float v, float maxValue) { if (fabs(v) > maxValue) { return (v > 0 )? maxValue : -maxValue; } else { return v; } } float getAngle(long pulses, long ppr) { float angle = (PI + PI2 * pulses / ppr); while (angle > PI) { angle -= PI2; } while (angle < -PI) { angle += PI2; } return angle; } float getCartDistance(long pulses, long ppr) { return 2.0 * PI * pulses / PPR * SHAFT_R; } void driveMotor(float u) { digitalWrite(DIR_PIN, u > 0.0? LOW : HIGH); analogWrite(PWM_PIN, fabs(u)); } boolean isControllable(float theta) { return fabs(theta) < THETA_THRESHOLD; } void log_state(float control, float u) { if (fabs(w) > 100 ) { return;

void encoderHandler() { int MSB = (PINE & ( 1 << PE5)) >> PE5; //MSB = most significant bit int LSB = (PINE & ( 1 << PE4)) >> PE4; //LSB = least significant bit int encoded = (MSB << 1 ) | LSB; //converting the 2 pin value to single number int sum = (lastEncoded << 2 ) | encoded; //adding it to the previous encoded value if(sum == 0b1101 || sum == 0b0100 || sum == 0b0010 || sum == 0b1011) { encoderValue++; //CW } if(sum == 0b1110 || sum == 0b0111 || sum == 0b0001 || sum == 0b1000) { encoderValue--; //CCW } lastEncoded = encoded; //store this value for next time } /**

• Pendulum encoder handler
• Encoder attached to pins:
• Phase A - 18 PD
• Phase B - 19 PD */ void refEncoderHandler() { int MSB = (PIND & ( 1 << PD3)) >> PD3; //MSB = most significant bit int LSB = (PIND & ( 1 << PD2)) >> PD2; //LSB = least significant bit int encoded = (MSB << 1 ) | LSB; //converting the 2 pin value to single number int sum = (lastRefEncoded << 2 ) | encoded; //adding it to the previous encoded value if(sum == 0b1101 || sum == 0b0100 || sum == 0b0010 || sum == 0b1011) { refEncoderValue++; //CW } if(sum == 0b1110 || sum == 0b0111 || sum == 0b0001 || sum == 0b1000) { refEncoderValue--; //CCW }

lastRefEncoded = encoded; //store this value for next time }

MATLAB Code:

% SETUP_IP02_SIP % % IP02 Single Inverted Pendulum (SIP) Control Lab: % Design of a LQR position controller % % SETUP_IP02_SIP sets the SIP and IP % model parameters accordingly to the user-defined configuration. % SETUP_IP02_SIP can also set the controllers' parameters, % accordingly to the user-defined desired specifications. % % Copyright (C) 2012 Quanser Consulting Inc. % Quanser Consulting Inc. clear all; % ############### USER-DEFINED IP02 with SIP CONFIGURATION ############### % if IP02: Type of Cart Load: set to 'NO_LOAD', 'WEIGHT' IP02_LOAD_TYPE = 'NO_LOAD'; % IP02_LOAD_TYPE = 'WEIGHT'; % Type of single pendulum: set to 'LONG_24IN', 'MEDIUM_12IN' PEND_TYPE = 'LONG_24IN'; % PEND_TYPE = 'MEDIUM_12IN'; % Turn on or off the safety watchdog on the cart position: set it to 1 , or 0 X_LIM_ENABLE = 1; % safety watchdog turned ON %X_LIM_ENABLE = 0; % safety watchdog turned OFF % Safety Limits on the cart displacement (m) X_MAX = 0.3; % cart displacement maximum safety position (m) X_MIN = - X_MAX; % cart displacement minimum safety position (m) % Turn on or off the safety watchdog on the pendulum angle: set it to 1 , or 0 ALPHA_LIM_ENABLE = 1; % safety watchdog turned ON %ALPHA_LIM_ENABLE = 0; % safety watchdog turned OFF % Safety Limits on the pendulum angle (deg) global ALPHA_MAX ALPHA_MIN ALPHA_MAX = 20; % pendulum angle maximum safety position (deg) ALPHA_MIN = - ALPHA_MAX; % pendulum angle minimum safety position (deg) % Amplifier Gain: set VoltPAQ amplifier gain to 1 K_AMP = 1; % Amplifier Type: set to 'VoltPAQ' or 'Q3' AMP_TYPE = 'VoltPAQ'; % AMP_TYPE = 'Q3'; % Digital-to-Analog Maximum Voltage (V); for MultiQ cards set to 10 VMAX_DAC = 10; % ############### USER-DEFINED CONTROLLER DESIGN ############### % Type of Controller: set it to 'LQR_AUTO', 'MANUAL' %CONTROLLER_TYPE = 'LQR_AUTO'; % LQR controller design: automatic mode CONTROLLER_TYPE = 'MANUAL'; % controller design: manual mode % Initial Condition on alpha, i.e. pendulum angle at t = 0 (deg) global IC_ALPHA % IC_ALPHA0 = 0.1; IC_ALPHA0 = 0; % conversion to radians

disp( [ 'K(2) = ' num2str( K(2) ) ' V/rad' ] ) disp( [ 'K(3) = ' num2str( K(3) ) ' V.s/m' ] ) disp( [ 'K(4) = ' num2str( K(4) ) ' V.s/rad' ] ) else error( 'Error: Please set the type of controller that you wish to implement.' ) end

Simulink Model:

Physical Model:

Figure: initial form

Figure : Final form