WiiLab Example Code - Inverted Pendulum

Note: MATLAB R2007a or greater is required for this code to run correctly. The program uses the Wiimote class, along with the UND function set EG111-H.

Annotations

The inverted pendulum is a classic problem involving a pendulum with its mass located above its pivot point, making it inherently unstable. The goal is to keep it balanced upright, which can be done by applying torque at the pivot point or by moving the pivot point horizontally, often by being mounted on a small, motion-capable cart.

This implementation makes use of the former balancing method, with the Wiimote's X-axis acceleration used as a representation of applied torque. The user must continually twist the Wiimote either clockwise or counter-clockwise to attempt to balance the inverted pendulum. A bit of simulated gravity adds to the realism and difficulty.

Program Breakdown

I will break down the program into pieces to make it easier to explain and to understand. The entire block of code can also be found at the bottom of this page.

I have defined the demo as a function so it can be called remotely without the user having to be bogged down by all the code.
The Wiimote functions and basic graphics functions are made accessible for use in the program. Then an empty window is initialized, and a connection with the Wiimote is established.

function InvertedPendulum()
%  usage:    InvertedPendulum()
%  purpose:  allows the user to attempt to balance an
%            inverted pendulum using the Nintendo Wiimote

    % add the path containing simple graphics functions
    addpath .\EG111-H
    % add the path containing the Wiimote functions
    addpath .\WiimoteFunctions
    
    % create a new empty window
    createWindow(600, 450, 400, 300);
    setTitle('Inverted Pendulum');
    
    % connect the Wiimote
    initializeWiimote();

A few constants are defined to make the program easy to modify. We establish a fixed length of the rod part of the pendulum. The lowest realistic angle between the rod and the ground is also calculated, which will be useful when determining how far the pendulum can rotate before it hits the bottom of the window. The 'center' constant aids in finding the pendulum's equilibrium point.

    % CONSTANTS
    % length of rod
    length = 155;
    % angle of elevation such that the mass is resting on the ground
    lowestAngle = asin(20/(length + 20));
    % equilibrium point
    center = 200;
    % /CONSTANTS

The x- and y-coordinates of the ends of the rod are put into an array to meet MATLAB's guidelines for the 'line' function. The given coordinates put the inverted pendulum into a state of equilibrium to begin with. Then the pendulum is drawn as a simple thick line with a large circle at one end.

    % coordinates of the rod's ends
    x = [center center];
    y = [0 length];
            
    % draw the inverse pendulum
    rod = line(x, y, 'Color', 'black', 'LineWidth', 5);
    mass = drawCircle(center, 175, 20);

The program checks to see that the Wiimote was successfully connected, then displays basic directions for the user.

    if(isWiimoteConnected() > 0)
        % user directions
        t1 = text(center, 290, 'Push A to begin', 'HorizontalAlignment', 'center', 'FontSize', 12);
    
        % user should push the Wiimote's A button to begin
        waitForButtonPress('A');
    
        % cycle through more directions
        hide(t1);
        t2 = text(center, 290, 'Tilt the Wiimote to keep the mass balanced.', 'HorizontalAlignment', 'center', 'FontSize', 12);
        pause(4);
        hide(t2);
        t3 = text(center, 290, 'Ready?', 'HorizontalAlignment', 'center', 'FontSize', 12);
        pause(2);
        hide(t3);
        text(center, 290, 'GO!', 'HorizontalAlignment', 'center', 'FontSize', 12);
        pause(0.1);
        text(390, 10, 'Press HOME to quit', 'HorizontalAlignment', 'right');

A loop is entered, so that the user can continue to try balancing the pendulum until the Wiimote's 'HOME' button is pressed.

        % pressing the Wiimote's HOME button will terminate the program
        while (~isButtonPressed('HOME'))

The pendulum is rotated around its pivot point based on the direction and amount of twisting or acceleration along its X-axis. The value returned by the Wiimote's accelerometer is multiplied by a negative factor because MATLAB's built-in 'rotate' method runs counter-clockwise, as is standard in most mathematical applications.

The horizontal displacement of the mass from its balancing point is used to calculate a proportional amount of gravity to further affect the angle of rotation.

            % want to rotate the inverse pendulum based on the Wiimote's
            % x-acceleration
            theta = -2*getWiimoteAccel();

            % obtain the mass's current x-coordinate
            XCMass = getCenter(mass);
            % account for gravity by increasing the angle of rotation as the
            % pendulum gets further from its equilibrium point
            theta = theta - 0.08*(XCMass - center);

A bit of trigonometry is employed to ensure that the pendulum will only rotate within the confines of the figure.

            % use a basic trigonometric function to calculate the pendulum's
            % current angle of elevation
            beta = acos((XCMass-center)/(length + 20));
            % determine what the new angle of elevation will be upon rotation
            alpha = beta - deg2rad(-1*theta);
            % use the new angle to determine what the mass's height will be
            newYCMass = (length + 20)*sin(alpha);

If the angle of rotation that was calculated earlier, based on the Wiimote's acceleration and "gravity", would cause part of the pendulum to unrealistically exit the window, the angle is adjusted appropriately. It is also important to take into account in which half of the figure the pendulum is located, as this will affect the measure of the lowest angle that we mentioned earlier.

            % if the obtained angle of rotation would put the pendulum
            % unrealistically "into the ground", adjust the angle by which it
            % will rotate so that it just hits the bottom of the window and
            % stops there
            if (newYCMass < 20)
                alpha = lowestAngle;
                % if the mass is to the left of the center/equilibrim point
                if (XCMass < center)
                    % adjust the angle so that it correctly lies in the second
                    % quadrant
                    alpha = pi-alpha;
                end %if
                % adjust the rotation angle based on the lowest realistic angle
                % of elevation and the pendulum's current angle of elevation
                theta = -1*rad2deg(beta - alpha);
            end %if

Now that the correct angle has been determined, the pendulum can finally be rotated, followed by a slight pause at the end of the loop, required for any sort of "animation" in MATLAB.

            % rotate the pendulum around its fixed base by the specified angle
            rotate(rod, [0,0,1], theta, [center,0,0]);
            rotate(mass, [0,0,1], theta, [center,0,0]);

            % slight pause to keep animation running smoothly
            pause(0.1);

        end %while

Finally, the figure is closed and the Wiimote disconnected so it can be connected to again for use with other programs.

        pause(0.5);
        % close window
        close;
        % done using Wiimote
        disconnectWiimote();

If the earlier attempt to connect to the Wiimote was unsuccessful, display a message in the window to tell the user, then close the figure.

    % if wiimote is not connected    
    else 
        % notify the user
        text(center, 290, 'Wiimote is not connected.  Please try again.', ...
            'HorizontalAlignment', 'center', 'FontSize', 12);
        
        pause(1.5);
        % close window
        close;
        
    end %if
    
end

Raw Source Code

The source code is also available as a .m file download at the bottom of the page.

function InvertedPendulum()
%  usage:    InvertedPendulum()
%  purpose:  allows the user to attempt to balance an
%            inverted pendulum using the Nintendo Wiimote

    % add the path containing simple graphics funcions
    addpath .\EG111-H
    % add the path containing the wiimote functions
    addpath .\WiimoteFunctions
    
    % create a new empty window
    createWindow(600, 450, 400, 300);
    setTitle('Inverted Pendulum');
    
    % connect the Wiimote
    initializeWiimote();
    
    % CONSTANTS
    % length of rod
    length = 155;
    % angle of elevation such that the mass is resting on the ground
    lowestAngle = asin(20/(length + 20));
    % equilibrium point
    center = 200;
    % /CONSTANTS
    
    % coordinates of the rod's ends
    x = [center center];
    y = [0 length];
            
    % draw the inverse pendulum
    rod = line(x, y, 'Color', 'black', 'LineWidth', 5);
    mass = drawCircle(center, 175, 20);
    
    if(isWiimoteConnected() > 0)
        % user directions
        t1 = text(center, 290, 'Push A to begin', 'HorizontalAlignment', 'center', 'FontSize', 12);
    
        % user should push the Wiimote's A button to begin
        waitForButtonPress('A');
    
        % cycle through more directions
        hide(t1);
        t2 = text(center, 290, 'Tilt the Wiimote to keep the mass balanced.', 'HorizontalAlignment', 'center', 'FontSize', 12);
        pause(4);
        hide(t2);
        t3 = text(center, 290, 'Ready?', 'HorizontalAlignment', 'center', 'FontSize', 12);
        pause(2);
        hide(t3);
        text(center, 290, 'GO!', 'HorizontalAlignment', 'center', 'FontSize', 12);
        pause(0.1);
        text(390, 10, 'Press HOME to quit', 'HorizontalAlignment', 'right');

        % pressing the Wiimote's HOME button will terminate the program
        while (~isButtonPressed('HOME'))
            % want to rotate the inverse pendulum based on the Wiimote's
            % x-acceleration
            theta = -2*getWiimoteAccel();

            % obtain the mass's current x-coordinate
            XCMass = getCenter(mass);
            % account for gravity by increasing the angle of rotation as the
            % pendulum gets further from its equilibrium point
            theta = theta - 0.08*(XCMass - center);

            % use a basic trigonometric function to calculate the pendulum's
            % current angle of elevation
            beta = acos((XCMass-center)/(length + 20));
            % determine what the new angle of elevation will be upon rotation
            alpha = beta - deg2rad(-1*theta);
            % use the new angle to determine what the mass's height will be
            newYCMass = (length + 20)*sin(alpha);

            % if the obtained angle of rotation would put the pendulum
            % unrealistically "into the ground", adjust the angle by which it
            % will rotate so that it just hits the bottom of the window and
            % stops there
            if (newYCMass < 20)
                alpha = lowestAngle;
                % if the mass is to the left of the center/equilibrim point
                if (XCMass < center)
                    % adjust the angle so that it correctly lies in the second
                    % quadrant
                    alpha = pi-alpha;
                end %if
                % adjust the rotation angle based on the lowest realistic angle
                % of elevation and the pendulum's current angle of elevation
                theta = -1*rad2deg(beta - alpha);
            end %if

            % rotate the pendulum around its fixed base by the specified angle
            rotate(rod, [0,0,1], theta, [center,0,0]);
            rotate(mass, [0,0,1], theta, [center,0,0]);

            % slight pause to keep animation running smoothly
            pause(0.1);

        end %while
        
        pause(0.5);
        % close window
        close;
        % done using Wiimote
        disconnectWiimote();
        
    % if wiimote is not connected    
    else 
        % notify the user
        text(center, 290, 'Wiimote is not connected.  Please try again.', ...
            'HorizontalAlignment', 'center', 'FontSize', 12);
        
        pause(1.5);
        % close window
        close;
        
    end %if
    
end

• InvertedPendulum.m