chip updates: stop being scared! let's try to stand on two legs [updates]
Sometimes, you just have to try things and go for it. We're wondering if PID control will work for this situation? The system is too complex to model? Well it's time to just try it. Have a little time right now.
So... here's what we're gonna doooo... We're going to go into the balance code and we're going to edit it so only the FR and BL legs have the IMU stabilization. And we're going to turn the IMU.ON parameter in defaults to True. We're also making this stabilization active on only TEST mode.
Then what we're going to do is go into defaults.py and add a line where it lifts the FL and BR legs! And then we'll add some code to MASTER that publishes this value when we push a certain button when in test mode.
Same commands as getting the robot to stand and sit but it's stand and lift legs now.
And what we'll do is make the robot lift its legs, see if it balances, it won't, so we will put the legs back down, and we will see if changing the PID parameters lets it balance, and then keep trying that.
Kp = 25.0, Ki = 0.0, Kd = 0.0 --> this definitely won't work but we're going to try it anyways. We'll likely also increase the limits on the x-direction of the PID controller.
We're going to bench-test TEST mode before we make the robot try to stand on two legs of course. I will say, if we can find SOME PID where this thing balances --> not only will it be amazing, we'll have solved all of CHIP's problems. The theory here is the x-movement of the legs will provide the major stabilization and the z-movement will provide the minute corrections you know. So Kp of z should probably be a lot lower we'll try this as we go. We're not even going to try with lifting the robot first, we're just going to pick up the robot, put it in test mode, and then put the robot back down on the two legs to see if that does anything. We'll tune like that first. And kind of read the average xyz position the legs are in. Let's at least test this amount of code right now. We're not using the trajectory feature for now either --> full simplification.
As we can see from the above video the code does what we told it to do. I'm now going to place the robot on the floor on two of its legs and watch it fall over.
What this tells me is the Kp needs to actually be higher. What we want to see is when the platform falls forward the lower legs should move forward and then it should fall backward again and then forward and backward, we want this thing to start oscillating. Let's make Kp=50 and then we can get to work with I and D when we get that kind of behavior.
Wait, first I noticed something, we're giving the wrong sign to the X command because when I tilt the robot backward the legs should go backward! We should be doing -PITCH_OUTPUT, let's the that before we play with Kp! I think the roll output is right.
Okay so the legs are moving in the right direction now, I still want to increase Kp before we run another test so let's do that let's make the Kps like 40 on the x and leave the 25 on the z. (When I say x and z I actually mean shoulder and hinge, sorryyy).
So this is the direction of response we wanted. I think instead of increasing the Kp we should bring the Kp back down to 25 and we should increase the limits on the output, I think we should also take the Kp of the z down to zero because it might be interfering more than it is helping. Let's see if we can get some oscillatory behavior with the new settings and see if we can then stabilize them. This Kp actually feels pretty okay, we might drop it to 30 instead of 35, and I might drop the output range to 20 because it should be able to balance within that region. I think it might be time to add D to stabilize the system! We're going to start with a HIGH kD of like 5...
Actually we'll start with Kd of 2 because 5 on the stand was showing wobbling, like a lot of wobbling. Let's pick it off the stand now. Kp=30, Kd=2, and Range is magnitude 20 rotations.
So that's not horrible in the small range it still grows to infinity - I'm thinking we might need to decrease Kp a lot this is pretty violent. The other thing I was thinking of trying was making the KNEE joints do stability not the shoulder but I'm not so sure about that b/c those more move the platform in the Y not the x-direction. We're getting closer with Kp=20.0, and I want to add in a little of Z-stability only Kp=5 there. Just for that smidgen of z-adjustement. I want to increase Kd to 10 with the Kp=20.
Nope... Kd=10 is way too high. Let's try decreasing Kp even more and Kd down to 0.5, we're going to up the z-kp to 7.0 and see if that does something. Let's make kp=10 and kd=5, and see what happens.
Okay now I want to know what happens if we try this balancing but with three legs on the ground just because we need to start with something stable to get anywhere with this I think. The two legs is like taming a wild horse.
So we're going to lift the back-right leg and add in the front-left for stabilization. Let's see what happens here, we'll set Kp=15, and we'll set everything else to 0 no z-stabilizaiton. The goal here is to be able to lift any of the legs. And we don't control the leg begin lifted with the IMU.
The moment we made Kp=35 for the x and kp=10 for the z, our robot was lifting its back left leg off the ground. Now I want to increase the Kp of z a bit because I think it could be a little better. And we want to test if picking the foot up off the ground keeps the platform stable. Not just placing it on three legs.
This did work but it was kind of violent. So we want to see if we can't make it not violent. Large Kp but lower limits on the x!
Well that was a disaster...
We need to re-think this strategy a little bit. What we want is for when we command any leg to lift up, the platform does not fall over it in-fact stays level. We might need to try something else here. I can't seem to figure out what the best strategy here is. The two legs was barely going to work. For three legs we just need the CoG to be within the three feet and that's, for some reason, proving to be really difficult. We got the imu to balance when we placed on leg on the thing. But it's hard to get it to balance when we start form standing or something.
Some more thinking is required - also I really want to know where this CoG is and why it's so strange...