Anything after the forever block won’t run. Have it in a separate thread. The logic looks fairly fine on first glance. Make sure you tune your constants properly when you get to that too, otherwise it won’t work well.
1 Like
Make a function block for your pid movements and call them in your auto loop
1 Like
What was the physical result?
Did you try using lower values for targetPosition?
1 Like
You calculate all the variables in the forever loop, but none of the outputs are ever passed into the motors. The math looks correct, just reading over it, though. I believe all you would have to do is move the constants out of the forever loop (as they only need to be defined once), and bring the blocks that control the motors into the forever loop so their speeds are actually updated.
Also, as @Penguin1 said, you’d definitely want to make this a function so you can call it multiple times. Hope this helps!
1 Like