Modeling in Acumen

The mathematical model is derived using Euler-Lagrange equation. The following equation is found:

  angle''=  (g * sin(angle)  - pos'' * cos(angle)  )/len;

The differential equation describe how  the angle changes when affected by gravity and the acceleration of the structure the inverted pendulum is fastened on.

 

For deriving the control parameters the equation is simplified: angle = 0 is assumed.

 

Then by using the Laplace transform and control theory. The poles of the system can be analysed. When the poles is placed on the left hand side of the imaginary axis the system is stabilized. 

/**

 * Virtual prototype of Inverted pendulum controller

 *

 * Authour: 

 * Viktor Frimodig

 * Martin Gustavsson

 * 

 * 2014-10-25: Created

 * 2014-10-30: Comments added

 */ 

 

class Pendulum(theta)

 private

  angle :=theta;angle' := 0; angle'' := 0; 

  len := 0.5;

  x:=0;

  g := 9.8;

  pos:=0;pos':=0;pos'':=-0;

  K1:=50; K2:=5;

  _3D := []

 end

  _3D =  

  [["Cylinder",[0,pos+sin(angle),cos(angle)],[0.05,2],yellow,[-angle+pi/2,0,0]],

  ["Sphere",[0,pos+2*sin(angle),2*cos(angle)],0.15,yellow,[0,0,0]],

  ["Box",[0,pos,-0.25],[0.5,0.5,0.5],red,[0,0,0]]];

  angle''=

  (g * sin(angle) 

  + len * x - pos'' 

  * cos(angle)

  )/len;

end

 

class Controller(theta, jitter, resolution, p, d)

// Jitter = the time between two samples

 private

  angle := theta; angle':= 0;

  angle_sample := theta; angle_dt := 0;

  k1 := p;

  k2 := d;

  acceleration :=0;

  t :=0;t':=1;

 end

   acceleration := k1 * angle  + k2 * angle';

end

 

/**

 * Potentiometer

 * 

 * theta = initial displacment of angle

 * 

 * resoulution = the resolution the ADC gives 

 * ex: for 100 values on one turn, 360 degrees (2 Pi radians) ->  resolution = 2*pi/100  

 * 

 */

 

class Mediator(angle)

 private

  pend := create Pendulum(angle);

  ctrl := create Controller(angle,0.05,2*pi*10/(1024),24,4);

 // pot := create Potentiometer(pi/20,2*pi*10/1024/*0.05*/);

 end

  //pot.angle = pend.angle;

  ctrl.angle= pend.angle;

  ctrl.angle' = pend.angle';

  //ctrl.angle_sample = pot.angle_sample;

  pend.pos'' = ctrl.acceleration;

end

 

class Main(simulator)

 private

  mode := "init";

 end

  switch mode

   case "init"

    simulator.endTime  := 10;

    simulator.timeStep := 0.01;

    create Mediator(pi/20);      

    mode := "wait";

   case "wait"

  end

end