Maths issue and servo direction calculation.

[dr pepper]

I have C code controlling a servo which gets its position command over serial.

It works well, 'cept for one thing, the position goes from 0 - 1440, if I'm on position 1440 and the servo gets a position 0 command, it wants to go forwards all the way around to 1440 instead of back 1.
My code runs other stuff so it needs to be non blocking, I can make it work if I take a value and then force the servo to that position ignoring all other commands meantime, but I want the servo to try and go to the latest position command, ie if the servo is still moving itself to a position and it gets a new position command I want it to abandon the last and go towards the latest command.

 

[MaxHeadRoom78]

Are you obtaining the position from a 360 pulse encoder, 360 x4 =1440?
It sounds as though you need some direction calculation based on the degree of command, is the maximum move 1440?
Max.

 

[dr pepper]

Yes 360 x 4, but the servo gets zero from a optical switch and then keeps a count based on steps.

The max move is 1440 yes.

I'm thinking of adding a piece of code that detects if the move is more that 720 (half a turn), and if it is then jump out and go the other direction.

 

[jpanhalt]

Or, add a flag bit to tell direction. I can foresee problems with using position 720 as the deciding element.

John

 

[MaxHeadRoom78]

So once it is zeroed can it not position to absolute position, if 1440 is maximum, then if a lesser position value is commanded compared to the current position then it would command a move in the opposite direction?
This is opposite to the shortest distance programming where it would pass through 1440 in order to make a shorter move to a low value position.
Max.

 

[Pommie]

Why does it want to go forward? If your code is written correctly then it should go backward.

Mike.

 

[dr pepper]

If (requiredposition != actualposition){
if (requiredposition > actualposition) forwardastep;
else backastep;
}

Is what I use to command direction, it does so one step at a time, as the code runs periodically.
The issue is if I was on 1440 and was commanded 0, because 0 is less than 1440 the servo would step all the way around to 1440 forwards instead of one backwards.

I cant think of another way to do it without calcing if the move is more than 1/2 a turn away and reverse direction.

 

[Les Jones]

I am assuming that this is a system that has continues rotation (Like an antenna position rotator but without the problem of winding the cable round the mast.)
I think your idea of testing if the magnitude of the error is greater than 720 and then reversing the sense of the error signal is the way to go. (This is the suggestion I was going to make after thinking about it overnight.) If it is a proportional servo (As opposed to one that just has a fixed speed.) then ther is another problem. When near the zero position the magnitude of the error will be large even though there is only a small distance to move. So as well as reversing the sense of the error signal you will also need to subtract the magnitude of the error from 1440. I had this problem in the early 1970s with an antenna control system (In hard wired logic.) for following the amateur satellites but I can't remember how I solved it then.

Les.

 

[dr pepper]

No thankfully theres no PID in this system, the motor is a geared stepper and it runs at a speed where it can start, run and stop at a constant speed.

Yes it has contiuous rotation, I wouldnt use a opto switch if there was a stop, I'dve just driven the motor into it and cleared the position counter.

OK then I'll think about the 1/2 rotation thing today then while I'm pulling cables in.

 

[Les Jones]

I didn't have the detect zero problem as I had made an absolute position encoder. (8 bit gray scale giving 256 positions.)
Les.

 

[MaxHeadRoom78]

This is virtually the same situation faced in automatic multi tool changers on CNC machines, where the system is programmed to carry out the most efficient move when changing tools, If a change from high tool # to low tool # is required the rotation will be through zero instead of reversing the whole way around.
A simple mathematical calc is done to perform it.
Max.

 

[dr pepper]

I'm familiar with fanuc 800b's, and heidenhain's.
But I never went into the code that deeply, past G code was black box stuff.

 

[MaxHeadRoom78]

Tool changer logic in Ladder can be 'Fun'!
Max.

 

[Pommie]

Ah, I see the problem now. How about,
if(abs(required-present)<abs(present-required)) Forward;
else backward;

Mike.

 

[Les Jones]

Hi Mike,
Wouldn't the abs value of both expressions always be the same ? I suspect you meant if abs(error) < abs(1440 -abs(error))
Les.

 

[Pommie]

Les,
You are of course correct. I'll have to rethink that.

Mike.

 

[Tony Stewart]

Output is direction and number of steps. The move from A to B (B-A) is the magnitude and polarity for direction. If the magnitude is greater than 719, then get sign then absolute value ( direction and #steps) then subtract from 1440 and toggle #sign bit and move in opposite direction.

If there are 1440 positions are they 0-1439 (preferred) or upto 1440 as you suggest meaning 1 is the 1st position. Is the optical detector undirectional? meaning only on CCW or CW, is it accurate.?

e.g let's assume datum is position 0 and it is accurate in both directions.
then if A is 1000 and B is 1
B-A = -999 ( backwards long way)
calculation is #dir= -1 #steps= 999
then subtract from 1440 gives #steps= 441 and toggle direction, #dir= 1

e.g. if A is 1 and B is 1439 then B-A= 1438 or #dir =1 & #steps= 1438
then subtract from 1440 gives #steps = 2 and #dir = -1

You can also choose ccw and cw direction assignments for #dir to be 0,1 and exclusive OR to toggle.

But Datum or Home ( position 0) accuracy is another issue as well as your 1441 positions ??

 

[dr pepper]

This is what I came up with, its a bit clunky but I'll try it later and see what happens.

I should point out that I used unsigned int for req and actual position variables, but error is a signed int.

error = requiredposition - actualposition

If (requiredposition != actualposition){
if (requiredposition > actualposition) {
if (abs(error) < 1440 - abs(error)) forwardastep;
else backastep;
}
if (requiredposition < actualposition){
if (abs(error) < 1440 - abs(error)) backastep;
else forwardastep;
}
}

And of course I also need to mod the forward and back a step functions so that they loop around at zero -1 and 1440 + 1.

 

[Tony Stewart]

You didn't check if the abs(error) is > 1440/2 which determines if your error polarity is the shortest direction to go. If <719, then reverse direction and subtract abs(difference) from 1440

Is 1339 your last position before 0?
Is the home sensor accurate in both directions or only one?