Skip to content

Commit

Permalink
Use .at() for indexing instead of C style array indices (#58)
Browse files Browse the repository at this point in the history 
* Change all array indexing to use .at()

* wpiformat

* wpiformat

* wpiformat

* Use constexpr std array instead of const std vector

* wpiformat

* Remove vector include in Constants.hpp

---------

Co-authored-by: Joshua Nichols <[email protected]>
  • Loading branch information
@JoshuaAN
JoshuaAN and Joshua Nichols committed Mar 31, 2023
1 parent 9785b27 commit 2e0c3b1
Show file tree
Hide file tree
Showing 6 changed files with 50 additions and 84 deletions.
74 changes: 37 additions & 37 deletions src/main/cpp/RobotContainer.cpp
View file
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
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
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
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
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
6 changes: 2 additions & 4 deletions src/main/include/Constants.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,6 @@
#include <numbers>

#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 @@ -48,9 +47,8 @@ constexpr int kRedBlueSwitchPort = 10;
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

constexpr std::array<bool, 4> kStripDirections = {
false, true, true, true}; // false means up, true means down
} // namespace ElectricalConstants

namespace DriveConstants {
Expand Down

0 comments on commit 2e0c3b1

Please sign in to comment.