arrays of arrays using shift registers

[SaraBaber]

Hello

I am using inverse kinematics for 5 DOF!

Right now its working fine if I want to feed x,y,z positions and move my robot to that angles as given my IK!Thisis being done in the false loop as in my code that is attached! the other part is just adding the joint limits at the right side!

And my true loop is just for the compensation for motor slippage when i run it actually!

what the code is dooing right now is when il give x,y,z positions and press small stop button it will pass values to the motors, to move it back to initial position i will press the bigger stop button and then smaller again so it will return to its initial position!

What i want to do is

to move it from one position to another, keeping in view if the new position that is P2-P1 is within my range of joint limits or not!

So for that I am taking the difference of the previous degree angles and the current that is fed, and then extracting the elements of the difference array to feed further to motors by checking if that is in our limits or not!

But I donot know why this is not working! below are the parameter that I am setting for my IK

three working points are:

1. 0.10101010----x

     0.10101010---y

     0.101010101----z

2. 0.606060606-----x

    0.343434343------y

     0.1010101010-----z

3. 0.606060606-----x

    -0.606060606-----y

    0.141414141------z

So if we take difference of angles of P2-P1, they should be: 251, 13.9, 0, 0, 94.8 approx, I donot know If my logic is right or wrong or I have to add something, but the values are constantly flickering! but i am unable to do it!please help  me in making this logic

thanks

Sara

[RavensFan]

What does your question have to do with the subject of your message?  I don't see where you are using "arrays of arrays using shift registers".

[SaraBaber]

Yes I am sorry for that! it should be arrays using shift registers!

[SaraBaber]

I need to further clarify the situation a little bit that, it should work in a way that all three points are given at once, so we donot have to enter second point when reaches first and then third when it reaches second!

A way maybe in the form of table that we can enter our values of all three points at once and it picks up one after another!

Is that possible?

[apok]

your code made me dizzy...when i see lots of code repeating the same algorithm, i think array and for loop...i am sure something can be done with the in range and select functions. i am just too quizzee to do so

Spoiler