Added Kalman Filter + Minor Motion Control Changes #3420
Conversation
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
itsarune
reviewed
Jan 13, 2025
itsarune
reviewed
Jan 13, 2025
itsarune
reviewed
Jan 13, 2025
itsarune
reviewed
Jan 13, 2025
itsarune
reviewed
Jan 13, 2025
src/proto/tbots_software_msgs.proto
Outdated
Comment on lines
20
to
23
| map<uint32, Angle> robot_orientations = 5; | ||
|
|
||
| uint64 sequence_number = 4; | ||
| } |
There was a problem hiding this comment.
Suggested change
| map<uint32, Angle> robot_orientations = 5; | |
| uint64 sequence_number = 4; | |
| } | |
| uint64 sequence_number = 4; | |
| map<uint32, Angle> robot_orientations = 5; | |
| } |
itsarune
reviewed
Jan 13, 2025
itsarune
reviewed
Jan 13, 2025
itsarune
reviewed
Jan 13, 2025
itsarune
reviewed
Jan 13, 2025
itsarune
reviewed
Jan 13, 2025
itsarune
reviewed
Jan 13, 2025
itsarune
reviewed
Jan 13, 2025
itsarune
reviewed
Jan 13, 2025
itsarune
reviewed
Jan 13, 2025
itsarune
reviewed
Jan 13, 2025
itsarune
reviewed
Jan 13, 2025
itsarune
reviewed
Jan 13, 2025
itsarune
reviewed
Jan 13, 2025
itsarune
reviewed
Jan 13, 2025
itsarune
reviewed
Jan 13, 2025
itsarune
reviewed
Jun 3, 2025
itsarune
reviewed
Jun 3, 2025
itsarune
reviewed
Jun 3, 2025
itsarune
reviewed
Jun 4, 2025
itsarune
reviewed
Jul 14, 2025
| { | ||
| if (!initialized_) | ||
| { | ||
| return std::nullopt; |
There was a problem hiding this comment.
maybe we should have a LOG(INFO) here telling people that the IMU is uninitialized
itsarune
reviewed
Jul 14, 2025
| int ret = ioctl(file_descriptor_, I2C_SLAVE_FORCE, 0x6b); | ||
| if (ret < 0) | ||
| { | ||
| return std::nullopt; |
There was a problem hiding this comment.
Also LOG(INFO) that this SPI operation failed
itsarune
reviewed
Jul 14, 2025
itsarune
reviewed
Jul 14, 2025
itsarune
reviewed
Jul 14, 2025
There was a problem hiding this comment.
changes to this file seem accidental
itsarune
reviewed
Jul 14, 2025
itsarune
reviewed
Jul 14, 2025
Comment on lines
+104
to
117
| reserved 5; | ||
| oneof primitive | ||
| { | ||
| MovePrimitive move = 1; | ||
| StopPrimitive stop = 2; | ||
| DirectControlPrimitive direct_control = 3; | ||
| } | ||
| Angle orientation = 7; | ||
|
|
||
| // Sequence number for the primitive | ||
| uint64 sequence_number = 4; | ||
|
|
||
| reserved 5; | ||
|
|
||
| // Epoch timestamp when primitives were assigned | ||
| Timestamp time_sent = 6; |
There was a problem hiding this comment.
nit: can you make these in order?
itsarune
reviewed
Jul 14, 2025
itsarune
reviewed
Jul 14, 2025
itsarune
reviewed
Jul 14, 2025
itsarune
reviewed
Jul 14, 2025
itsarune
reviewed
Jul 14, 2025
itsarune
reviewed
Jul 18, 2025
Comment on lines
+114
to
+121
| auto least_significant = | ||
| int16_t(i2c_smbus_read_byte_data(file_descriptor_, YAW_LEAST_SIG_REG)); | ||
| auto most_significant = | ||
| int16_t(i2c_smbus_read_byte_data(file_descriptor_, YAW_MOST_SIG_REG)); | ||
|
|
||
|
|
||
| auto foo = (int16_t)(most_significant << 8); | ||
| auto full_word = foo + least_significant; |
There was a problem hiding this comment.
don't use auto. Use the types directly. Especially when using bit shift operators, it becomes very difficult to predict how the number will get interpreted.
itsarune
reviewed
Jul 18, 2025
| int16_t(i2c_smbus_read_byte_data(file_descriptor_, YAW_MOST_SIG_REG)); | ||
|
|
||
|
|
||
| auto foo = (int16_t)(most_significant << 8); |
There was a problem hiding this comment.
Suggested change
| auto foo = (int16_t)(most_significant << 8); | |
| int16_t foo = static_cast<int16_t>(most_significant << 8); |
itsarune
reviewed
Jul 18, 2025
| @@ -238,6 +247,10 @@ void Thunderloop::runLoop() | |||
| // Save new primitive | |||
| primitive_ = new_primitive; | |||
|
|
|||
| auto orientation_msg = primitive_.orientation(); | |||
There was a problem hiding this comment.
avoid auto unless it's a really long type name. It makes the code harder to read for newer members (especially since we have multiple "angle"-ish types in the codebase)
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
Please fill out the following before requesting review on this PR
Description
Many changes were done.
Implemented a kalman filter for thunderloop that composes IMU data to track robot position more accurately.
Testing Done
Moved robot
Tested IMU variance, got about 0.0019 deg variance, expected about 0.0016 deg.
Resolved Issues
Length Justification and Key Files to Review
Review Checklist
It is the reviewers responsibility to also make sure every item here has been covered
.hfile) should have a javadoc style comment at the start of them. For examples, see the functions defined inthunderbots/software/geom. Similarly, all classes should have an associated Javadoc comment explaining the purpose of the class.TODO(or similar) statements should either be completed or associated with a github issue