Post-comp commit

4698RaiderRobotics · Oct 30, 2023 · f311efa · f311efa
1 parent c7d7d6c
commit f311efa
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 7 deletions.
diff --git a/src/main/cpp/commands/ArmSet.cpp b/src/main/cpp/commands/ArmSet.cpp
@@ -30,12 +30,12 @@ void ArmSet::Initialize() {
   if (m_delayProportion > 0) {
     if (m_delayArmAngle > 0_deg /* && m_arm->GetArmAngle() < m_startingArmAngle + m_delayArmAngle */) {
       // fmt::print("Wrist Delayed until arm at {}, current {}, time {}\n", m_startingArmAngle + m_delayArmAngle, m_arm->GetArmAngle(), frc::Timer::GetFPGATimestamp());
-      // m_arm->GotoAngle(m_armAngle, m_startingWristAngle);
+      m_arm->GotoAngle(m_armAngle, m_startingWristAngle);
       m_isWaiting = true;
     }
     else if (m_delayArmAngle < 0_deg /* && m_arm->GetWristAngle() < m_startingWristAngle + m_delayWristAngle */) {
       // fmt::print("Arm Delayed until wrist at {}, current {}, time {}\n", m_startingWristAngle + m_delayWristAngle, m_arm->GetWristAngle(), frc::Timer::GetFPGATimestamp());
-      // m_arm->GotoAngle(m_startingArmAngle, m_wristAngle);
+      m_arm->GotoAngle(m_startingArmAngle, m_wristAngle);
       m_isWaiting = true;
     }
     /*

diff --git a/src/main/cpp/subsystems/ArmSubsystem.cpp b/src/main/cpp/subsystems/ArmSubsystem.cpp
@@ -104,7 +104,7 @@ void ArmSubsystem::Periodic() {
 
 // Changed the setpoint of the arm
 void ArmSubsystem::GotoAngle(units::degree_t armAngle, units::degree_t wristAngle) {
-    // fmt::print( "GoToAngle = {}\n", angle);
+    // fmt::print( "GoToArmAngle = {}, GoToWristAngle = {} \n", armAngle, wristAngle );
     m_armAngleGoal = armAngle;
 
     m_wristAngleGoal = wristAngle;
@@ -115,6 +115,7 @@ void ArmSubsystem::GotoAngle(units::degree_t armAngle, units::degree_t wristAngl
 
     if (m_wristAngleGoal > maxWristAngle) m_wristAngleGoal = maxWristAngle;
     if (m_wristAngleGoal < minWristAngle) m_wristAngleGoal = minWristAngle;
+    // fmt::print( "GoalArm = {}, GoalWrist = {}\n", m_armAngleGoal, m_wristAngleGoal );
 }
 
 // Allows setpoint of arm to be nudged
@@ -193,10 +194,10 @@ void ArmSubsystem::ArmData() {
     frc::SmartDashboard::PutNumber("ArmPosition", GetArmAngle().value());
     frc::SmartDashboard::PutNumber("ArmRawPosition", m_armEncoder.GetRawPosition());
 
-    frc::SmartDashboard::PutNumber("Goal Angle", m_wristSetpoint.position.value());
-    frc::SmartDashboard::PutNumber("Goal Velocity", m_wristSetpoint.velocity.value());
-    frc::SmartDashboard::PutNumber("Current Angle", wristAngle.value());
-    frc::SmartDashboard::PutNumber("Current Velocity", m_enc.GetVelocity() * 0.0129 * 6.0);
+    frc::SmartDashboard::PutNumber("Goal Wrist Angle", m_wristSetpoint.position.value());
+    frc::SmartDashboard::PutNumber("Goal Wrist Velocity", m_wristSetpoint.velocity.value());
+    frc::SmartDashboard::PutNumber("Current Wrist Angle", wristAngle.value());
+    frc::SmartDashboard::PutNumber("Current Wrist Velocity", m_enc.GetVelocity() * 0.0129 * 6.0);
     frc::SmartDashboard::PutNumber("WristPosition", GetWristAngle().value());
     frc::SmartDashboard::PutNumber("WristRawPosition", m_wristEncoder.GetRawPosition());
 }