Merge pull request #91 from PhyXTGears-programming/develop

Develop

Author: boxofrox

src/main/cpp/Robot.cpp

@@ -44,6 +44,7 @@ RaiseHatch* Robot::m_RaiseHatch;
 
 GrabHatchFromLoadingStation*  Robot::m_GrabHatchFromLoadingStation;
 ReadyHatch*                   Robot::m_ReadyHatch;
+ReadyTakeHatch*               Robot::m_ReadyTakeHatch;
 
 // Initialize JSON reader
 wpi::json Robot::m_JsonConfig;
@@ -95,6 +96,7 @@ Robot::Robot() {
     // Allocate and initialize commands - Hatch
     m_GrabHatchFromLoadingStation  = new GrabHatchFromLoadingStation();
     m_ReadyHatch                   = new ReadyHatch();
+    m_ReadyTakeHatch               = new ReadyTakeHatch();
     m_RaiseHatch                   = new RaiseHatch();
     m_LowerHatch                   = new LowerHatch();
 }
@@ -122,7 +124,9 @@ void Robot::RobotPeriodic() {
     frc::SmartDashboard::PutNumber("climb stage", m_ClimbStep->GetSegment());
     frc::SmartDashboard::PutBoolean("climb ready", GetCreeperClimb().IsArmAtPosition("arm-ready"));
     frc::SmartDashboard::PutBoolean("climb done", GetCreeperClimb().IsArmAtPosition("arm-climb"));
-    frc::SmartDashboard::PutNumber("hatch anlge", GetHatchMechanism().GetArmPosition());
+    frc::SmartDashboard::PutNumber("hatch angle", GetHatchMechanism().GetArmPosition());
+
+    frc::SmartDashboard::PutString("drive mode", GetDriveTrain().GetIdleModeText());
 
     bool manualControl = abs(m_OI.GetOperatorConsole().GetThrottle()) >= 0.75;
     bool cargoControl = m_OI.GetOperatorConsole().GetThrottle() >= 0.75;
@@ -195,8 +199,6 @@ void Robot::AutonomousInit() {
     GetHatchMechanism().StopRotation();
     GetHatchMechanism().HoldPID();
 
-    m_CanSandstormStepDrive = true;
-
     m_OI.ClearButtonBuffer();
 }
 
@@ -205,8 +207,6 @@ void Robot::AutonomousPeriodic() {
 }
 
 void Robot::TeleopInit() {
-    m_CanSandstormStepDrive = false;
-
     m_OI.ClearButtonBuffer();
 }
 
@@ -230,10 +230,9 @@ void Robot::TestPeriodic() {}
 
 void Robot::CompetitionJoystickInput() {
     // DRIVER CONTROLS
-    if (m_OI.GetDriverJoystick().GetBButton() && m_CanSandstormStepDrive) {
+    if (m_OI.GetDriverJoystick().GetBButtonPressed()) {
         std::cout << "Comp Joy Input: Driver: B Button Down" << std::endl;
-        m_DriveSandstormStepWithCargo->Start();
-        m_CanSandstormStepDrive = false;
+        GetDriveTrain().ToggleIdleMode();
     }
 
     if (m_OI.GetDriverJoystick().GetAButtonPressed()) {
@@ -340,10 +339,8 @@ void Robot::CompetitionJoystickInput() {
         m_ReadyHatch->Start();
         m_Bling.SetBling(m_Bling.HatchPattern);
     } else if (console.GetHatchLowerPressed()) {
-        std::cout << "Comp Joy Input: Console: Hatch Lower Pressed" << std::endl;
-        m_LowerHatch
-          ->Until([]() { return Robot::m_OI.GetOperatorConsole().GetHatchLowerReleased(); })
-          ->Start();
+        std::cout << "Comp Joy Input: Console: Hatch Bottom Position Pressed" << std::endl;
+        m_ReadyTakeHatch->Start();
         m_Bling.SetBling(m_Bling.HatchPattern);
     }
 
@@ -406,6 +403,7 @@ void Robot::CompetitionJoystickInput() {
         m_Bling.SetBling(m_Bling.ClimbReady);
     } else if (console.GetCreeperClimbEnabled()) {
         std::cout << "Comp Joy Input: Console: Climb Sequence Pressed" << std::endl;
+        GetDriveTrain().SetIdleMode(IdleMode::kBrake);
         m_ClimbStep->Start();
         m_Bling.SetBling(m_Bling.Climb);
     }

src/main/cpp/commands/ClimbStep.cpp

@@ -111,8 +111,7 @@ void ClimbStep::Execute() {
         case Segment::RaiseCylinder:
             Robot::GetCreeperClimb().RotateArmToPosition("home");
             Robot::GetCreeperClimb().PistonRetract();
-            m_StopCylinderDelay.Start();
-            m_Segment = Segment::StopCylinder;
+            m_Segment = Segment::End;
             break;
         case Segment::StopCylinder:
             if (m_StopCylinderDelay.IsDone()) {

src/main/cpp/commands/ReadyTakeHatch.cpp

@@ -0,0 +1,54 @@
+#include "commands/ReadyTakeHatch.h"
+#include "Robot.h"
+
+#include <iostream>
+using std::cout;
+using std::endl;
+
+/* GOAL:
+ *
+ * Lift/lower hatch mechanism to bottom position and release hatch grip.
+ *
+ * Command start:
+ * - Lift/lower hatch to bottom position with PID.
+ * - Release hatch grip.
+ *
+ * Command execute:
+ *
+ * Command finished when:
+ *  - Arm reaches bottom position.
+ *
+ * Command end:
+ *  - Leave PID enabled to hold position.  Arm lifts/lowers to setpoint with
+ *    hatch and weight of hatch should not pull arm down.
+ *
+ * Command interrupted:
+ *  - Stop rotation.  Command was probably interrupted for a reason.
+ *
+ * Follow-up options:
+ *  - None.
+ */
+
+ReadyTakeHatch::ReadyTakeHatch() {
+    Requires(&Robot::GetHatchMechanism());
+}
+
+void ReadyTakeHatch::Initialize() {
+    HatchMechanism &hatch = Robot::GetHatchMechanism();
+
+    // Start moving to bottom position.
+    hatch.RotateToBottomPosition();
+    hatch.ReleaseHatch();
+}
+
+void ReadyTakeHatch::Execute() {}
+
+bool ReadyTakeHatch::IsFinished() {
+    return Robot::GetHatchMechanism().IsArmRotationDone();
+}
+
+void ReadyTakeHatch::End() {}
+
+void ReadyTakeHatch::Interrupted() {
+    Robot::GetHatchMechanism().StopRotation();
+}

src/main/cpp/subsystems/DriveTrain.cpp

@@ -5,46 +5,31 @@
 #include <frc/smartdashboard/SendableBuilder.h>
 
 #include <cmath>
+#include <iostream>
+#include <iomanip>
 
 DriveTrain::DriveTrain(wpi::json &jsonConfig) : frc::Subsystem("DriveTrain") {
 #   ifndef PROTOBOT
-        // Set up TalonSRXs.
-        m_MotorRightFront.ConfigFactoryDefault();
-        m_MotorRightBack.ConfigFactoryDefault();
-        m_MotorLeftFront.ConfigFactoryDefault();
-        m_MotorLeftBack.ConfigFactoryDefault();
-
-        int contLimit = 40;
-        int peakLimit = 50;
-        int peakTime = 1800;
-
-        m_MotorRightFront.EnableCurrentLimit(true);
-        m_MotorRightFront.ConfigContinuousCurrentLimit(contLimit, 10);
-        m_MotorRightFront.ConfigPeakCurrentLimit(peakLimit, 10);
-        m_MotorRightFront.ConfigPeakCurrentDuration(peakTime, 10);
-
-        m_MotorRightBack.EnableCurrentLimit(true);
-        m_MotorRightBack.ConfigContinuousCurrentLimit(contLimit, 10);
-        m_MotorRightBack.ConfigPeakCurrentLimit(peakLimit, 10);
-        m_MotorRightBack.ConfigPeakCurrentDuration(peakTime, 10);
-
-        m_MotorLeftFront.EnableCurrentLimit(true);
-        m_MotorLeftFront.ConfigContinuousCurrentLimit(contLimit, 10);
-        m_MotorLeftFront.ConfigPeakCurrentLimit(peakLimit, 10);
-        m_MotorLeftFront.ConfigPeakCurrentDuration(peakTime, 10);
-
-        m_MotorLeftBack.EnableCurrentLimit(true);
-        m_MotorLeftBack.ConfigContinuousCurrentLimit(contLimit, 10);
-        m_MotorLeftBack.ConfigPeakCurrentLimit(peakLimit, 10);
-        m_MotorLeftBack.ConfigPeakCurrentDuration(peakTime, 10);
-
-        // The documentation says to do this, so that both sides get the proper values.
-        // See https://phoenix-documentation.readthedocs.io/en/latest/ch15_WPIDrive.html?highlight=wpi_talon
-        m_MotorRightFront.SetInverted(false);
-        m_MotorRightBack.SetInverted(false);
-        m_MotorLeftFront.SetInverted(true);
-        m_MotorLeftBack.SetInverted(true);
+        // m_MotorRightFront.SetSmartCurrentLimit(45);
+        // m_MotorRightFront.SetSecondaryCurrentLimit(60);
 
+        // m_MotorRightBack.SetSmartCurrentLimit(45);
+        // m_MotorRightBack.SetSecondaryCurrentLimit(60);
+
+        // m_MotorLeftFront.SetSmartCurrentLimit(45);
+        // m_MotorLeftFront.SetSecondaryCurrentLimit(60);
+
+        // m_MotorLeftBack.SetSmartCurrentLimit(45);
+        // m_MotorLeftBack.SetSecondaryCurrentLimit(60);
+
+        m_MotorRight1.SetInverted(false);
+        m_MotorRight2.SetInverted(false);
+        m_MotorRight3.SetInverted(false);
+        m_MotorLeft1.SetInverted(true);
+        m_MotorLeft2.SetInverted(true);
+        m_MotorLeft3.SetInverted(true);
+
+        SetIdleMode(m_IdleMode);
 #   endif
 
     m_EncoderLeft.SetDistancePerPulse(kEncoderDistPerPulse);
@@ -54,7 +39,7 @@ DriveTrain::DriveTrain(wpi::json &jsonConfig) : frc::Subsystem("DriveTrain") {
         Subsystem::AddChild("Left Drive PID", &m_LeftPID);
         Subsystem::AddChild("Right Drive PID", &m_RightPID);
     #else
-        m_MaxAcceleration = jsonConfig["drive"]["max-acceleration"];
+        m_MinAcceleration = jsonConfig["drive"]["min-acceleration"];
 
         m_MaxNormalSpeed = jsonConfig["drive"]["max-normal-speed"];
         m_SandstormStepSpeed = jsonConfig["drive"]["sandstorm-step-speed"];
@@ -88,11 +73,11 @@ void DriveTrain::Drive(frc::XboxController& driver) {
     // Get left stick axes values.
     double hidX = -driver.GetX(frc::XboxController::kRightHand);
     double hidY = driver.GetY(frc::XboxController::kLeftHand);
-    double sprintFactor = 0.75;
+    double sprintFactor = 0.65;
     sprintFactor += driver.GetTriggerAxis(frc::XboxController::kRightHand) * 0.25;
-    sprintFactor -= driver.GetTriggerAxis(frc::XboxController::kLeftHand) * 0.25;
+    sprintFactor -= driver.GetTriggerAxis(frc::XboxController::kLeftHand) * 0.20;
 
-    double turnFactor = 1 - (driver.GetTriggerAxis(frc::XboxController::kLeftHand) * 0.3); // THIS
+    double turnFactor = 0.6 - (driver.GetTriggerAxis(frc::XboxController::kLeftHand) * 0.15); // THIS
 
     if (ENABLE_DRIVETRAIN_CONTROL) {
         ArcadeDrive(hidY * sprintFactor, hidX * turnFactor, true);
@@ -162,27 +147,7 @@ void DriveTrain::ArcadeDrive(double xSpeed, double zRotation, bool squareInputs)
         double currentLeft = m_LeftMotors.Get() / m_maxOutput;
         double currentRight = m_RightMotors.Get() / m_maxOutput;
 
-        double maxLeftAccel = m_MaxAcceleration, maxRightAccel = m_MaxAcceleration;
-
-        /* *
-        // Descrease the max velocity step size for the slower drive side so
-        // turning takes effect sooner.
-        //
-        // WARNING: Doesn't really work from initial testing.  Robot drives more straight.
-        if (0.01 < std::abs(desiredLeft) - std::abs(desiredRight)) {
-            // Left speed is faster than right.
-            maxLeftAccel = m_MaxAcceleration;
-            maxRightAccel = m_MaxAcceleration * desiredRight / desiredLeft;
-        } else if (0.01 < std::abs(desiredRight) - std::abs(desiredLeft)) {
-            // Right speed is faster than left.
-            maxLeftAccel = m_MaxAcceleration * desiredLeft / desiredRight;
-            maxRightAccel = m_MaxAcceleration;
-        } else {
-            // Both speeds are effectively same.
-            maxLeftAccel = m_MaxAcceleration;
-            maxRightAccel = m_MaxAcceleration;
-        }
-        /* */
+        double minLeftAccel = m_MinAcceleration, minRightAccel = m_MinAcceleration;
 
         double timeDelta = m_TimeDelta.Split();
 
@@ -194,17 +159,17 @@ void DriveTrain::ArcadeDrive(double xSpeed, double zRotation, bool squareInputs)
         //       v_{i+1} is the next velocity to send to the motors,
         //       a is the max acceleration, and
         //       dt is the time delta in seconds since the velocity step.
-        double allowedLeft = ComputeNextOutputDelta(
+        double allowedLeft = ComputeNextOutput(
             currentLeft,
             desiredLeft,
-            maxLeftAccel,
+            minLeftAccel,
             timeDelta
         );
 
-        double allowedRight = ComputeNextOutputDelta(
+        double allowedRight = ComputeNextOutput(
             currentRight,
             desiredRight,
-            maxRightAccel,
+            minRightAccel,
             timeDelta
         );
 
@@ -225,6 +190,38 @@ void DriveTrain::ArcadeDrive(double xSpeed, double zRotation, bool squareInputs)
     Feed();
 }
 
+void DriveTrain::SetIdleMode(rev::CANSparkMax::IdleMode mode) {
+    m_IdleMode = mode;
+
+    m_MotorRight1.SetIdleMode(mode);
+    m_MotorRight2.SetIdleMode(mode);
+    m_MotorRight3.SetIdleMode(mode);
+
+    m_MotorLeft1.SetIdleMode(mode);
+    m_MotorLeft2.SetIdleMode(mode);
+    m_MotorLeft3.SetIdleMode(mode);
+}
+
+void DriveTrain::ToggleIdleMode() {
+    if (IdleMode::kCoast == m_IdleMode) {
+        SetIdleMode(IdleMode::kBrake);
+    } else {
+        SetIdleMode(IdleMode::kCoast);
+    }
+}
+
+IdleMode DriveTrain::GetIdleMode() {
+    return m_IdleMode;
+}
+
+wpi::StringRef DriveTrain::GetIdleModeText() {
+    if (IdleMode::kCoast == m_IdleMode) {
+        return "Coast";
+    } else {
+        return "Brake";
+    }
+}
+
 void DriveTrain::StopMotor() {
     #ifdef USE_DRIVETRAIN_PID
         m_LeftPID.Disable();
@@ -301,7 +298,7 @@ void DriveTrain::UseDukesSpeedLimit() {
     #endif
 }
 
-double DriveTrain::ComputeNextOutputDelta(double iVel, double fVel, double maxAccel, double timeDelta) {
-    double deltaVel = fVel - iVel;
-    return iVel + std::copysign(std::min(std::abs(deltaVel), maxAccel * timeDelta), deltaVel);
+double DriveTrain::ComputeNextOutput(double iVel, double fVel, double minAccel, double timeDelta) {
+    double deltaVel = (fVel - iVel) * 0.4;
+    return iVel + std::copysign(std::min(std::fabs(deltaVel), minAccel), deltaVel);
 }

src/main/cpp/subsystems/HatchMechanism.cpp

@@ -24,6 +24,7 @@ HatchMechanism::HatchMechanism(wpi::json &jsonConfig) : Subsystem("HatchMechanis
 
     m_Config.MidPosition = jsonConfig["hatch"]["rotation"]["mid-position"];
     m_Config.TopPosition = jsonConfig["hatch"]["rotation"]["top-position"];
+    m_Config.BottomPosition = jsonConfig["hatch"]["rotation"]["bottom-position"];
     m_Config.GripThreshold = jsonConfig["hatch"]["rotation"]["grip-threshold"];
 
     AddChild("Hatch Arm Position", &m_ArmPosition);
@@ -43,6 +44,11 @@ void HatchMechanism::LowerHatch () {
     SetRotateSpeed(HatchGrabberSpeed);
 }
 
+void HatchMechanism::RotateToBottomPosition() {
+    m_ArmPID.SetSetpoint(m_Config.BottomPosition);
+    m_ArmPID.Enable();
+}
+
 void HatchMechanism::RotateToMidPosition() {
     m_ArmPID.SetSetpoint(m_Config.MidPosition);
     m_ArmPID.Enable();