} while (Pole->w == 0); } void SimulatePole(POLE* Pole) { INT s; REAL x, xDot, xDotDot; REAL w, wDot, wDotDot; REAL F; x = Pole->x; xDot = Pole->xDot; w = (Pole->w / 180) * PI; wDot = (Pole->wDot / 180) * PI; F = Pole->F; for (s=0; s<STEPS; s++) { wDotDot = (G*sin(w) + cos(w) * ((-F Mp*L*sqr(wDot)*sin(w)) / (Mc+Mp))) / (L * ((REAL) 4/3 - (Mp*sqr(cos(w))) / (Mc+Mp))); xDotDot = (F + Mp*L * (sqr(wDot)*sin(w) wDotDot*cos(w))) / (Mc+Mp); x xDot w wDot
+= += += +=
} Pole->x Pole->xDot Pole->w Pole->wDot
(T (T (T (T = = = =
/ / / /
STEPS) STEPS) STEPS) STEPS)
* * * *
xDot; xDotDot; wDot; wDotDot;
x; xDot; (w / PI) * 180; (wDot / PI) * 180;
} BOOL PoleStillBalanced(POLE* Pole) {
197