chip updates: step away! [updates]
Yes! Ya read that correctly. It's time to get this thing to take its first few steps. In reality, what we're going to do is (1) figure out a way to handles trajectories, generate a step trajectory, and see if we can get one leg to follow it now that we've got the inverse kinematics working. (2) it might also be wroth writing a forwards kinematics function that guesstimates to where the leg currently is. Let's start with number 2, and then get to 1.
That's forward kinematics. Pretty easy to determine where the leg position is now with the simplified model and looking at the diagram. This method returns an Array with the X, Y, Z position of the foot.
Now comes the harder part. How to handle a trajectory sent to the leg. And what we'll do is create a public variable trajectory, which is an ArrayList, and add values to it while the leg is parsing through them.
Now if we want to add points to the leg's motion path. All we will do is add them to the public list trajectory. Then the leg will move this trajectory by TRAVERSING from point-to-point in the trajectory. So now we need to write a leg.move() method which follows the trajectory set out for it at a certain speed.
Now we have a move function. What this will do is basically move the leg along the trajectory defined in the class. It will read to see if there is a trajectory. If NOT it will keep the leg stationary, if there is, it will traverse it point by point waiting for the leg to reach a point before commanding the next one.
The last modification we made actually is, we made traverseTo return a boolean value which says if the leg is done traversing. If it is, we will then command the next point. Comparing the position to the get position might introduces unnecessary error so we will try this first. This is also a lot more clean in code.
Now it's time to try it on the real robot. We'll give the front left leg a random 4-point trajectory.
We're giving it a 4-point/4-corner trajectory. It should just go to all four points. Let's try it! Just need to ensure the zero-position of the leg is correct.
So it seems to have moved to the first point fine, however, the moment it gets to the second point it over-currents. No idea why yet we're going to do some debugging. So I think the problem is we're immediately giving it reverse full-speed when we start moving to the next point which is overcurrenting it.
After some more tests, we notice the problem ONLY occurs when a motor is asked to SWITCH DIRECTIONS. That's when we get a large negative command and the system over-currents. So we need to some-how handle current switching. So the other thing I'm noticing is it really does not like this home position. IT keeps over-currenting even if I just sent it home (one-point-trajectory). That tells me this isn't an issue with how fast we are switching from point to point - it's a literal issue with some points themselves - this is likely an problem with the traverse method. Let's check that out.
So what I think is happening, is there are some points we are feeding the system that are straight-up outside the workspace of the robot leg with the simplified IK solution. Just because the leg can actually go there, doesn't mean the inverse kinematics method we are using will find valid solutions. The reason for this is because for the inverse kinematics to find a solution-the solution needs to be a valid triangle. Let's try that.
So now what we have done is instead of making trajectory public and adding straight to trajectory. We're adding points using an ADD method which checks if we can generate a valid triangle from the points before adding it to trajectory. That has eliminated a lot of the bad points we can't go to. I think if we're just careful about commanding points - this will work fine. Most of the points we are commanding won't be anywhere near the weird regions the model breaks down in. We'll make a list of the areas the model breaks down in later. For now. Let's program in a step trajectory of some sort and see if the leg follows it.
This is the square-step trajectory we're thinking of. Let's just program it in and try it!
So that didn't work. It seems when any angles goes negative it gets really weird. I'm going to try a different arctangent like atan2. I think that's the issue. Actually it may just be the atan2 when we send this thing a -x that will allow us to change everything back to before.
OMG PERFECT--it works!!!! All we needed was to change the thing to atan2. Let's try sending the leg home now!
Okay, HOME still doesn't work now. We'll try again with that later but for now this is some success. Let's try traversing at a faster speed.
Here we go... that's a leg traversing a path! It isn't great but it will work for now. We'll add some smoothing at a later point.