From d718a7cba92b5354c3ca74517b5ad6f9a11de2f2 Mon Sep 17 00:00:00 2001 From: stsmith306 Date: Fri, 10 Nov 2023 09:41:54 -0800 Subject: [PATCH 1/3] Increased IsFinished tolerance --- src/main/cpp/commands/DriveToPoseCommand.cpp | 2 +- src/main/cpp/subsystems/ArmSubsystem.cpp | 9 +++++++++ 2 files changed, 10 insertions(+), 1 deletion(-) diff --git a/src/main/cpp/commands/DriveToPoseCommand.cpp b/src/main/cpp/commands/DriveToPoseCommand.cpp index b3df05b..31d3fe0 100644 --- a/src/main/cpp/commands/DriveToPoseCommand.cpp +++ b/src/main/cpp/commands/DriveToPoseCommand.cpp @@ -104,7 +104,7 @@ bool DriveToPoseCommand::IsFinished() { // m_drive->GetPose().X(), m_drive->GetPose().Y(), m_drive->GetPose().Rotation().Degrees(), // distance_error, angle_error ); - bool atTargetLocation = distance_error < 1.0_cm && angle_error < 0.15_deg; + bool atTargetLocation = distance_error < 3.0_cm && angle_error < 1_deg; if( atTargetLocation ) { fmt::print( "DriveToPoseCommand::IsFinished target( {}, {}, {} ), robot( {}, {}, {} ) = L{}, A{}\n", m_targetpose.X(), m_targetpose.Y(), m_targetpose.Rotation().Degrees(), diff --git a/src/main/cpp/subsystems/ArmSubsystem.cpp b/src/main/cpp/subsystems/ArmSubsystem.cpp index 5924151..282b9a3 100644 --- a/src/main/cpp/subsystems/ArmSubsystem.cpp +++ b/src/main/cpp/subsystems/ArmSubsystem.cpp @@ -6,6 +6,8 @@ #include #include +#include "DataLogger.h" + ArmSubsystem::ArmSubsystem() { m_rightArm.SetInverted(true); m_rightArm.Follow(m_leftArm); @@ -218,6 +220,13 @@ void ArmSubsystem::ArmData() { 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.GetAbsolutePosition() * physical::kWristAbsoluteOffset.value()); + + // DataLogger::GetInstance().Send( "Arm/Left Arm Current", m_leftArm.GetOutputCurrent() ); + // DataLogger::GetInstance().Send( "Arm/Right Arm Current", m_rightArm.GetOutputCurrent() ); + // DataLogger::GetInstance().Send( "Arm/Goal Angle", m_armSetpoint.position.value() ); + // DataLogger::GetInstance().Send( "Arm/Goal Velocity", m_armSetpoint.velocity.value() ); + // DataLogger::GetInstance().Send( "Arm/Current Angle", armAngle.value() ); + // DataLogger::GetInstance().Send( "Arm/Current Angle", armAngle.value() ); } units::degree_t ArmSubsystem::GetArmAngle() { From 608679860cae7b8359584849a60fa2ce9c52b2e5 Mon Sep 17 00:00:00 2001 From: stsmith306 Date: Fri, 10 Nov 2023 13:49:45 -0800 Subject: [PATCH 2/3] Pre-comp tweaks --- src/main/cpp/subsystems/ArmSubsystem.cpp | 2 +- src/main/cpp/subsystems/Limelight.cpp | 2 +- src/main/include/Constants.h | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/cpp/subsystems/ArmSubsystem.cpp b/src/main/cpp/subsystems/ArmSubsystem.cpp index 282b9a3..7a14c84 100644 --- a/src/main/cpp/subsystems/ArmSubsystem.cpp +++ b/src/main/cpp/subsystems/ArmSubsystem.cpp @@ -219,7 +219,7 @@ void ArmSubsystem::ArmData() { 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.GetAbsolutePosition() * physical::kWristAbsoluteOffset.value()); + frc::SmartDashboard::PutNumber("WristRawPosition", m_wristEncoder.GetAbsolutePosition() * physical::kWristEncoderGearRatio); // DataLogger::GetInstance().Send( "Arm/Left Arm Current", m_leftArm.GetOutputCurrent() ); // DataLogger::GetInstance().Send( "Arm/Right Arm Current", m_rightArm.GetOutputCurrent() ); diff --git a/src/main/cpp/subsystems/Limelight.cpp b/src/main/cpp/subsystems/Limelight.cpp index 6fe3488..241e1ca 100644 --- a/src/main/cpp/subsystems/Limelight.cpp +++ b/src/main/cpp/subsystems/Limelight.cpp @@ -61,7 +61,7 @@ bool Limelight::getFieldAprilTagPose( frc::Pose2d&april_tag_pose, units::second_ m_poseLogEntry.Append( { april_tag_pose.X().value(), april_tag_pose.Y().value(), - april_tag_pose.Rotation().Degrees().value() }, timestamp.value() ); + april_tag_pose.Rotation().Degrees().value() } ); return true; } diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index 270d68a..25d7bc2 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -92,7 +92,7 @@ namespace physical { // Absolute encoder offset for the arm encoder constexpr double kArmAbsoluteOffset = 0.495; - constexpr units::degree_t kWristAbsoluteOffset = -60.5_deg; + constexpr units::degree_t kWristAbsoluteOffset = -53.7_deg; // IsFinished condition for arm constexpr units::degree_t kArmAngleError = 3_deg; From 8dad030a8bad5d0bd72f2c2f3e83b0f55c0c01da Mon Sep 17 00:00:00 2001 From: stsmith306 Date: Mon, 13 Nov 2023 08:34:43 -0800 Subject: [PATCH 3/3] Post-comp commit --- src/main/cpp/RobotContainer.cpp | 12 ++++++---- .../cpp/commands/autonomous/DoNothingAuto.cpp | 23 +++++++++++++++++++ .../cpp/commands/autonomous/PlaceAtPose.cpp | 2 +- src/main/cpp/subsystems/ArmSubsystem.cpp | 12 ++++------ src/main/cpp/subsystems/Drivetrain.cpp | 1 + src/main/include/AbsoluteEncoder.h | 10 ++++++-- src/main/include/Constants.h | 8 +++---- src/main/include/RobotContainer.h | 2 ++ .../commands/autonomous/DoNothingAuto.h | 19 +++++++++++++++ 9 files changed, 70 insertions(+), 19 deletions(-) create mode 100644 src/main/cpp/commands/autonomous/DoNothingAuto.cpp create mode 100644 src/main/include/commands/autonomous/DoNothingAuto.h diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index a6f88fc..91192f9 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -47,9 +47,10 @@ RobotContainer::RobotContainer() { // Configure the button bindings ConfigureButtonBindings(); - m_chooser.SetDefaultOption(kBalance, kBalance); - m_chooser.AddOption(kLeave, kLeave); - m_chooser.AddOption(kPlaceOnly, kPlaceOnly); + m_chooser.SetDefaultOption( kBalance, kBalance ); + m_chooser.AddOption( kLeave, kLeave ); + m_chooser.AddOption( kPlaceOnly, kPlaceOnly ); + m_chooser.AddOption( kDoNothing, kDoNothing ); frc::SmartDashboard::PutData("Auto Modes", &m_chooser); } @@ -89,7 +90,7 @@ void RobotContainer::ConfigureButtonBindings() { m_operatorController.Y().OnTrue(ArmSet(&m_arm, physical::kArmUpperPlaceHeight, physical::kWristUpperPlaceHeight, 0.3, true).ToPtr()); - m_operatorController.A().OnTrue(ArmSet(&m_arm, -90_deg, 14_deg, 0.3, true).ToPtr()); + m_operatorController.A().OnTrue(ArmSet(&m_arm, -90_deg, 12_deg, 0.3, true).ToPtr()); m_operatorController.B().OnTrue(PlaceMid( &m_arm, &m_grabber ).ToPtr() ); @@ -115,6 +116,9 @@ frc2::Command* RobotContainer::GetAutonomousCommand() { else if (m_autoSelected == kPlaceOnly) { m_autoCommand = new PlaceOnlyAuto(&m_drive, &m_arm, &m_grabber); } + else if ( m_autoSelected == kDoNothing ) { + m_autoCommand = new DoNothingAuto( &m_drive, &m_arm, &m_grabber ); + } return m_autoCommand; } diff --git a/src/main/cpp/commands/autonomous/DoNothingAuto.cpp b/src/main/cpp/commands/autonomous/DoNothingAuto.cpp new file mode 100644 index 0000000..4160cda --- /dev/null +++ b/src/main/cpp/commands/autonomous/DoNothingAuto.cpp @@ -0,0 +1,23 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "commands/autonomous/DoNothingAuto.h" + +#include +#include +#include + +// NOTE: Consider using this command inline, rather than writing a subclass. +// For more information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +DoNothingAuto::DoNothingAuto( Drivetrain *drive, ArmSubsystem *arm, GrabberSubsystem *grabber ) { + // Add your commands here, e.g. + // AddCommands(FooCommand{}, BarCommand{}); + AddCommands( + frc2::WaitCommand(0.5_s), + frc2::InstantCommand([this, arm] {arm->ArmOn(-0.75, 0.5); }, { arm }), + frc2::WaitCommand(0.4_s), + frc2::InstantCommand([this, arm] {arm->ArmOn(0.0, 0.0); }, { arm }) + ); +} diff --git a/src/main/cpp/commands/autonomous/PlaceAtPose.cpp b/src/main/cpp/commands/autonomous/PlaceAtPose.cpp index 5cc61c2..ef46944 100644 --- a/src/main/cpp/commands/autonomous/PlaceAtPose.cpp +++ b/src/main/cpp/commands/autonomous/PlaceAtPose.cpp @@ -30,6 +30,6 @@ PlaceAtPose::PlaceAtPose( Drivetrain *drive, ArmSubsystem *arm, GrabberSubsystem frc2::InstantCommand( [this, grabber] { grabber->Spin( -0.35 ) ;}, { grabber } ), frc2::WaitCommand( 0.25_s ), frc2::InstantCommand( [this, grabber] { grabber->Spin( 0.0 ) ;}, { grabber } ), - ArmSet(arm, -118_deg, 61_deg, 0.3) + ArmSet(arm, -118_deg, 50_deg, 0.3) ); } diff --git a/src/main/cpp/subsystems/ArmSubsystem.cpp b/src/main/cpp/subsystems/ArmSubsystem.cpp index 7a14c84..9fa56bc 100644 --- a/src/main/cpp/subsystems/ArmSubsystem.cpp +++ b/src/main/cpp/subsystems/ArmSubsystem.cpp @@ -21,7 +21,7 @@ ArmSubsystem::ArmSubsystem() { m_wrist.EnableVoltageCompensation(12); m_wristEncoder.ConfigSensorDirection( true ); - m_wristEncoder.SetPosition( m_wristEncoder.GetAbsolutePosition() * physical::kWristEncoderGearRatio - physical::kWristAbsoluteOffset.value() ); + m_wristEncoder.SetPosition( m_wristEncoder.GetAbsolutePosition() - physical::kWristAbsoluteOffset.value() ); armAngle = GetArmAngle(); wristAngle = GetWristAngle(); @@ -50,19 +50,15 @@ void ArmSubsystem::Periodic() { return; } + m_armEncoder.SetPositionOffset(); armAngle = GetArmAngle(); wristAngle = GetWristAngle(); - if (armAngle > 180_deg) { - m_armEncoder.SetPositionNegative(); - } + // if (wristAngle > 180_deg) { // m_wristEncoder.SetPositionNegative(); // } - armAngle = GetArmAngle(); - wristAngle = GetWristAngle(); - // Holds arm in position after enabled if ( disabled && frc::DriverStation::IsEnabled() ) { m_wristSetpoint.position = wristAngle; @@ -219,7 +215,7 @@ void ArmSubsystem::ArmData() { 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.GetAbsolutePosition() * physical::kWristEncoderGearRatio); + frc::SmartDashboard::PutNumber("WristRawPosition", m_wristEncoder.GetAbsolutePosition()); // DataLogger::GetInstance().Send( "Arm/Left Arm Current", m_leftArm.GetOutputCurrent() ); // DataLogger::GetInstance().Send( "Arm/Right Arm Current", m_rightArm.GetOutputCurrent() ); diff --git a/src/main/cpp/subsystems/Drivetrain.cpp b/src/main/cpp/subsystems/Drivetrain.cpp index 7af41e7..39f78bf 100644 --- a/src/main/cpp/subsystems/Drivetrain.cpp +++ b/src/main/cpp/subsystems/Drivetrain.cpp @@ -98,6 +98,7 @@ void Drivetrain::DriveTrajectory( frc::Trajectory::State trajectoryState, const } void Drivetrain::StopDrive( ) { + ArcadeDrive( 0, 0, 0 ); m_frontLeft.StopMotors( ); m_backLeft.StopMotors( ); m_frontRight.StopMotors( ); diff --git a/src/main/include/AbsoluteEncoder.h b/src/main/include/AbsoluteEncoder.h index 7e7a1c6..51c6796 100644 --- a/src/main/include/AbsoluteEncoder.h +++ b/src/main/include/AbsoluteEncoder.h @@ -37,8 +37,14 @@ class AbsoluteEncoder{ return m_absoluteEncoder.GetAbsolutePosition(); } - void SetPositionNegative(void) { - position_offset = 360.0_deg; + void SetPositionOffset(void) { + while (GetCountingPosition() > 180_deg) { + position_offset += 360_deg; + } + while ( GetCountingPosition() < -180_deg ) { + position_offset -= 360_deg; + } + } bool IsConnected( void ) { diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index 25d7bc2..04ac7b1 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -75,7 +75,7 @@ namespace physical { constexpr double kTurnGearRatio = 12.8; // Gear ratio between the absolute encoder for the wrist and the wrist - constexpr double kWristEncoderGearRatio = 24.0 / 34.0; + constexpr double kWristEncoderGearRatio = 26.0 / 34.0; // The width of the drive base from the center of one module to another adjacent one. constexpr units::meter_t kDriveBaseWidth = 23.25_in; @@ -92,7 +92,7 @@ namespace physical { // Absolute encoder offset for the arm encoder constexpr double kArmAbsoluteOffset = 0.495; - constexpr units::degree_t kWristAbsoluteOffset = -53.7_deg; + constexpr units::degree_t kWristAbsoluteOffset = -70.0_deg; // IsFinished condition for arm constexpr units::degree_t kArmAngleError = 3_deg; @@ -100,7 +100,7 @@ namespace physical { constexpr units::inch_t kLimelightXAxisOffset = 325.61_in; constexpr units::inch_t kLimelightYAxisOffset = 157.8_in; - constexpr units::inch_t kPlaceDistance = 11_in; + constexpr units::inch_t kPlaceDistance = 9_in; constexpr units::degree_t kArmLowerPlaceHeight = -60_deg; constexpr units::degree_t kWristLowerPlaceHeight = 20_deg; @@ -111,7 +111,7 @@ namespace physical { constexpr units::degree_t kArmCubeMidPlaceHeight = -30_deg; constexpr units::degree_t kWristCubeMidPlaceHeight = 0_deg; - constexpr units::degree_t kArmSubstationMidPlaceHeight = 63_deg; + constexpr units::degree_t kArmSubstationMidPlaceHeight = 53_deg; constexpr units::degree_t kWristSubstationMidPlaceHeight = -175_deg; constexpr double kMidDelayProportion = 0.3; diff --git a/src/main/include/RobotContainer.h b/src/main/include/RobotContainer.h index 9ac2563..0562b38 100644 --- a/src/main/include/RobotContainer.h +++ b/src/main/include/RobotContainer.h @@ -20,6 +20,7 @@ #include "commands/autonomous/PlaceOnlyAuto.h" #include "commands/autonomous/BalanceAuto.h" #include "commands/autonomous/LeaveAuto.h" +#include "commands/autonomous/DoNothingAuto.h" #include "ControllerAxis.h" @@ -56,6 +57,7 @@ class RobotContainer { const std::string kBalance = "Place Cube and Balance"; const std::string kLeave = "Place Cube and Leave"; const std::string kPlaceOnly = "Just Place Cube"; + const std::string kDoNothing = "Do Nothing"; std::string m_autoSelected; frc::PS4Controller m_driverController{ 0 }; diff --git a/src/main/include/commands/autonomous/DoNothingAuto.h b/src/main/include/commands/autonomous/DoNothingAuto.h new file mode 100644 index 0000000..f8edda4 --- /dev/null +++ b/src/main/include/commands/autonomous/DoNothingAuto.h @@ -0,0 +1,19 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include + +#include "subsystems/Drivetrain.h" +#include "subsystems/ArmSubsystem.h" +#include "subsystems/GrabberSubsystem.h" + +class DoNothingAuto + : public frc2::CommandHelper { + public: + DoNothingAuto( Drivetrain *drive, ArmSubsystem *arm, GrabberSubsystem *grabber ); +};