Merge branch 'main' of https://github.com/4698RaiderRobotics/FRC-2023

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;
 }

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(), 

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 <frc2/command/WaitCommand.h>
+#include <frc2/command/InstantCommand.h>
+#include <frc2/command/ParallelCommandGroup.h>
+
+// 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 })
+  );
+}

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)
   );
 }

src/main/cpp/subsystems/ArmSubsystem.cpp

@@ -6,6 +6,8 @@
 #include <frc/DriverStation.h>
 #include <frc/Timer.h>
 
+#include "DataLogger.h"
+
 ArmSubsystem::ArmSubsystem() {
     m_rightArm.SetInverted(true);
     m_rightArm.Follow(m_leftArm);
@@ -19,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();
@@ -48,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;
@@ -217,7 +215,14 @@ 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());
+
+    // 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() {

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( );

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;
 }

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 ) {

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,15 +92,15 @@ 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 = -70.0_deg;
 
     // IsFinished condition for arm
     constexpr units::degree_t kArmAngleError = 3_deg;
 
     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; 
 

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 };

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 <frc2/command/CommandHelper.h>
+#include <frc2/command/SequentialCommandGroup.h>
+
+#include "subsystems/Drivetrain.h"
+#include "subsystems/ArmSubsystem.h"
+#include "subsystems/GrabberSubsystem.h"
+
+class DoNothingAuto
+    : public frc2::CommandHelper<frc2::SequentialCommandGroup,
+                                 DoNothingAuto> {
+ public:
+  DoNothingAuto( Drivetrain *drive, ArmSubsystem *arm, GrabberSubsystem *grabber );
+};