Improve drivetrain calculations #14
Conversation
|
This PR also creates an empty Drivetrain class and a new launch file to make it easier to add to later. |
|
Apparently, we have a rectangular swerve drive, so we need to rewrite the code for that. |
|
Swerve finished |
GalexY727
left a comment
GalexY727
left a comment
There was a problem hiding this comment.
I didn't make it through pose2d, translation2d, or rotation2d, and think that it might be beneficial to have unit tests for them which we can probably steal from wpilib directly. We would want that on some other directory, but still accessible from the main repository, most likely. Maybe it would be enough to have them ran through GitHub Actions, but that seems a little extra.
Overall, looks good! I left a lot of mostly cosmetic comments, but do want you to quickly look at the one regarding swerve::optimize, regarding the modulus operator. Well done Kyle!
| #include "messages/msg/wheel_states.hpp" | ||
| #include "../../utils/math/Rotation2d.hpp" | ||
|
|
||
| #define WHEEL_RADIUS 0.06 |
There was a problem hiding this comment.
We should ask if we have an estimate for this value yet because iirc cad is actually done
There was a problem hiding this comment.
It's a lot bigger, about 0.3 meters, but we should probably change it when we change the proto file to avoid inaccurate calculations before then.
| } | ||
| } // namespace slugbot_driver |
There was a problem hiding this comment.
It's the same at the top of the file. The namespace and class declarations have the same indentation, probably to avoid too many nested indents. I copied this from the other file, but we could change it if you think another indent would make it look better.
slugbot_package/src/Simulation.cpp
Outdated
| void set_position(WbDeviceTag *side, float value) { | ||
| for (int i = 0; i < WHEEL_COUNT>>1; i++) { | ||
| for (int i = 0; i < 4; i++) { | ||
| wb_motor_set_position(*side, value); | ||
| side++; | ||
| } | ||
| } |
There was a problem hiding this comment.
Adding this comment to make sure that we don't need to increment the side anymore. I am not familiar with what it represents as of right now, but I trust your judgment.
There was a problem hiding this comment.
I thought I deleted this function. It isn't used anymore, since we really don't need to set all of the motors to the same value more than once, but incrementing the side pointer would still be necessary. I don't know why it is using this syntax to iterate through an array.
| const double TRACK_WIDTH = 0.5; | ||
| const double TRACK_LENGTH = 0.6; | ||
| const double WHEEL_RADIUS = 0.06; | ||
| const double MAX_WHEEL_SPEED = 25.0; |
There was a problem hiding this comment.
should we integrate the units in the constants' name?
i.e. MAX_WHEEL_SPEED -> MAX_WHEEL_SPEED_MPS
or TRACK_WIDTH -> TRACK_WIDTH_METERS
since this is a pretty high-level concern, it doesn't make or break this pr and isn't required for the merge.
There was a problem hiding this comment.
We could either do that or just assume everything is in meters, seconds, and radians. For now, I added comments for the units instead of making the variable names longer.
| void SlugbotDriver::optimize_wheel_states(messages::msg::WheelStates& wheels) { | ||
| for(int i = 0; i < 4; i++){ | ||
| double a = Rotation2d(wheels.angles[i] - current_wheel_states.angles[i]).modulus().getRadians(); | ||
| if (std::abs(a) > M_PI / 2){ |
There was a problem hiding this comment.
do we need ::abs if we are using .modulus()?
There was a problem hiding this comment.
I wrote modulus() to map it to (-π, π], so it can still be negative.
|
Regarding the switch to swerve, however, I think it might be useful to keep the legacy drivetrain code in case we ever decide to switch off of swerve, even if it is in a year. I don't think it's as easy as just making an IO implementation, but it can still be a lone, untouched file without causing any collateral damage. |
|
I added the old SlugbotDriver code in a commented out file so we can reference it in the future if needed. I don't think we need the Webots code, since it basically just sets the motors. |
|
Great judgment, thank you! |
This PR will improve the drivetrain speed and angle calculations. Previously, we made the wheels turn to turn the robot, which allowed the robot to drive without completely breaking the simulator physics. However, it still wasn't completely accurate, since the outer wheels have to travel a longer distance than the inner wheels, while the current code just drives them at the same speed.
To fix this, I modeled the robot's driving as following a circular path directly to the left or right of the center wheels. I calculate the radius of that circle, then figure out the speeds needed for each wheel to follow that path. The front and back wheels now rotate to be tangent to that circle as well.
The code in SlugbotDriver will not need to change if we switch to 4 wheels instead of 6., since the back wheels follow the circle the same way as the front ones. The center of the circle will be in line with the non-turning wheels instead of the center wheels.
This might not be perfect yet since there is a constant at the top of the code for the distance between wheels on the same side, but I don't want to try to get that from the proto file when we are getting the real CAD soon.