**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.

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.

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

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

Edit | Attach | ~~Watch~~ | Print version | History: r2 < r1 | Backlinks | Raw View | More topic actions

Topic revision: r2 - 2008-07-21 - JessicaSzweda

Copyright © 2008-2015 by the contributing authors. All material on this collaboration platform is the property of the contributing authors.

Ideas, requests, problems regarding TWiki? Send feedback

Ideas, requests, problems regarding TWiki? Send feedback