Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Use .at() for indexing instead of C style array indices #58

Merged
merged 7 commits into from
Mar 31, 2023
Merged

Use .at() for indexing instead of C style array indices #58

Show file tree
Hide file tree
Changes from 4 commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
7e5e1e5
Change all array indexing to use .at()
Mar 20, 2023
b7858f2
wpiformat
Mar 31, 2023
f627698
wpiformat
Mar 31, 2023
282c125
wpiformat
Mar 31, 2023
231d1df
Use constexpr std array instead of const std vector
Mar 31, 2023
5898fa2
wpiformat
Mar 31, 2023
db331a3
Remove vector include in Constants.hpp
Mar 31, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
74 changes: 37 additions & 37 deletions src/main/cpp/RobotContainer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -288,7 +288,7 @@ std::optional<size_t> RobotContainer::GetAutoSwitchIndex() const {
for (size_t switchIndex = 0;
switchIndex < ElectricalConstants::kAutoSwitchPorts.size();
switchIndex++) {
if (!m_autoSwitch[switchIndex].Get()) {
if (!m_autoSwitch.at(switchIndex).Get()) {
SmartDashboard::PutNumber("Auto Rotary Switch Index", switchIndex);
return switchIndex;
}
Expand Down Expand Up @@ -343,25 +343,25 @@ void RobotContainer::ApplyLEDSingleStrip(

static constexpr auto kStripLen = kLEDStripLength;
size_t firstLEDIdx = stripID * kStripLen;
bool stripDirection = kStripDirections[stripID];
bool stripDirection = kStripDirections.at(stripID);
for (size_t stripBuffIdx = 0; stripBuffIdx < kStripLen; stripBuffIdx++) {
if (stripDirection) { // if goes down
m_ledBuffer[firstLEDIdx + stripBuffIdx].SetRGB(
std::get<0>(stripBuffer[kStripLen - 1 - stripBuffIdx]),
std::get<1>(stripBuffer[kStripLen - 1 - stripBuffIdx]),
std::get<2>(stripBuffer[kStripLen - 1 - stripBuffIdx]));
m_ledBuffer.at(firstLEDIdx + stripBuffIdx)
.SetRGB(std::get<0>(stripBuffer.at(kStripLen - 1 - stripBuffIdx)),
std::get<1>(stripBuffer.at(kStripLen - 1 - stripBuffIdx)),
std::get<2>(stripBuffer.at(kStripLen - 1 - stripBuffIdx)));
} else { // if goes up
m_ledBuffer[firstLEDIdx + stripBuffIdx].SetRGB(
std::get<0>(stripBuffer[stripBuffIdx]),
std::get<1>(stripBuffer[stripBuffIdx]),
std::get<2>(stripBuffer[stripBuffIdx]));
m_ledBuffer.at(firstLEDIdx + stripBuffIdx)
.SetRGB(std::get<0>(stripBuffer.at(stripBuffIdx)),
std::get<1>(stripBuffer.at(stripBuffIdx)),
std::get<2>(stripBuffer.at(stripBuffIdx)));
}
}
}

void RobotContainer::ClearLED() {
for (size_t clrIdx = 0; clrIdx < kLEDBuffLength; clrIdx++) {
m_ledBuffer[clrIdx].SetRGB(0, 0, 0);
m_ledBuffer.at(clrIdx).SetRGB(0, 0, 0);
}
}

Expand All @@ -376,28 +376,28 @@ void RobotContainer::GamePieceLED() {
yellow = !yellow;
}
if (yellow) {
m_ledBuffer[i].SetRGB(255, 100, 0);
m_ledBuffer.at(i).SetRGB(255, 100, 0);
} else {
m_ledBuffer[i].SetRGB(150, 0, 255);
m_ledBuffer.at(i).SetRGB(150, 0, 255);
}
}
}

void RobotContainer::Yellow() {
for (int i = 0; i < kLEDBuffLength; i++) {
m_ledBuffer[i].SetRGB(255, 100, 0);
m_ledBuffer.at(i).SetRGB(255, 100, 0);
}
}

void RobotContainer::Green() {
for (int i = 0; i < kLEDBuffLength; i++) {
m_ledBuffer[i].SetRGB(0, 255, 0);
m_ledBuffer.at(i).SetRGB(0, 255, 0);
}
}

void RobotContainer::Purple() {
for (int i = 0; i < kLEDBuffLength; i++) {
m_ledBuffer[i].SetRGB(150, 0, 255);
m_ledBuffer.at(i).SetRGB(150, 0, 255);
}
}

Expand All @@ -408,7 +408,7 @@ void RobotContainer::SnakeBOI() {
} else {
m_previousSnakeIndex++;
}
m_ledBuffer[m_previousSnakeIndex].SetRGB(255, 0, 0);
m_ledBuffer.at(m_previousSnakeIndex).SetRGB(255, 0, 0);
}

template <typename TE, typename TT>
Expand Down Expand Up @@ -453,7 +453,7 @@ void RobotContainer::AutoLED() {
selectedAutoIdx++) {
for (size_t chunkIdx = 0; chunkIdx < 2; chunkIdx++) {
for (size_t stripIdx = 0; stripIdx < 4; stripIdx++) {
stripBuffers[stripIdx][selectedAutoIdx * 3 + chunkIdx] = {
stripBuffers.at(stripIdx).at(selectedAutoIdx * 3 + chunkIdx) = {
m_isBlue ? 0 : 255, 0, m_isBlue ? 255 : 0};
}
}
Expand Down Expand Up @@ -485,66 +485,66 @@ void RobotContainer::AutoLED() {
for (size_t goodPoseIdx = kLEDStripLength - 1;
goodPoseIdx >= kLEDStripLength - 1 - 4; goodPoseIdx--) {
for (size_t stripIdx = 0; stripIdx < 4; stripIdx++) {
stripBuffers[stripIdx][goodPoseIdx] = {0, 255, 0};
stripBuffers.at(stripIdx).at(goodPoseIdx) = {0, 255, 0};
}
}
}
if (std::get<2>(errors) == 0) { // if rotation is good
for (size_t goodPoseIdx = kLEDStripLength - 1 - 3;
goodPoseIdx >= kLEDStripLength - 1 - 3 - 2; goodPoseIdx--) {
for (size_t stripIdx = 0; stripIdx < 4; stripIdx++) {
stripBuffers[stripIdx][goodPoseIdx] = {0, 255, 0};
stripBuffers.at(stripIdx).at(goodPoseIdx) = {0, 255, 0};
}
}
}
if (std::get<0>(errors) == +1) { // if too far right
for (size_t badPoseIdx = kLEDStripLength - 1;
badPoseIdx >= kLEDStripLength - 1 - 2; badPoseIdx--) {
stripBuffers[1][badPoseIdx] = {255, 0, 0};
stripBuffers[2][badPoseIdx] = {255, 0, 0};
stripBuffers.at(1).at(badPoseIdx) = {255, 0, 0};
stripBuffers.at(2).at(badPoseIdx) = {255, 0, 0};
}
}
if (std::get<0>(errors) == -1) { // if too far left
for (size_t badPoseIdx = kLEDStripLength - 1;
badPoseIdx >= kLEDStripLength - 1 - 2; badPoseIdx--) {
stripBuffers[0][badPoseIdx] = {255, 0, 0};
stripBuffers[3][badPoseIdx] = {255, 0, 0};
stripBuffers.at(0).at(badPoseIdx) = {255, 0, 0};
stripBuffers.at(3).at(badPoseIdx) = {255, 0, 0};
}
}
if (std::get<1>(errors) == +1) { // if too far up
for (size_t badPoseIdx = kLEDStripLength - 1;
badPoseIdx >= kLEDStripLength - 1 - 2; badPoseIdx--) {
stripBuffers[2][badPoseIdx] = {255, 0, 0};
stripBuffers[3][badPoseIdx] = {255, 0, 0};
stripBuffers.at(2).at(badPoseIdx) = {255, 0, 0};
stripBuffers.at(3).at(badPoseIdx) = {255, 0, 0};
}
}
if (std::get<1>(errors) == -1) { // if too far down
for (size_t badPoseIdx = kLEDStripLength - 1;
badPoseIdx >= kLEDStripLength - 1 - 2; badPoseIdx--) {
stripBuffers[0][badPoseIdx] = {255, 0, 0};
stripBuffers[1][badPoseIdx] = {255, 0, 0};
stripBuffers.at(0).at(badPoseIdx) = {255, 0, 0};
stripBuffers.at(1).at(badPoseIdx) = {255, 0, 0};
}
}
if (std::get<2>(errors) == +1) { // if too far counterclockwise
for (size_t badPoseIdx = kLEDStripLength - 1 - 3;
badPoseIdx >= kLEDStripLength - 1 - 3 - 2; badPoseIdx--) {
stripBuffers[0][badPoseIdx] = {255, 0, 0};
stripBuffers[1][badPoseIdx] = {255, 0, 0};
stripBuffers[2][badPoseIdx] = {255, 0, 0};
stripBuffers[3][badPoseIdx] = {255, 0, 0};
stripBuffers.at(0).at(badPoseIdx) = {255, 0, 0};
stripBuffers.at(1).at(badPoseIdx) = {255, 0, 0};
stripBuffers.at(2).at(badPoseIdx) = {255, 0, 0};
stripBuffers.at(3).at(badPoseIdx) = {255, 0, 0};
}
}
if (std::get<2>(errors) == -1) { // if too far clockwise
for (size_t badPoseIdx = kLEDStripLength - 1 - 3;
badPoseIdx >= kLEDStripLength - 1 - 3 - 2; badPoseIdx--) {
stripBuffers[0][badPoseIdx] = {0, 0, 255};
stripBuffers[1][badPoseIdx] = {0, 0, 255};
stripBuffers[2][badPoseIdx] = {0, 0, 255};
stripBuffers[3][badPoseIdx] = {0, 0, 255};
stripBuffers.at(0).at(badPoseIdx) = {0, 0, 255};
stripBuffers.at(1).at(badPoseIdx) = {0, 0, 255};
stripBuffers.at(2).at(badPoseIdx) = {0, 0, 255};
stripBuffers.at(3).at(badPoseIdx) = {0, 0, 255};
}
}
// apply to actual LED strips
for (size_t stripIdx = 0; stripIdx < 4; stripIdx++) {
ApplyLEDSingleStrip(stripBuffers[stripIdx], stripIdx);
ApplyLEDSingleStrip(stripBuffers.at(stripIdx), stripIdx);
}
}
5 changes: 1 addition & 4 deletions src/main/cpp/commands/DriveTrajectory.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -65,8 +65,5 @@ void DriveTrajectory::End(bool interrupted) {
}

bool DriveTrajectory::IsFinished() {
// m_timestamp.HasElapsed(m_trajectory->GetTotalTime());
return m_timestamp.Get() >
m_trajectory->GetTotalTime() +
0_ms; // add extra time at end to correct errors
return m_timestamp.Get() > m_trajectory->GetTotalTime();
}
5 changes: 0 additions & 5 deletions src/main/cpp/subsystems/Vision.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -35,11 +35,6 @@ AprilTagFieldLayout CustomFieldLayout() {
return AprilTagFieldLayout({tagTwo, tagSix}, 10_m, 10_m);
}

// AprilTagFieldLayout CustomFieldLayout() {
// AprilTag tagOne{1, Pose3d(0_m, 0_m, 0_m, Rotation3d())};
// return AprilTagFieldLayout({tagOne}, 10_m, 10_m);
// }

Vision::Vision()
: m_poseEstimator(LoadAprilTagLayoutField(AprilTagField::k2023ChargedUp),
// CustomFieldLayout(),
Expand Down
12 changes: 6 additions & 6 deletions src/main/cpp/util/Trajectory.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,8 +31,8 @@ Trajectory::Trajectory(std::vector<Trajectory::State> states)
: m_states{states} {}

Trajectory::State Trajectory::Sample(second_t t) const {
if (t < m_states[0].t) {
return m_states[0];
if (t < m_states.at(0).t) {
return m_states.at(0);
}
if (t > GetTotalTime()) {
return m_states.back();
Expand All @@ -44,15 +44,15 @@ Trajectory::State Trajectory::Sample(second_t t) const {

while (low != high) {
int mid = (low + high) / 2;
if (m_states[mid].t < t) {
if (m_states.at(mid).t < t) {
low = mid + 1;
} else {
high = mid;
}
}

auto previousState = m_states[low - 1];
auto currentState = m_states[low];
auto previousState = m_states.at(low - 1);
auto currentState = m_states.at(low);

if ((currentState.t - previousState.t).value() == 0) {
return currentState;
Expand All @@ -62,7 +62,7 @@ Trajectory::State Trajectory::Sample(second_t t) const {
}

Pose2d Trajectory::GetInitialPose() const {
return m_states[0].pose;
return m_states.at(0).pose;
}

second_t Trajectory::GetTotalTime() const {
Expand Down
32 changes: 4 additions & 28 deletions src/main/cpp/util/photonlib2/PhotonPoseEstimator.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -101,9 +101,9 @@ std::optional<photonlib::EstimatedRobotPose> PhotonPoseEstimator::Update() {
if (auto tagCorners = CalcTagCorners(id); tagCorners.has_value()) {
auto targetCorners = target.GetDetectedCorners();
for (size_t cornerIdx = 0; cornerIdx < 4; ++cornerIdx) {
imagePoints.emplace_back(targetCorners[cornerIdx].first,
targetCorners[cornerIdx].second);
objectPoints.emplace_back((*tagCorners)[cornerIdx]);
imagePoints.emplace_back(targetCorners.at(cornerIdx).first,
targetCorners.at(cornerIdx).second);
objectPoints.emplace_back((*tagCorners).at(cornerIdx));
}
}
}
Expand All @@ -122,26 +122,6 @@ std::optional<photonlib::EstimatedRobotPose> PhotonPoseEstimator::Update() {

auto begin = std::chrono::system_clock::now();

// if (objectPoints.size() <= 4) { // single target
// // std::vector<cv::Mat> rvecs{2};
// // std::vector<cv::Mat> tvecs{2};

// cv::Mat rvec(3, 1, cv::DataType<double>::type);
// cv::Mat tvec(3, 1, cv::DataType<double>::type);

// cv::solvePnP(objectPoints, imagePoints, m_cameraMatrix,
// m_distortionCoefficients, rvec, tvec, false,
// cv::SOLVEPNP_IPPE_SQUARE);

// SmartDashboard::PutNumber("", objectPoints[0].x);

// // pose1 = ToPose3d(tvecs[0], rvecs[0]);
// // pose2 = ToPose3d(tvecs[1], rvecs[1]); // TODO change order to match
// OpenCV

// pose1 = pose2 = ToPose3d(tvec, rvec);

// } else { // multi target
if (objectPoints.size() >= 8) {
cv::Mat rvec(3, 1, cv::DataType<double>::type);
cv::Mat tvec(3, 1, cv::DataType<double>::type);
Expand Down Expand Up @@ -204,11 +184,7 @@ Pose3d PhotonPoseEstimator::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) {
rv[2] = +rvec.at<double>(1, 0);

return Pose3d(Translation3d(meter_t{tv[0]}, meter_t{tv[1]}, meter_t{tv[2]}),
Rotation3d(
// radian_t{rv[0]},
// radian_t{rv[1]},
// radian_t{rv[2]}
rv, radian_t{rv.norm()}));
Rotation3d(rv, radian_t{rv.norm()}));
}

cv::Point3d PhotonPoseEstimator::TagCornerToObjectPoint(meter_t cornerX,
Expand Down
9 changes: 4 additions & 5 deletions src/main/include/Constants.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,9 +4,9 @@

#include <array>
#include <numbers>
#include <vector>

#include <frc/RobotBase.h>
#include <frc/geometry/Rotation2d.h>
#include <frc/geometry/Rotation3d.h>
#include <frc/geometry/Transform3d.h>
#include <frc/geometry/Translation2d.h>
Expand Down Expand Up @@ -40,16 +40,15 @@ constexpr int kIntakeRightWheelPort = 17;
constexpr int kArmEncoderPort = 0;

/// Maps rotary switch positions/indices to digital input pins on the RIO
constexpr std::array<int, 8> kAutoSwitchPorts = {11, 12, 13, 18,
19, 20, 21, 22};
const std::vector<int> kAutoSwitchPorts = {11, 12, 13, 18, 19, 20, 21, 22};
constexpr int kRedBlueSwitchPort = 10;

/// LED strip
constexpr int kLEDStripLength = 22;
constexpr int kAltLEDStripLength = 18;
constexpr int kLEDBuffLength = kLEDStripLength * 4 + kAltLEDStripLength;
constexpr bool kStripDirections[] = {false, true, true,
true}; // false means up, true means down
const std::vector<bool> kStripDirections = {
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This may violate this issue: #56

false, true, true, true}; // false means up, true means down

} // namespace ElectricalConstants

Expand Down