chip updates: making the puppy walk, determining if the CoG is within the foot positions [updates]
So we have a new technique for determining if the CoG is within the global xz foot positions translated with respect to the IMU coordinate frame.
According to: https://math.stackexchange.com/questions/51326/determining-if-an-arbitrary-point-lies-inside-a-triangle-defined-by-three-points we can use the barycentric weights of the triangle.
Here is the barycentric weight method in code form. Hopefully this will solve the problem we were having of the CoG never being within the triangle. I think the Area approach we were using was valid but since we weren't using integers the areas were never equaling. There were roundoff errors and etc. That's what makes the most sense to me. This method should be better/more robust that way.
We've also written the methods that take weight off the foot, lift the foot, step the foot, and then execute that for all four feet.
Let's see if it works. Shall we?
So that didn't work. Let's try this.
Okay that didn't work either meaning something's wrong somewhere else.
FOUND IT: it's the values we are passing in -- oops we forgot we're in the XZ plane. I think there's an issue in transferring coordinates as well maybe.
The global foot positions are clearly wrong. I don't know where the issue is coming from but we're going to fix it tomorrow. Maybe edit the whole thing so the CoG node publishes global foot positions and etc instead of just the CoG.