Better drivetrain control

[varun7654]

Our current implementation has a few issues with it with most of them stemming from A. Commanded Speed =/= Actual speed and B. Not considering how the rotation and translation components of our movement affect each other.

To improve the accel limiting I think we should consider implementing an acceleration limit that recognizes the direction. We’ve talked about having different acceleration limits for the battery side, but I also realized that we should be able to accelerate much faster when moving diagonally as well. I’m still what exactly the physics on this would be. I've created what I think is an approximate solution by calculating the distance to the frame permitter from our CG (using the direction of accel). The exact values/relationship would need to be determined experimentally

Another issue we have is if the robot is moving and we command a quick turn the robot can slow suddenly while it turns. This happens because right before we command the motor speeds (but after we've issued our wanted speeds) we desaturate the wheel speeds to ensure that no wheel is being commanded to go over its maximum velocity.
This leads to issues since we're not directly controlling this. We could implement a cap on the max turning speed, but my current idea (that would be necessary for the solution below) for this was to do some sort of binary search for a solution that meets all our constraints so that we're completely in control over how the wheel speeds are being desaturated.

One of the assumptions we've been making is that our commanded speed is the speed the wheel would be driven at. At best it's closeish to reality, but it's a poor assumption to make.
The robot characterization that we did for our robot does give us kA values so that we can add voltage to our wheels when accelerating, but they're difficult to use especially when you start to add in rotation and when we’re reaching out voltage limits.
I think the foolproof way to solve this is to consider all commanded speeds to the robot as forces (actually directly use voltage, but into the chassis speeds object, so we don't need to worry about mass) instead.
This would require us to do a few things.

1. Disable break mode (we would instead explicitly supply a force to slow the robot)
2. Figure out the moment of inertia of our robot. We could probably get this cad, but it would probably be better to experimentally figure this out.
3. Be able to calculate the friction forces (translational and rotational) (this is actually our kV when we set it to our current speed values)
4. Do some of our own voltage compensation since we don't want to arbitrarily limit our motors output power. We'd need to dynamically calculate how much power we have available and factor that into our calculations. (maybe?)

All of this would give us much more precise control over our robot. It'd allow us to determine values such as the acceleration of our robot which could be factored into our shoot and move for example without the need to depend on noisy sensors. We could also use this to have better control over our robot during auto (especially when we're turning and moving where we tend to drift off our path). Better understanding of our robot dynamics could allow us to more easily aim with the robot knowing the exact forces that it would need to apply to slow the robots turn.