Parameters

From Class Wiki
Jump to navigation Jump to search
The printable version is no longer supported and may have rendering errors. Please update your browser bookmarks and please use the default browser print function instead.

Inverted Penululm Project

Below is some Octave or Matlab code with the parameters of one of the pendulums.

% Double Pendulum Parameters (Tentative:  There are two pendulums with different parameters.  I'm not sure which these go to.)

% Run parameters
%f = input('Control Frequency (Hz) = ');
%crad = input('Pole Radius (1/s) = ');
%psi = input('Spreading Angle (deg) = ');
%eta = psi*pi/180;
%obshift = input('Observer Shift = ');
%Trun = input('Run Time (s) = ');
f=130;
crad=19;
psi=10;
eta=psi*pi/180;
obshift=2;
Trun=60;

kmax = round(f*Trun);
T = 1/f;
Maxpos = 0.25;              % Max carriage travel +- 0.25 m
Maxangle = 0.175;           % Max rod angle -- 10 deg
Maxvoltage = 20;            % Max motor voltage, V
pstart = 0.005;             % Carriage position starting limit, m
astart = 1*pi/180;          % Angle starting limit, rad

g = 9.81;                   % m/s^2     Gravitational constant

% SYSTEM PARAMETERS
% Measured Mechanical Parameters
d1 = 0.323;    % m            Length of pendulum 1 (long)
d2 = 0.079;         % m            Length of pendulum 2 (short)
%mp1 = 0.0208;        % kg        Mass of pendulum 1
mp1 = 0.0318;
%mp2 = 0.0050;        % kg        Mass of pendulum 2
mp2 = 0.0085;
m = 0.3163;            % kg        Mass of carriage
rd = 0.0254/2;      % m            Drive pulley radius
md = 0.0375;         % kg        Mass of drive pulley (cylinder)
%mc1 = 0.0036;        % kg        Mass of clamp 1*
%mc2 = 0.0036;        % kg        Mass of clamp 2*
mc1 = 0.0085;
mc2 = mc1;

% *Clamp Dimensions
%  Rectangular 0.0254 x 0.01143 m
%  The pivot shaft is 0.00714 m from the end

% Motor Parameters (Data Sheet)
Im = 43e-7;     % kg m^2/rad    Rotor moment of inertia
R = 4.09;       % ohms            Resistance
kt = 0.0351;    % Nm/A            Torque constant
ke = 0.0351;    % Vs/rad        Back emf constant

% Derived Mechanical Parameters

                                % kg m^2/rad    Moment of inertia, clamp 1
%Ic1 = mc1*(0.01143^2 + 0.0254^2)/12 + mc1*(0.0127-0.00714)^2;
Ic1 = mc1*(0.0098^2 + 0.0379^2)/12;
Ic2 = Ic1;                      % kg m^2/rad    Moment of inertia, clamp 2
Id = md*(rd^2)/2;               % kg m^2/rad    Moment of inertia, drive pulley
Imd = Im + Id;                  % kg m^2/rad    Moment of inertia, combined

J1 = Ic1 + mp1*(d1^2)/3;        % Total moment of inertia, pendulum 1 (long)
J2 = Ic2 + mp2*(d2^2)/3;        % Total moment of inertia, pendulum 2 (short)
Jd = Im + Id;                   % Total moment of inertia, motor drive
Mc = m + mc1 + mc2;             % Total carriage mass

% Friction Test Data
%   Carriage Slope = 19 deg;  Terminal Velocity xdotss = 0.312 m/s; From
%        twincarriage.m; formula b = m g sin(theta)/xdotss
%   Pendulum 1 (long) Exponent a1 = 0.0756 1/s;  From longfit.m
%   Pendulum 2 (short) Exponent a2 = 0.2922 1/s; From shortfit.m
%        formula b = 2 a J

%alpha = 19;
alpha = 12.2;
%xdotss = 0.312;
xdotss = 0.4852;
%a1 = 0.0756;
%a2 = 0.2922;
a1 = 0.0185;
a2 = 0.012;
                        % Ns/m    Viscous friction of carriage system
b = (Mc + mp1 + mp2)*g*sin(alpha*pi/180)/xdotss;
b1 = 2*a1*J1;            % Nms/rad    Viscous friction of pendulum 1 (rotational)
b2 = 2*a2*J2;            % Nms/rad    Viscous friction of pendulum 2 (rotational)


        % It appears that the labels on the pendulum for positive x
        % and negative x are backwards, and it appears that the angle 
        % for theta is also backwards.  It is really CW is positive,
        % not CCW.  A positive pwm value to the motor moves the pendulum
        % toward what is marked as NEG x (to the right).
        % I made the scales below reflect that to correct the sign problems
        % with the measured variables.  You must correct the motor voltage
        % in your code.


scale = [-rd*2*pi/4096  -2*pi/4096 -0.05/250];


T = 1/f;

Pendulum Hardware Control

Here is a pendulum hardware control script that should help with yours. You basically need to modify tcpdemo4 (immediately below) with your control code in the appropriate place.

% tcpdemo4 - demo script to communicate with new pendulum interface
%
% Syntax:
%    tcpdemo4
%
% cnt is number of packets to receive
% srate is sample rate in Hz
%
% communication with the new ctrlbox is by the ctrlbox.m library
%
% receive data:
%    [long_pend_angle, short_pend_angle, motor_shaft_angle, knob_angle]
%    pendulum and motor shaft angles are 4096 counts/rev
%
% Pwm values are +/-32767 for full scale motor voltage.
% A positive pwm value causes the carriage to move in +X direction.
% Encoder data is 32 bit raw counts, clockwise is positive.
% Encoders are zeroed when "PROG" button is pressed on Spartan3 board.
% Leave long pendulum hanging down when zeroing, then rotate up 180 degrees
% when starting.  Subtract pi/2 from long_pend_angle to remove offset.
%
% Protocol is send pwm value, then read encoders.  Sampling in ctrlbox is
% done according to a hardware sample clock.
%
pkg load sockets control;
ctrlbox;            % load ctrlbox comm functions

cnt=4000;            % number of times through loop
srate = 400;            % sample rate in Hz

store = zeros(cnt,5);
rdata = [0,0,0,0];        % receive data

% Connection must first be established with the ctrlbox.

try
    ctrlbox_init();

    disp('finished init');

    % send sample period
    period = 1000000./srate;
    
    ctrlbox_send(0,0,period);

    disp('finished send');

    x = 1:cnt;
    tic;
        for c=1:cnt
        % read encoder values
        rdata = ctrlbox_recv();

        %
        % your control law goes here
        %

        % example control law pwm generation
        pwm = (30000 * sin(6.28*8*double(c)/min(cnt,1000))) - rdata(1);

        % write pwm values and enable motor
        ctrlbox_send(pwm, 1, 0);

        % force matlab to check for interrupts and flush event queue
        drawnow;
           
        % save data
        store(c,:) = [rdata,pwm];
    end
    runtime = toc;
    fprintf('transactions=%d seconds=%d transactions/sec=%f\n',
        c, runtime, c/runtime);
    drawnow;

catch
    % if something failed, display error and loop count
    fprintf('c=%d\n',c);
    disp(lasterror.message);
end
% disable motor and disconnect
ctrlbox_shutdown();

The function ctrlbox which is used above is given below for your edification.

% ctrlbox.m
%
%    Functions for communication with the inverted pendulum
%    ctrlbox interface via ethernet.
1;
%
% ctrlbox_init - initialize connection to ctrlbox
%
function rval = ctrlbox_init()
    global ctrlbox_con;

    ctrlbox_con = socket();
    sinfo = struct("addr","169.254.0.100", "port", 47820);
    rval = connect(ctrlbox_con,sinfo);

    return;
endfunction
%
% ctrlbox_send(cmdval,enable,period)
%  - send command value, enable, and sample period to ctrlbox
%    - cmdval = -32768 to +32767, where 32767=100% of DC bus voltage
%    - enable = 0 or 1
%    - period in usec
%
%  - future: measure time avg of pwm value, shutoff motor
%    if excessive.
function rval = ctrlbox_send(cmdval,enable,period)
    global ctrlbox_con;

    pwm = min(max(cmdval,-32000),32000);
    data = [pwm, 0, enable, period];
    send(ctrlbox_con, typecast(int32(data(1:4)),'uint8'));

    rval = 0;
    return;
endfunction
%
% ctrlbox_recv - receive an array of four values from ctrlbox
%
function data = ctrlbox_recv()
    global ctrlbox_con;

    [rdata,len] = recv(ctrlbox_con,16);

    if (len ~= 16)
        fprintf('short data: %d\n', len);
    end

    data = double(typecast(rdata,'int32'));
    return;

endfunction

%
% ctrlbox_shutdown - shutdown connection to ctrlbox
%
function ctrlbox_shutdown()
    global ctrlbox_con;

    % turn off motor
    send(ctrlbox_con,typecast(int32([0,0,0,0]),'uint8'));

    disconnect(ctrlbox_con);
endfunction