Search

So today we want to add in some basic IMU stability! And it will be some form of dynamic stability. Basically, we're including a stabilizer node that has one job, to keep the platform level (for now). No dynamic recovery at the moment though that will be its job in the future, to stop the robot from falling if it is. But not yet.

Just for reference, here is the reference frame of the IMU/System for Arudpilot so we know to go in the right directions while programming this thing in.

The first steps are to read the IMU Data and then covert the read angles from the quaternion to actual Euler roll/pitch/yaw angles. Now... if:

Bx is positive, that means we need to increase the height of the RIGHT Legs and decrease the height of the left ones.

By is positive, that means we need to increase the height of the BACK legs and decrease the height for the FRONT.

We don't care about Bz, we aren't doing Yaw correction just yet.

And this is what we end up with. Let's add it to the launch file and see what happens when we ask chip to stand up. We'll also likely need to re-flat trim this IMU. I don't think the accelerometers are calibrated correctly either to the platform so we may have to do that. This and the KP/KI/KD tuning will need work as well.

Ok so it's pretty clear to me I have no idea what an Euler angle is and how to get one from a quaternion. Let's leave that up to the smart ppl. And use: https://www.kaggle.com/c/career-con-2019/discussion/85303 this python function!

The data coming out of this node makes so much more sense than the data that came out of the node I wrote. I clearly don't understand this from my quick skim of wikipedia, but I'll get there eventually. The important thing is now does the IMU program work? Let's try it! We're starting with K Gains of 0.01! (Very small so our friend IMU doesn't go nuts over here). Before we do all that we've been working on the enabler as well.

Our system is getting really complicated. We're going to work on this ENABLER node and testing the IMU node tomorrow.