three dimensional driven-arm inverted pendulum

THE FLOATING DUTCHMEN Three Dimensional Driven-Arm Inverted Pendulum

Project Report for ECSE-4962 Control Systems Design

Team 2 Teresa Bernardi

Brian Lewis Matthew Rosmarin

Friday, March 24, 2006

Rensselaer Polytechnic Institute

Table of Contents Executive Summary:........................................................................................................... 1 Introduction......................................................................................................................... 2 Preliminary Results............................................................................................................. 4

Introduction..................................................................................................................... 4 Section 1: Physical Modeling ......................................................................................... 4 Section 2: Parameter ID.................................................................................................. 9

Friction:....................................................................................................................... 9 Summary of progress: ....................................................................................................... 11

Plan and schedule:......................................................................................................... 11 Unanticipated events:.................................................................................................... 11 Further Plan of Action: ................................................................................................. 12 Costs:............................................................................................................................. 12

Bibliography: .................................................................................................................... 13 Appendix A: Terms........................................................................................................... 14 Appendix B: Costs ............................................................................................................ 15 Appendix C: SimMechanics Modeling............................................................................. 16 Appendix D: Schedule ...................................................................................................... 19 Appendix F: Mechanical Drawings ................................................................................. 21 Appendix G: Wiring Chart............................................................................................... 28 Appendix H: Batch File Code.......................................................................................... 30

Executive Summary: The purpose of this project is to create a Three Dimensional Arm-Driven Inverted

Pendulum. In order to meet this goal, a universal joint with two high resolution encoders

needs to be constructed. See appendix for drawings. While construction of the joint was

being built, parameter id and computer simulations were completed. The current stage of

the project is in system integration and modification. There have been a few

unanticipated events such as torque requirements, and range of motion. That has required

a few mechanical modifications. Even though the team has had these difficulties, the

project is on schedule to be completed by the end of the semester.

1

Introduction The primary objective of the project is to design and balance a Three Dimensional Arm-Driven Inverted Pendulum. Since the size and dimensions of this system greatly influence how easy or difficult it will be to control, four different mechanical configurations will be created. These configurations will range from being very easy to control to being very difficult to control. Part of the project will be to identify what mechanical characteristics cause the system to be easy or difficult to control. Once the configurations are decided upon, then they will have to be implemented in hardware. Secondary objectives of the project are implementing the entire controller on the FPGA section of the cRIO and adding a human interface that will allow the user to act as the control system. The first of these two goals will require special attention to numerical issues and filtering; while the second will contribute a human versus computer competition aspect to the project. Some objectives that will have to be achieved in order for the overall goal of balancing a Three Dimensional Arm-Driven Inverted Pendulum to be accomplished include Matlab simulation, control design, physical system design, and software implementation. Other than the general specification of the system balancing, the Inverted Pendulum must also be able to compensate for disturbances in the form of a non-zero initial angle, non-zero initial velocity, and variation in arm length by ±10%, and torque disturbance. The two areas of analyzing system performance will be first in simulation and then in final implementation. The design approach has three main components: the physical model, control, and implementation and integration. The physical model stage is where the system modeling, both in Matlab and derived by hand, and the parameter identification occur. The control stage consists of physical parameter selection for the arms, linearization of the model, control design, and nonlinear simulation and evaluation. The mechanical fabrication of the system, the electrical aspects and the controller are completed in the implementation and integration stage. The project so far consists of a Matlab model using SimMechanics and Simulink, a Solidworks model, a basic configuration of the physical system, a manual control joystick, and a parameter test interface. A control interface is in the works and is scheduled to be completed concurrent with the completion of the manufacturing of the sized arms. The batch file to determine the optimal sizing of the arms is completed and will be implemented as soon as an accurate parameter identification of the system is finished. Due to the available torque from the motors and the current gear ratios, there are concerns that the actuators will not be able to physically control the actuated arm. This concern sprouts from experiments done with a sample arm set, where the actuators would be unable to recover from any near horizontal angle when a significant weight was being driven. Originally, two dimensional swing up was thought to be almost impossible;

2

however, once the torque problems with the motors are solved, swing up should be fairly simple. With respect to the original schedule, the project is basically on target. The reason why some tasks are yet to be completed is that they rely on other tasks that were not scheduled to be completed yet. The fabrication of the physical system is ahead of schedule, especially the universal joint. Unfortunately, until the setup of the basic physical system is complete, some of the system parameters cannot be tested. The sizing of the components relies on accurate measurements of the system parameters; so while the programs designed to calculate the component sizes are in place, they cannot yet be implemented. With respect to the other tasks, the project is on target and is expected to be completed on time. The prognosis for the remainder of the project looks good. If the controller performs as simulation suggests, then the balancing of a three-dimensional arm-driven inverted pendulum seems plausible. Given the present state of affairs, rudimentary swing up and potentially a rotary inverted pendulum configuration may also be possible.

3

Preliminary Results

Introduction The following sections summarize the preliminary results to date. Much of the rest of our work is awaiting the final modifications to the hardware.

Section 1: Physical Modeling As it was planned, the primary method of modeling used in this project is Simulinks’s SimMechanics. MSC Adams was also explored as an alternative, but it was decided that the full analysis using Adams would not be the best use of time. Initial simulations were quite insightful, but on several occasions certain configurations were found to be uncontrollable. Upon further investigation, it was discovered that this was due to an error in the way the inertia of several system components was calculated. Now that the inertia calculations have been fixed, uncontrollable system configurations have not been encountered. Using SimMechanics, batch tests were performed where three parameters were varied. As seen below, the parameters that were varied are the mass of the end weight, the length of the upper arm, and the length of the lower arm.

4

The following plots show some example cases where one parameter was varied. The case below varies the length of the upper arm. The initial condition for the below simulation has the upper arm offset by one degree. In these simulations, the length of the upper arm is varied from 0.1m to 1m, the lower arm is fixed at 0.25m, and the end mass is fixed at 0.1 kg.

5

0 2 4 6 8 10 12 14 16 18 20-5

0

5

10

15

20

Time (sec)

Dis

plac

emen

t (D

eg)

Balancing Arm Angle (Deg) (Refrence to Actuator Arm)

Upper:0.1Lower:0.25EndMass:0.1Upper:0.2Lower:0.25EndMass:0.1Upper:0.3Lower:0.25EndMass:0.1Upper:0.4Lower:0.25EndMass:0.1Upper:0.5Lower:0.25EndMass:0.1Upper:0.6Lower:0.25EndMass:0.1Upper:0.7Lower:0.25EndMass:0.1Upper:0.8Lower:0.25EndMass:0.1Upper:0.9Lower:0.25EndMass:0.1Upper:1Lower:0.25EndMass:0.1

0 2 4 6 8 10 12 14 16 18 20-18

-16

-14

-12

-10

-8

-6

-4

-2

0

2

Time (sec)

Dis

plac

emen

t (D

eg)

Actuator Arm Angle (Deg) (Refrence to Ground)

Upper:0.1Lower:0.25EndMass:0.1Upper:0.2Lower:0.25EndMass:0.1Upper:0.3Lower:0.25EndMass:0.1Upper:0.4Lower:0.25EndMass:0.1Upper:0.5Lower:0.25EndMass:0.1Upper:0.6Lower:0.25EndMass:0.1Upper:0.7Lower:0.25EndMass:0.1Upper:0.8Lower:0.25EndMass:0.1Upper:0.9Lower:0.25EndMass:0.1Upper:1Lower:0.25EndMass:0.1

The case below varies the length of the lower arm. The initial condition for the below simulation has the upper arm offset by one degree. In these simulations, the length of the lower arm is varied from 0.1m to 1m, the upper arm is fixed at 0.25m, and the end mass is fixed at 0.1 kg.

6

0 2 4 6 8 10 12 14 16 18 200

0.5

1

1.5

2

2.5

3

3.5

4

4.5

5

Time (sec)

Dis

plac

emen

t (D

eg)


Upper:0.25Lower:0.1EndMass:0.1Upper:0.25Lower:0.2EndMass:0.1Upper:0.25Lower:0.3EndMass:0.1Upper:0.25Lower:0.4EndMass:0.1Upper:0.25Lower:0.5EndMass:0.1Upper:0.25Lower:0.6EndMass:0.1Upper:0.25Lower:0.7EndMass:0.1Upper:0.25Lower:0.8EndMass:0.1Upper:0.25Lower:0.9EndMass:0.1

0 2 4 6 8 10 12 14 16 18 20-4.5

-4

-3.5

-3

-2.5

-2

-1.5

-1

-0.5

0

Time (sec)

Dis

plac

emen

t (D

eg)


Upper:0.25Lower:0.1EndMass:0.1Upper:0.25Lower:0.2EndMass:0.1Upper:0.25Lower:0.3EndMass:0.1Upper:0.25Lower:0.4EndMass:0.1Upper:0.25Lower:0.5EndMass:0.1Upper:0.25Lower:0.6EndMass:0.1Upper:0.25Lower:0.7EndMass:0.1Upper:0.25Lower:0.8EndMass:0.1Upper:0.25Lower:0.9EndMass:0.1

The next case below varies the mass of the end weight. The initial condition for the below simulation has the upper arm offset by one degree. In these simulations, upper arm is fixed at 0.25m, the lower arm is fixed at 0.25m, and the end mass is varied from 0.1kg to 1kg.

7

0 2 4 6 8 10 12 14 16 18 20-15

-10

-5

0

5

10

15

20

Time (sec)

Dis

plac

emen

t (D

eg)


Upper:0.25Lower:0.25EndMass:0.1Upper:0.25Lower:0.25EndMass:0.2Upper:0.25Lower:0.25EndMass:0.3Upper:0.25Lower:0.25EndMass:0.5Upper:0.25Lower:0.25EndMass:0.6Upper:0.25Lower:0.25EndMass:0.7Upper:0.25Lower:0.25EndMass:0.8Upper:0.25Lower:0.25EndMass:0.9Upper:0.25Lower:0.25EndMass:1

0 2 4 6 8 10 12 14 16 18 20-15

-10

-5

0

5

10

Time (sec)

Dis

plac

emen

t (D

eg)


Upper:0.25Lower:0.25EndMass:0.1Upper:0.25Lower:0.25EndMass:0.2Upper:0.25Lower:0.25EndMass:0.3Upper:0.25Lower:0.25EndMass:0.5Upper:0.25Lower:0.25EndMass:0.6Upper:0.25Lower:0.25EndMass:0.7Upper:0.25Lower:0.25EndMass:0.8Upper:0.25Lower:0.25EndMass:0.9Upper:0.25Lower:0.25EndMass:1

The data from these above cases represent only a fraction of the many simulations that have been performed. These many simulations have allowed for great insight on how the system responds and also better clarifies the effects of nonlinearities such as friction and torque saturation.

8

So far, the greatest concern seems to be torque saturation. It appears that torque saturation will be the chief factor in ascertaining the stability and robustness of the system. Simulink Diagrams and m-files can be found in the Appendix.

Section 2: Parameter ID

Friction: A VI was developed to automate the friction measurement of the system. The friction in the system was observed to be highly non-linear. The primary cause of this non-linearity appears to be non-uniform rubbing in the belt. The effects of this can be seen as oscillation in steady state velocity. To get the best possible data, averaging was incorporated in the VI.

Friction Test X Axis

0.06

0.065

0.07

0.075

0.08

0.085

0.09

0.095

0.1

-50 0 50 100 150 200 250 300 350 400 450

Theta Dot (degrees per second)

Torq

ue (N

-m)

9

Friction Test Y Axis

0.06

0.065

0.07

0.075

0.08

0.085

0.09

0.095

0.1

-50 0 50 100 150 200 250 300 350

Theta Dot (degrees per sec)

Torq

ue(N

-M)

The plots above show the preliminary friction data for each axis along with a linear trend line. Since the mechanical gear train is still undergoing modifications, the tests will have to be repeated. Even so, since the automated test program has already been created, retesting will only take a few minutes. .

10

Summary of progress:

Plan and schedule: The plan of action has been successfully implemented. The team is moving full steam ahead in the final parameter identification and control optimization. Currently the team is ahead of schedule with fabrication and controller optimization. Because time was spent on manufacturing, linearization of the hand calculations for the two dimensional model is yet to be completed. Besides this, the team is ahead of schedule.

Completed Tasks: • Solid Works models (see appendix for drawings) • Hand Calculations for equations of motion • Inertia calculations • Batch files for manufactured parameters for arm length and weight. • Manufacturing of the universal join with encoders • Friction identification with old system • Wiring of system. See appendix

To be done: • Friction Identification with new gears and belt • Counter weights to lower torque specifications • Linearization of hand calculations • Controller optimization • Manufacture the chosen arm lengths and weights.

Unanticipated events: The weight of the universal joint was too high so extra material will need to be milled out. Also to help fix this problem the gear sizes and belts in the system need to be changed to increase the torque. Torque saturation could be an issue so a counter weight will need to be used to balance out the weight of the universal joint. Another issue that needed to be resolved was the range of motion. Spacers were added to increase the space between the yoke cross beam and the yoke tilt shaft. Unfortunately, this change required a new yoke motor mount plate. Other than those few unanticipated events everything else has been moving along smoothly.

11

Further Plan of Action: In order to complete the project, the team will need to move ahead on picking the final lengths and weighs of the pendulum and implement the controller. The next step is optimization of the control and testing. See schedule in appendix.

Costs: So far there have been no major unexpected costs. Material has been mostly donated as well as the necessary encoders and motors for the project. The only additional expense will be the remanufacturing of the yoke motor mount plate, in addition to new belts for the system. See costs in appendix.

12

Bibliography: [1] CJC, CTM Example: Inverted Pendulum Modeling

http://www.engin.umich.edu/group/ctm/examples/pend/invpen.html (c) 1997 [2] Craig, Kevin Inverted Pendulum Systems: Rotary and Arm-Driven, A Mechatronic

System Design Case Study [3] Kajiwara, Hiroyuki, Wide Range Stabilization of an Arm-Driven Inverted Pendulum

Using Linear Parameter-Varying Techniques February 1998 [4] Kajita, Shuuji A Realtime Pattern Generator for Biped Walking IEEE Robotics &

Automations Conference May 2002

13

Appendix A: Terms Abbreviations: 2D = two dimensional 3D = three dimensional cRIO = Compact Remote Input Output or Compact RIO FPGA = Field Programmable Gate Array Device Parts:

14

Appendix B: Costs Parts: Part Number: Description Quantity Price Total Cost Our Cost

S2-2048-B US Digital Encoder 4 $ 85.55 $ 342.20 $ - GM8724S017 Pittman Motor 2 $ 210.00 $ 420.00 $ - MSC: 74924887 Flange Bearings 4 $ 7.44 $ 29.76 - Pan gear A 1 $ 9.97 $ 9.97 $ - Pan gear B 1 $ 22.02 $ 22.02 $ - Tilt gear A 1 $ 7.95 $ 7.95 $ - Tile gear B 1 $ 22.02 $ 22.02 $ - Pan belt 1 $ 3.92 $ 3.92 $ - Tilt belt 1 $ 4.00 $ 4.00 $ - Yoke Motor Mount Plate 1 $ 1.42 $ 1.42 $1.42 Parts Total $931.74 $ 1.42 Raw Materials: Part Number: Description Quantity Price Total Cost Our Cost

MSC: 32000820

Alloy 3003 Aluminum Material: Aluminum - Alloy 3003 Outside Diameter: 1/2 Wall Thickness: 0.035 In. 2 $9.66 $19.32 -

MSC: 02629335

Alloy 6061 Aluminum Material: Aluminum - Alloy 6061 Diameter: 1 Length: 12 1 $7.56 $7.56 -

MSC: 32012254

Alloy 6061 Aluminum Material: Aluminum - Alloy 6061 Length: 72 Length: 6 Thickness: 1/4 Width: 1 1 $12.38 $12.38 -

Raw Materials Total $39.26 -Labor Costs Name: Title Hours Cost Total Cost Our Cost Brian Lewis Engineer 135 $40 $5,400 $ - Teresa Bernardi Engineer 135 $40 $5,400 $ - Matthew Rosmarin Engineer 135 $40 $5,400 $ - Labor Costs Total $21,600 $ - Final Cost Our Final Cost $22,418.84 $ 1.42

15

Appendix C: SimMechanics Modeling

Figure 1: SimMechanics 2D System

Figure 2: Simulink 2D System

16

Figure 3: SimMechanics 3D System

17

Figure 4: Simulink 3D System

18

Appendix D: Schedule See next page.

19

ID Task Name Start Finish Resource Names1 Design Plan of Action Wed 2/8/06 Tue 4/18/06

2 System Modeling Wed 2/8/06 Tue 2/28/06 All

3 Sim Mechanics Wed 2/8/06 Mon 2/13/06 Matthew Rosmarin,Ter

4 Hand Calculations Thu 2/9/06 Tue 2/28/06 Brian Lewis

5 Linearize Sun 2/12/06 Mon 2/20/06 All

6 Sim Mechanics Sun 2/12/06 Sun 2/12/06 Matthew Rosmarin,Ter

7 Hand Calculations Sun 2/19/06 Mon 2/20/06 Brian Lewis

8 Control Design Mon 2/13/06 Mon 2/20/06

9 Sim Mechanics Mon 2/13/06 Mon 2/13/06 Matthew Rosmarin,Ter

10 Hand Calculations Sun 2/19/06 Mon 2/20/06 Brian Lewis

11 Parameter Identification Wed 2/15/06 Fri 2/24/06 All

12 Friction Wed 2/15/06 Tue 2/21/06 Brian Lewis,Matthew R

13 Evaluate Inertia: Mon 2/20/06 Fri 2/24/06 All

14 solid works Mon 2/20/06 Fri 2/24/06

15 experimental Mon 2/20/06 Fri 2/24/06

16 Backlash / Compliance Mon 2/20/06 Fri 2/24/06

17 Size Components by Difficulty: Thu 2/23/06 Tue 2/28/06 All

18 Easy Thu 2/23/06 Tue 2/28/06

19 Medium Thu 2/23/06 Tue 2/28/06

20 Hard Thu 2/23/06 Tue 2/28/06

21 Telescope System Thu 2/23/06 Tue 2/28/06

22 Construction and Fabrication Thu 2/23/06 Tue 4/18/06 All

23 Ordering Components Thu 2/23/06 Fri 2/24/06 Brian Lewis

24 Construction of Components Fri 3/3/06 Thu 3/30/06 Brian Lewis

25 Sensors and Universal Joint Fri 3/3/06 Thu 3/30/06 Brian Lewis

26 System Integration Tue 3/28/06 Wed 4/5/06

27 System Tuning Wed 4/5/06 Tue 4/18/06

28 Performance Analysis Thu 3/16/06 Tue 4/4/06 All

29 Control System Robustness Thu 3/16/06 Tue 4/4/06

30 Reports and Presentation Deadlines Wed 2/1/06 Wed 4/26/06 All

31 Concept Design Memo Wed 2/1/06 Wed 2/1/06

32 Project Proposal Sections Due Thu 2/16/06 Thu 2/16/06

33 Project Proposal Report Wed 2/22/06 Wed 2/22/06

34 Project Proposal Presentation Wed 2/22/06 Wed 2/22/06

35 Design Progress Presentation Wed 3/29/06 Wed 3/29/06

36 Design Progress Report Wed 3/29/06 Wed 3/29/06

37 Project Demonstration Wed 4/19/06 Wed 4/19/06

38 Final Presentation Wed 4/26/06 Wed 4/26/06

Matthew Rosmarin,Teresa Bernardi

Brian Lewis


Brian Lewis


Brian Lewis

Brian Lewis,Matthew Rosmarin

Brian Lewis

Brian Lewis

Brian Lewis

2/1

2/16

2/22

2/22

3/29

3/29

4/19

4/26

January 21 February February Februar March 1 March 11 March 21 April 1 April 11 April 21 May 1

Task

Split

Progress

Milestone

Summary

Project Summary

External Tasks

External Milestone

Deadline

Page 1

Project: scheduleDate: Wed 3/29/06

Appendix F: Mechanical Drawings

See next page

21

SHEET 1 OF 6

3/28/06BL

UNLESS OTHERWISE SPECIFIED:

SCALE: 1:1 WEIGHT:

REVDWG. NO.

ASIZE

TITLE: High Resolution Encoder

NAME DATE

COMMENTS:

Q.A.

MFG APPR.

ENG APPR.

CHECKED

DRAWN

FINISH

MATERIAL

INTERPRET GEOMETRICTOLERANCING PER:

DIMENSIONS ARE IN INCHESTOLERANCES:FRACTIONALANGULAR: MACH BEND TWO PLACE DECIMAL THREE PLACE DECIMAL

5 4 3 2 1

DO NOT SCALE DRAWING SHEET 1 OF 6


SCALE: 1:2 WEIGHT:

REVDWG. NO.

ASIZE

TITLE:

NAME DATE

COMMENTS:

Q.A.

MFG APPR.

ENG APPR.

CHECKED

DRAWN

FINISH

MATERIAL



5 4 3 2 1

0.50

<MO

D-D

IAM

>0.3

8

0.25

<MO

D-D

IAM

>0.2

5

0.50

<MOD-DIAM>2.20

0.43

2.43

SHEET 2 OF 6

3/28/06BL


SCALE: 1:1 WEIGHT:

REVDWG. NO.

ASIZE

TITLE: Actuated Arm Clamp

NAME DATE

COMMENTS:

Q.A.

MFG APPR.

ENG APPR.

CHECKED

DRAWN

AluminumFINISH

MATERIAL



5 4 3 2 1

2.00

<MO

D-D

IAM

>0.5

0

0.25

0.80

0.34

R0.43

<MO

D-D

IAM

>0.3

8

Balance Arm Holder

SHEET 3 OF 6

3/28/06BL


SCALE: 1:1 WEIGHT:

REVDWG. NO.

ASIZE

TITLE:

NAME DATE

COMMENTS:

Q.A.

MFG APPR.

ENG APPR.

CHECKED

DRAWN

FINISH

MATERIAL: Aluminum



5 4 3 2 1

0.25

0.25

0.40

<MO

D-D

IAM

>0.5

0

1.00

1.20

0.25

0.40

<MOD-DIAM>0.25

0.25

0.65

Balance Arm Encoder Mount

SHEET 4 OF 6

3/28/06BL


SCALE: 1:1 WEIGHT:

REVDWG. NO.

ASIZE

TITLE:

NAME DATE

COMMENTS:

Q.A.

MFG APPR.

ENG APPR.

CHECKED

DRAWN

FINISH

MATERIAL: Aluminum



5 4 3 2 1

1.00

3.00

<MO

D-D

IAM

>0.3

8

0.50

0.50 0.50

1.00

0.80

0.25

0.50

<MO

D-D

IAM

>0.2

5

UJOINT

SHEET 5 OF 6

3/28/06BL


SCALE: 1:1 WEIGHT:

REVDWG. NO.

ASIZE

TITLE:

NAME DATE

COMMENTS:

Q.A.

MFG APPR.

ENG APPR.

CHECKED

DRAWN

aluminumFINISH

MATERIAL



5 4 3 2 1

1.00

<MO

D-D

IAM

>0.3

8

0.50

<MO

D-D

IAM

>0.2

5

0.25

2.50

1.00

0.25

0.50

2.000.25<M

OD

-DIA

M>

0.05

SHEET 6 OF 6

3/28/06BL


SCALE: 1:1 WEIGHT:

REVDWG. NO.

ASIZE

TITLE: Main Universal Joint

NAME DATE

COMMENTS:

Q.A.

MFG APPR.

ENG APPR.

CHECKED

DRAWN

FINISH

MATERIAL



5 4 3 2 1

ITEMNO. PART NUMBER Number

1 UJOINT 12 Balance Arm Encoder

Mount 1

3 High ResolutionEncoder 2

4 dowl 15 Blance Arm Holder 16 Balance Weight 17 Balance Arm 18 Actuated Arm 19 Actuated Arm Clamp 1

10 Weight 1

7

10

3

5

1

8

9

6

2

28

Appendix G: Wiring Chart Wiring Chart Definitions: EncMX ChA Motor Encoder X direction Channel A EncMX ChB Moter Encoder X direction Channel B EncMY ChA Moter Encoder Y direction Channel A EncMY ChB Moter Encoder Y direction Channel B EncAX ChA Actuated Arm Encoder X direction Channel A EncAX ChB Actuated Arm Encoder X direction Channel B EncAY ChA Actuated Arm Encoder Y direction Channel A EncAY ChB Actuated Arm Encoder Y direction Channel B EncBX ChA Balanced Arm Encoder X direction Channel A EncBX ChB Balanced Arm Encoder X direction Channel B EncBY ChA Balanced Arm Encoder Y direction Channel A EncBY ChB Balanced Arm Encoder Y direction Channel B CAT5 Wire Assignments: Orange NC Orange/White NC

Blue Motor Encoder Channel A

Blue/White Moter Encoder Channel B

Green Actuated Arm Encoder direction Channel A Green/White Actuated Arm Encoder direction Channel B Brown Power Brown/White GND Pin Assignments: cRIO 9401 Pin: Assignment: DI00 14 EncMX ChA DI01 16 EncMX ChB DI02 17 EncMY ChA DI03 19 EncMY ChB DI04 20 EncAX ChA DI05 22 EncAX ChB DI06 23 EnCAY ChA DI07 25 EnCAY ChB cRIO 9411 DI0A 1 EncBX ChA DI0B 9 GND DI1A 2 EncBX ChB DI1B 10 GND

DI2A 3 EncBY ChA DI2B 11 GND DI2A 6 EncBY ChB DI2B 13 GND

29

Appendix H: Batch File Code % Sim Mechanics Simulation Batch Setup for 2D Balancing Pendulum % Mar 21, 2006 M.R. & T.B. clc clear close all tic % Varying parameters for the batch experimentation % All other parameters should remain constant ria=.00317; % Inner diameter of actuated arm roa=.0032; % Outer diameter of actuated arm rib=.00317; % Inner diameter of balanced arm rob=.0032; % Outer diameter of actuated arm % Define the desired lengths of the actuated and balanced arms minAA=250; % Minimum length of actuated arm(in mm) maxAA=250; % Maximum length of actuated arm (in mm) minBA=250; % Minimum length of balanced arm (in mm) maxBA=250; % Maximum length of balanced arm (in mm) interval=100; %Interval length in mm % Specify density per unit length of arms density_act=0; % Density for actuated arm density_bal=0; % Density for balanced arm % Specify End Mass Properties minEM=0; %minimum desired mass(g) maxEM=300; %maximum desired mass (g) minterval=100;%interval length in g %Actuator Actuator.Mass=0; %kg Actuator.Inertia= eye(3)*0.002;%kg*m^2

30

%Damping and Stiction UpperJoint.Damping=.1; LowerJoint.Damping=.1; %Inital Conditions BalanceArm.Angle=1.0; %deg BalanceArm.Velocity=0; %deg/s DrivenArm.Angle=0; %deg DrivenArm.Velocity=0; %deg/s Motor.TorqueSaturation=0.5; %N-m %begin batch code run=0; numerror.uncontrollable=0; numerror.unstablenonlinearity=0; %Batch for Endmass for BE = minEM:minterval:maxEM %DrivenArm.Length=X:Y EndMass.Mass=BE/1000; % EndMass.Inertia=eye(3)/EndMass.Mass*.00001; %Define inertia tensor height_endmass=.03; radius_endmass=.03; EndMass.Inertia=[0.5*EndMass.Mass*radius_endmass^2 0 0;0 EndMass.Mass*(3*radius_endmass^2+height_endmass^2)/12 0;0 0 EndMass.Mass*(3*radius_endmass^2+height_endmass^2)/12]; %Define inertia tensor %Batch for Actuated Arm for BAA = minAA:interval:maxAA %DrivenArm.Length=X:Y DrivenArm.Length=BAA/1000; DrivenArm.Mass=DrivenArm.Length*density_act; %kg % DrivenArm.Inertia= [0.00001 0 0;0 0.00001 0; 0 0 0.00001];%kg*m^2 DrivenArm.Inertia= 0.5*density_act*pi*[roa^4-ria^43 0 0;0 (roa^4-ria^4)*0.5+DrivenArm.Length^2*(roa^2-ria^2)/6 0; 0 0 0.5*(roa^4-ria^4)+DrivenArm.Length^2*(roa^2-ria^2)/6];%kg*m^2

31

%Batch for Balanced Arm for BBA=minBA:interval:maxBA BalanceArm.Length=BBA/1000; BalanceArm.Mass=BalanceArm.Length*density_bal; %kg % BalanceArm.Inertia= [0.00001 0 0;0 0.00001 0; 0 0 0.00001];%kg*m^2 BalanceArm.Inertia= density_bal*pi*0.5*[rob^4-rib^4 0 0;0 0.5*(rob^4-rib^4)+(BalanceArm.Length^2*(rob^2-rib^2))/6 0; 0 0 0.5*(rob^4-rib^4)+BalanceArm.Length^2*(rob^2-rib^2)/6];%kg*m^2 %Control Design uncontrolable=0; try [A B C D] = linmod('Plant2DBatch'); %Linearize System SS = ss(A,B,C,D); Q=[100 0 0 0 0 100 0 0 0 0 100 0 0 0 0 100]; [K,S,e] = lqr(A,B,Q,1) ;%Create Controller Kc=1; valid=0; sim('HighLevel2DBatch'); catch valid=0; numerror.uncontrollable=numerror.uncontrollable+1; uncontrolable =1; end if (valid) run=run+1; data(run).DrivenArmLength=DrivenArm.Length; data(run).BalanceArmLength=BalanceArm.Length; data(run).Endmass=EndMass.Mass; data(run).UpperJointPosition=yout(:,1) data(run).LowerJointPosition=yout(:,2) data(run).time=tout; else if(uncontrolable==0) numerror.unstablenonlinearity=numerror.unstablenonlinearity+1; end

32

end end end end figure(1) hold all for x=1:run plot(data(x).time,data(x).UpperJointPosition) plottitle(x)={strcat('Upper: ',num2str(data(x).BalanceArmLength),' ','Lower: ',num2str(data(x).DrivenArmLength),'EndMass:',num2str(data(x).Endmass))}; end try legend(plottitle); end title('Balancing Arm Angle (Deg) (Refrence to Actuator Arm)'); xlabel('Time (sec)'); ylabel('Displacement (Deg)'); hold off figure(2) hold all for x=1:run plot(data(x).time,data(x).LowerJointPosition) plottitle(x)={strcat('Upper: ',num2str(data(x).BalanceArmLength),' ','Lower: ',num2str(data(x).DrivenArmLength),'EndMass:',num2str(data(x).Endmass))}; end try legend(plottitle); end title('Actuator Arm Angle (Deg) (Refrence to Ground)'); xlabel('Time (sec)'); ylabel('Displacement (Deg)'); hold off clc disp('Stats');

33

disp('Number of succesful runs'); disp(run); disp('Number of Uncontrolable configurations'); disp(numerror.uncontrollable); disp('Number of Unstable configurations'); disp(numerror.unstablenonlinearity); disp('Run Time (s)'); disp(toc);

34

three dimensional driven-arm inverted pendulum

Documents