diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index ecdb296..c303eea 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -288,7 +288,7 @@ std::optional 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; } @@ -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); } } @@ -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); } } @@ -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 @@ -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}; } } @@ -485,7 +485,7 @@ 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}; } } } @@ -493,58 +493,58 @@ void RobotContainer::AutoLED() { 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); } } diff --git a/src/main/cpp/commands/DriveTrajectory.cpp b/src/main/cpp/commands/DriveTrajectory.cpp index c65238b..088c41a 100644 --- a/src/main/cpp/commands/DriveTrajectory.cpp +++ b/src/main/cpp/commands/DriveTrajectory.cpp @@ -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(); } diff --git a/src/main/cpp/subsystems/Vision.cpp b/src/main/cpp/subsystems/Vision.cpp index 861c33d..317a723 100644 --- a/src/main/cpp/subsystems/Vision.cpp +++ b/src/main/cpp/subsystems/Vision.cpp @@ -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(), diff --git a/src/main/cpp/util/Trajectory.cpp b/src/main/cpp/util/Trajectory.cpp index 83a13ad..0b87870 100644 --- a/src/main/cpp/util/Trajectory.cpp +++ b/src/main/cpp/util/Trajectory.cpp @@ -31,8 +31,8 @@ Trajectory::Trajectory(std::vector 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(); @@ -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; @@ -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 { diff --git a/src/main/cpp/util/photonlib2/PhotonPoseEstimator.cpp b/src/main/cpp/util/photonlib2/PhotonPoseEstimator.cpp index a91e8d3..52d18be 100644 --- a/src/main/cpp/util/photonlib2/PhotonPoseEstimator.cpp +++ b/src/main/cpp/util/photonlib2/PhotonPoseEstimator.cpp @@ -101,9 +101,9 @@ std::optional 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)); } } } @@ -122,26 +122,6 @@ std::optional PhotonPoseEstimator::Update() { auto begin = std::chrono::system_clock::now(); - // if (objectPoints.size() <= 4) { // single target - // // std::vector rvecs{2}; - // // std::vector tvecs{2}; - - // cv::Mat rvec(3, 1, cv::DataType::type); - // cv::Mat tvec(3, 1, cv::DataType::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::type); cv::Mat tvec(3, 1, cv::DataType::type); @@ -204,11 +184,7 @@ Pose3d PhotonPoseEstimator::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) { rv[2] = +rvec.at(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, diff --git a/src/main/include/Constants.hpp b/src/main/include/Constants.hpp index d175a4d..775d57d 100644 --- a/src/main/include/Constants.hpp +++ b/src/main/include/Constants.hpp @@ -6,7 +6,6 @@ #include #include -#include #include #include #include @@ -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 kStripDirections = { + false, true, true, true}; // false means up, true means down } // namespace ElectricalConstants namespace DriveConstants {