1 /*
2 * IPModel.java
3 *
4 * Created on March 31, 2004, 12:08 PM
5 *
6 * Copyright 2004 Andrew Kaluzniacki
7 * All rights reserved.
8 * http://drewk.net
9 */
10
11
12 /**
13 * An IPModel object represents the dynamics of an inverted pendulum attached to a
14 * sliding block.
15 * @author Andrew Kaluzniacki
16 */
17 public final class IPModel {
18
19 /** Creates a new instance of IPModel */
20 public IPModel() {
21 this.reset();
22 }
23
24 /**
25 * propogate the state of the system through a small time h
26 *
27 * Runge Kutta Notes - taken from Mathematical Methods of Physics - 2nd ed Mathews + Walker
28 *
29 * dy/dx = f(x,y) - this is the equation we know, what we want to find is y(x)
30 *
31 * Suppose y = y0 at x = x0, we choose a small interval h and calculate
32 *
33 * k1 = h * f(x0, y0)
34 * k2 = h * f(x0 + h/2, y0 + k1/2)
35 * k3 = h * f(x0 + h/2, y0 + k2/2)
36 * k4 = h * f(x0 + h, y0 + k3)
37 *
38 * Then at x = x0 + h , y (=) y0 + 1/6(k1 + 2 * k2 + 2 * k3 + k4)
39 *
40 *
41 * For several coupled differential equations it is a little messier
42 *
43 * We have to do this for four first order diff eqs for x,phi,v,w.
44 *
45 * Note that our diffeq are not actually explicitly dependent on t - since we take Fx and Fphi to be constant in our interval h.
46 * But I include t in the calls I make for clarity.
47 */
48 private void stepRK(double h) {
49
50 double v = dx; // we call v dx
51 double w = dphi;
52 double t = time;
53
54 // calculate all the new values - using old values
55 double k1a = h * eqDx(t, x, phi, v, w);
56 double k1b = h * eqDphi(t, x, phi, v, w);
57 double k1c = h * eqDv(t, x, phi, v, w);
58 double k1d = h * eqDw(t, x, phi, v, w);
59
60 double k2a = h * eqDx(t + h/2.0, x + k1a/2.0, phi + k1b/2.0, v + k1c/2.0, w + k1d/2.0);
61 double k2b = h * eqDphi(t + h/2.0, x + k1a/2.0, phi + k1b/2.0, v + k1c/2.0, w + k1d/2.0);
62 double k2c = h * eqDv(t + h/2.0, x + k1a/2.0, phi + k1b/2.0, v + k1c/2.0, w + k1d/2.0);
63 double k2d = h * eqDw(t + h/2.0, x + k1a/2.0, phi + k1b/2.0, v + k1c/2.0, w + k1d/2.0);
64
65
66 double k3a = h * eqDx(t + h/2.0, x + k2a/2.0, phi + k2b/2.0, v + k2c/2.0, w + k2d/2.0);
67 double k3b = h * eqDphi(t + h/2.0, x + k2a/2.0, phi + k2b/2.0, v + k2c/2.0, w + k2d/2.0);
68 double k3c = h * eqDv(t + h/2.0, x + k2a/2.0, phi + k2b/2.0, v + k2c/2.0, w + k2d/2.0);
69 double k3d = h * eqDw(t + h/2.0, x + k2a/2.0, phi + k2b/2.0, v + k2c/2.0, w + k2d/2.0);
70
71 double k4a = h * eqDx(t + h, x + k3a, phi + k3b, v + k3c, w + k3d);
72 double k4b = h * eqDphi(t + h, x + k3a, phi + k3b, v + k3c, w + k3d);
73 double k4c = h * eqDv(t + h, x + k3a, phi + k3b, v + k3c, w + k3d);
74 double k4d = h * eqDw(t + h, x + k3a, phi + k3b, v + k3c, w + k3d);
75
76 double newX = x + (k1a + 2.0 * k2a + 2.0 * k3a + k4a)/6.0;
77 double newPhi = phi + (k1b + 2.0 * k2b + 2.0 * k3b + k4b)/6.0;
78 double newV = v + (k1c + 2.0 * k2c + 2.0 * k3c + k4c)/6.0;
79 double newW = w + (k1d + 2.0 * k2d + 2.0 * k3d + k4d)/6.0;
80
81 // now update the old values with the new
82 time = t + h;
83 x = newX;
84 phi = newPhi;
85 dx = newV;
86 dphi = newW;
87 }
88
89
90 private double eqDx(double t, double x, double phi, double v, double w) {
91 return v; // x" = v
92 }
93
94 private double eqDphi(double t, double x, double phi, double v, double w) {
95 return w; // phi" = w
96 }
97
98 private double eqDv(double t, double x, double phi, double v, double w) {
99
100 double n = 2*(l*Fx - Math.cos(phi) * Fphi + l * Math.sin(phi)* ( g * m1 * Math.cos(phi) + l * m2 * w * w));
101
102 double d = l * (2*m1 + m2 - m2 * Math.cos(2*phi));
103
104 return n/d; // v"
105 }
106
107 private double eqDw(double t, double x, double phi, double v, double w) {
108
109 double n = 2*l*m2*Math.cos(phi)*Fx + 2*(m1+m2)*(-Fphi + g*l*m1*Math.sin(phi)) + l*m2*m2*Math.sin(2*phi)*w*w;
110
111 double d = l*l*m2*(-2*m1-m2+m2*Math.cos(2*phi));
112
113 return n/d; // w"
114 }
115
116 /**
117 * propagate the model state through deltaTime using the Runge-Kutta method.
118 */
119 public void update(double deltaTime) {
120
121 int n = 100; // break deltaTime into even smaller time steps.
122 for (int i = 0; i < n; i++) {
123 stepRK(deltaTime/n);
124 }
125
126 };
127
128 /**
129 * reset the state of the model to an initial state
130 */
131 public void reset() {
132 m1 = 1.0;
133 x = 0;
134 dx = 0;
135
136 Fx = 0;
137 Fphi = 0;
138 m2 = 1.0;
139 l = 1;
140 phi = Math.PI -0.01; // almost verticle
141 dphi = 0;
142
143 time = 0;
144 }
145
146 /**
147 * return a string representing the state of the model
148 */
149 public String toString() {
150 java.text.DecimalFormat nf = new java.text.DecimalFormat();
151
152 nf.setMaximumFractionDigits(2);
153 nf.setMinimumFractionDigits(2);
154 nf.setPositivePrefix("+");
155
156 return "model: phi=" + nf.format(phi)
157 + " dphi=" + nf.format(dphi)
158 + " x=" + nf.format(x)
159 + " dx=" + nf.format(dx)
160 + " time:" + nf.format(time);
161 }
162
163 /**
164 * return the current value of x
165 */
166 public double getX() {
167 return x;
168 }
169
170 /**
171 * return the current value of phi
172 */
173 public double getPhi() {
174 return phi;
175 }
176
177 /**
178 * return the current value of l
179 */
180 public double getL() {
181 return l;
182 }
183
184 // the internal state of the model is private to the model
185 // reset() sets these values.
186
187 private double m1 = 0; // mass of block
188 private double x = 0; // the position of the block mass (pendulum pivot) - increases to the right
189 private double dx = 0; // first time derivative of x
190
191 private double Fx = 0; // the external force applied to the block
192 private double Fphi = 0; // the external force applied to the pendulum
193
194 private double m2 = 0; // mass of pendulum weight
195 private double l = 0; // length of pendulum
196 private double phi = 0; // angle of the pendulum - verticle down is 0, increasing counter clockwise
197 private double dphi = 0; // first time derivative of phi
198
199 private double time = 0; // time elapsed in simulation
200
201 private final double g = 9.8; // gravity is final
202
203
204 /**
205 * main() serves as a simple demo of IPModel and runs the model through 10 seconds of
206 * time outputing the state to the console
207 * In typical use of IPModel, main() would not be called
208 * @param args the command line arguments - these are ignored
209 */
210 public static void main(String[] args) {
211 System.out.println("Inverted Pendulum Model - run for 10 seconds..\n");
212
213 IPModel model = new IPModel();
214
215 // run for 10 seconds.
216 for (double t = 0.0; t <= 10.0; t = t + 0.1) {
217 System.out.println(model);
218 model.update(0.1);
219 }
220 }
221 }
222