chip updates: rewriting, everything, + PID yet again [updates]
Yes so today is the day since I don't have much class we're going to re-write, oh I don't know, everything. We're going to start by creating a new catkin workspace and working on the ROS nodes. And then we will load the new code on the RoboRIO with some very low PID constants so that we can be sure not to over current anything. And then we'll get to actual PID tuning one joint at a time. We'll do some actual math on the power limited hopefully.
ros setup stuffs!
(1) created catkin package CHIP_ROS
(2) Copied over defaults, control_modes, rio_data_read, and rio_data_write
Let's start with data write, that has the least to change. We're going to change the name of the topic to /CMDS and simply change the NT values its writing to. The CMDS vector is a float64multiarray where [fl_s, fl_h, fl_k ...] so on instead of X, Y, Z...
Yea... that's it for the write node. Straightforward. The read node is a little more involved. First off, it's only publishing two topics now, the positions, and then the currents. So that's all we need to do really. But don't forget, we actually do still have 3-topics to publish because now we need to publish the average bus voltage to the network!
Those are the changes. We've added VBUS and we've added shoulder, hinge, knee and not x, y, z! So that's it for what's here. That's the DEFAULTS (which we aren't changing just yet), CONTROL_MODES (also not changing yet), the READ, and the WRITE nodes.
pid stuffs (testing those current limits)!
Now we're moving onto new stuff. We're going to start with PID constant of 0.05, 0.0, 0.0 and see how well that goes. There is other good news, if we limit the power output to like 0.5, what that means is the gearboxes will be in their "ok" range instead of their "I'm gonna break" range! So this is a good thing!
ANYWAYSSSS let's write a quick master node that lets us send the legs set-points. We'll do some basic hand math to figurer out what set point to send the motors to. Let's say we want to give each leg pi degrees of motion max. We're going to start with pi/(2*pi) is 0.5 turns of the output shaft x 100:1 ratio is 50 turns of the motor, so 50 as the set point. We'll start with like 4 or 5 to make sure the leg is going in the right direction and then we will go to 50 and tune PID and the output power to see if we can get the response we want. But first! Let's write a quick master node that'll let us use the joystick to do this. Now that's for the shoulder and knee joints. For the hinge joints we don't want that thing to me moving more than 0.05 in either direction so let's see what that is in commands. For the hinge joint, we shouldn't command more than 10 rotations in either direction for safety, we can start with 3 or so.
With a few small changes the code to the roboRIO deployed just fine. Now let's open up the ROS node and watch this thing overcurrent!
So interesting news. At Kp=0.05 and if we send the leg to 5.0 (just one knee joint) then there's no over currenting and the VBUS doesn't really change. So let's see if we can send it to 20.0 and if it will over current.
YES - if I sent the motor (one of them) to 20.0, the voltage drops to 7.0 and then the system overcurrents. So we can now officially see what's happening - the RIO is browning out. That's what happening it's not an overcurrent. It's too much power into the system. Remember our electrical system can only handle 40A total. Each motor on the FRC PDP is supposed to handle 40A which means we need to likely limit the total power of the motors to 1/12 of what is possible on output and the system should not overcurrent.
We could set each motor to 1/12 power but that doesn't appear like its going to do anything just yet because we don't know if this is (a) a current issue. Because there's only one motor running and the system is still shutting down. Maybe a VRM on the RoboRIO is the best move after all. I saw the voltage drop lowest to 7.0 volts, the regulator would be able to handle something like that. Because what's happening isn't that the legs can't handle the power, the RIO can't handle the voltage drop and we should expect a voltage drop. STILL, limiting the power is a good idea, because we don't need that kind of power and we don't want to add an extra component. We aren't looking for the KP where the system over currents. We should be looking for the power settings at which the system doesn't over current. This is a KP=0.05!
Let's do some math: 0.05*(0-20.0) = 1.0 --> and since the power then gets limited on 0-1.0, I think what this means is at 20 revolutions away and at a KP of 0.05, we're commanding MAX power. But at 5 revs 0.05*(0-5.0) = 0.25, we're only commanding 25% of power max. So let's see what the limit on the output is shall we?
So before this version of CHIP, what was happening was we were using a KP of about 0.2 and feeding no limits to the outputs of the PID right. But we were limiting how large the error could be! We were only setting it 0.7 rotations at a time, or when we were playing with it I think we got up to about 1.25 rotations before the system maxed out. But that does not account for I or D.
And last time, before, we were commanding a KP of 0.2 at a distance of 0.7 away --> (0.2*0.7) = 0.14 ~14% power and it over current around 1.25 rotations away which is ~25% power. I think that's roughly the limit! But that does not account for I or D either.
Let's try 10.0 as a set point --> that's max output of 50% power. (0.05*10) = 50%
As we can see there, the system does not overcurrent and on the readout there's not much change in the voltage either. But what previous math told us is that this system can stand with the output limits at like 14% power to the motors. So we can do some more math to figure out if we know what KP we really want we can figure out what the maximum output power needs to be for the system to not overcurrent. But we want to find the actual limit first. Let's try 15.0 rotations. (this is limit for one leg though remember).
15 works lets try 18.0! 18.0 works, 19.0? 19.0 is the limit which means at KP of 0.05*19.0 = that's 95%.
That's amazing because what that means is this system can fully handle the full output of exactly one motor like the 40A breakers on the FRC PDP suggests... hm...
Let's say we want KP of 0.5 - we know the limit for the entire system is 1.0 so each individual motor can have a limit of 1.0/12 if they're all moving at the same time and if only 8 are moving at any given time the limit is 1/8 ~0.125 power outputted. Assume there's some safety and we ca ego a little higher than that. But that puts us right around the KP=0.2 and rotations is 0.7 like we were using last time. So if we limit all the motor to 14% output power, the system won't over current. So if we limit everything to -0.14 to 0.14 we can safely move all the motors because experimentally we did that before (that's essentially what we were doing), I'm going to bump this limit to account for I and D and say it was about ~23%! And see what happens so -0.23 to 0.23 are the output limits.
So let's set the output limits to that and tune the PIDS! This also assumes we know that kMinOutput and kMaxOutput do. So let's set those fist, keep kp at 0.05 and send the leg from 0 to 50.0 and observe the speed.
Beautiful. This is exactly what we want. Now we want to tune the PID gains of the hinge, the knee, and the shoulder. Since we can't reasonably tune the gains of the hinge since we don't have the range of motion to do so. We're going to tune the knee and the shoulder and use the shoulder PID for the hinge because the shoulder and hinge are moving (roughly) the same amount of weight. So here's the method we're going to use:
We're going to toggle between -10 and 10 for the motor so we can stably tune the system. We want the highest possible KP. We know it has to be greater than 0.1! That's what worked last time. 0.2 would be preferable, and 0.5 would be amazing, the higher the KP the less wiggly the motors will be. Even though overall, I think this won't be very wiggly anymore. We're going to start with a high kp of 0.5 because this means we might just overshoot. We may need to go back to not limiting the output power for this... Let's see.
0.5 is working fine LOL - we should turn off the limits and take the system to 1 rotations only. Let's see if it overshoots. REMEMBER, we're trying to get it to overshoot. No overshoot at KP = 0.5 ... let's try 0.6? We can't go KP higher than 1.0 in this case the system will overcurrent. There might be too much damping for overshoot here... If this doesn't overshoot we're going to likely set Ku =0.5 and then use the PID tunings above. Yea... it's not overshooting. Let's go back to limiting it and just try Ku = 0.5... Let's assume Tu is one second. Makes sense for a system like this.
KP = 0.3
KI = 0.6
KD = 0.0375
Here's another tuning method but we have no way to graph! We can try the huge KI above and see where that gets us. If KI is too large we'll saturate the controller too early. But in this case, we're saturating the controller anyways because we want it to saturate. So we may be fine? Let's try it.
Okay... now it oscillated with a period of around 3.89 seconds! So that was with Kp = 0.3, KI=0.6, and KD is small. So let's say P = 0.5*0.6, I=1.2*0.5/3.89, D=3*0.5*3.89/40. KP=0.3, KI=0.15, KD=1.4... hmmm okay let's see. KD being so high makes me uncomfortable but let's see if it works!
OKAY SO WE'VE REALLY MESSED THIS UP OOPS LOL.
Okay so what we're going to do... is keep the saturation. And increase kp to like 4.0 and see if it oscillate because it won't over current. Then we can reduce KP to 0.6 and increase until it oscillates, and then use the method above... Learning here we go! Remember it should still settle. The system shouldn't just continuously oscillate forever...
So with Kp = 2.0, the system didn't oscillate but the motor started clicking, which means it was upset. and 2.0 * 0.6 is KP=1.2 which is way higher of a KP than I'm comfortable with. So the highest KP I'm comfortable with is 0.5. What we're going to do is set the KP to 0.4 and use the following method. Add D until it's unstable, and then I until it's stable again. We'll do this for the knee and the hinge independently.
So yes, none of this is ideal but it doesn't have to be, it has to do two things (1) maintain any set point we give it, (2) and not drift. It's the drift we're really tuning for since the KP is higher than 0.2!
If we hear buzzing, that is the motor oscillating. KP=0.4, right now KD=1 and KI=0. More buzzing! Loud annoying buzzing this time. The KD of 1 is as high as I'm comfortable with, let's add KI until the buzzing stops, and then we'll call it a day for the knees. And post a video! KI of 0.1 made is oscillate. KI of 0.005 made it oscillate but then go to 0. So I'm going to drop to 0.3, 0.005, and KD of 0.6! It oscillated faster this time.
Now let's make KP = 0.3, KD = 0.01, and KI = 0.0001 and that should do it. Anything higher and we overshoot. Here's the video. Remember we are prioritizing KP because that's what's going to give us the best disturbance rejection!!! That's STABLE.
NOW ONTO THE SHOULDER AND HINGE YAY!!!! And then let's see if we can move all of the legs at the same time! We're going to start with the following PID Values: 0.4, 0.0005, 0.01 and tune them down.
Okay that didn't work: 0.4, 0.0002, 0.01? We increased KP by 0.1 because better disturbance rejection on the shoulder would be good since there could be more torque on it. There's still a very slight bit of overshoot - let's go to like 0.35, 0.0001, 0.01?
Here are the final values:
Now let's try moving all the legs at the same time!!! Okay we tried it and it didn't work I think the power needs to be limited even more oops. Lets try 0.14 for now! If this becomes a problem we can move back to the original PID values of 0.2, 0.0005, 0.01 or something. So this thing is still overshooting but at least the thing isn't over currenting. I'm just going to set all PID values to this, this is what worked in the last code version, we're just a lot smoother now:
Let's try some finessing. Here are the final PID values, and a video of the result.
Analysis: the system isn't faster than it was before. Which is actually good because otherwise it would be unstable. But it's a lot smoother which is good because this helps the motor life and just give us finer control on the output of the robot.
So that's it for now. We've fixed the PID and we've fixed the over-currenting issue for now. Later we'll look at starting to re-build up the CHIP_ROS system, standing tomorrow! (Or maybe later tonight).