how to code a servo motor?

[madMAx4]

hi,
i gonna code a servo motor which is attached to a btGeneric6DofConstraint which is limited to angular movement only.

the idea is to set a max. torque the motor can apply.
every simulation loop i want to calculate a point. this point describes whether to apply max. torque to supply the current movement or to apply it against to stop perfectly at a given angle _without_ overshooting.
this is a kind of PD controller i think.

now i have to calculate the current velocity at the constraint which is needed to determine the above mentioned point.
are there already any bullet solutions to that?

in the meantime i try to code a solution which uses the local angular velocities of the two bodys in combination with the pivot points.

thx in advance.

[madMAx4]

after tens of hours it's working now perfectly.
but theres one problem with getting out the real torque which has been applied:

Code: Select all

real_torque = applied_torque_by_the_servo + external_torque

external_torque can be gravity or water resistance if you want

real_torque is calculated from the angular velocity at the joint at which the servo is attached.
i use this method for real_torque: every loop the velocity is pushed into a LIFO. and every loop i substract the latest velocity by the oldest in the LIFO.
so

Code: Select all

real_torque = ( delta_velocity * inertia_of_bodyB ) / delta_time

but the calculation isnt correct. to simplify the whole thing i turned off gravity, so real_torque should be the same as applied_torque_by_the_servo. but it isnt at the beginning and i dont know why.

EDIT: i just found out that this happens only when the center of mass is not at the axis of rotation, but i appyied the "parallel axes rule" everywhere in my code... but looking at the output plots it has something in common with the linear velocity caused by the center of mass displacment.

in the picture you can see the applied_torque_by_the_servo in blue and real_torque in yellow (velocity is green and error is red):

as you can see real_torque (yellow) is going down for no reason, the applied_torque_by_the_servo (blue) is constant and external_torque is zero.

i want to use real_torque to detect and compensate external_torque.

i'm stuck at this point. remember i'm only a 18 year old student
thx for any help.