Collision fixing

[JosephBye]

Collision detection added, ready for merge

[JosephBye]

for merge, keep all constants from main

[JosephBye]

Also, Alexis, if you're reading this I just realized that the home command sometimes almost swings the elbow around 360 degrees instead of directly going back, i.e. if the target angle is 0 and the robot is at 330 degrees, it'll spin 330 degrees instead of 30.

[allengregoryiv]

for merge, keep all constants from main

You need to do this on your branch when you merge main into your branch before doing the PR. Don't make PRs that can't be just directly merged to the best of your ability.

[JosephBye]

Fixed 👍

[JosephBye]

@cycIes elbow collision added/fixed

[cycIes]

For this part, I'm assuming you're saying the elbow can move if it's just trying to go higher? Would you have to account for negative angles as well? (the 0 for the elbow is pointing up)
edited: oops i meant for this to be a simple comment

[cycIes]

Also I realize now that checking elbow against elevator position is tricky because depending on where the shoulder is, the elbow vertical displacement is different

[JosephBye]

I gave weights to elbows and shoulders based on real length to get a percent of total height, then using that to check for elevator.
Elbow constraints off of Elevator being high enough in comparison to shoulder+elbow, right now we're assuming a 1 to 1 ratio between elevator and shoulder+elbow, which means that for basically every percent of max height I allow elevator to go up, shoulder+elbow's total position is able to increase by that percent. This can be changed in future for optimization.