Merge pull request #82 from PhyXTGears-programming/feature/hatch-pid

Feature/hatch pid

Author: boxofrox

src/main/cpp/Robot.cpp

@@ -11,12 +11,8 @@ CreeperClimb*   Robot::m_CreeperClimb;
 HatchMechanism* Robot::m_HatchMechanism;
 
 // Initialize Commands - Intake
-GrabHatchFromDispenser*       Robot::m_GrabHatchFromDispenser;
-ReleaseHatch*                 Robot::m_ReleaseHatch;
 RotateCargoForCargoShip*      Robot::m_RotateCargoForCargoShip;
 RotateCargoForLevelOneRocket* Robot::m_RotateCargoForLevelOneRocket;
-RotateHatchForFloor*          Robot::m_RotateHatchForFloor;
-RotateHatchForDispenser*      Robot::m_RotateHatchForDispenser;
 
 ShootCargoForCargoShip*      Robot::m_ShootCargoForCargoShip;
 ShootCargoForLevelOneRocket* Robot::m_ShootCargoForLevelOneRocket;
@@ -35,6 +31,13 @@ ClimbStep*       Robot::m_ClimbStep;
 DriveSandstormStepWithCargo* Robot::m_DriveSandstormStepWithCargo;
 DriveSandstormStepWithHatch* Robot::m_DriveSandstormStepWithHatch;
 
+// Initialize Commands - Hatch
+LowerHatch* Robot::m_LowerHatch;
+RaiseHatch* Robot::m_RaiseHatch;
+
+GrabHatchFromLoadingStation*  Robot::m_GrabHatchFromLoadingStation;
+ReadyHatch*                   Robot::m_ReadyHatch;
+
 // Initialize JSON reader
 wpi::json Robot::m_JsonConfig;
 
@@ -65,12 +68,8 @@ Robot::Robot() {
     m_DriveSandstormStepWithHatch = new DriveSandstormStepWithHatch();
 
     // Allocate and initialize commands - Intake
-    m_GrabHatchFromDispenser       = new GrabHatchFromDispenser();
-    m_ReleaseHatch                 = new ReleaseHatch();
     m_RotateCargoForCargoShip      = new RotateCargoForCargoShip();
     m_RotateCargoForLevelOneRocket = new RotateCargoForLevelOneRocket();
-    m_RotateHatchForFloor          = new RotateHatchForFloor();
-    m_RotateHatchForDispenser      = new RotateHatchForDispenser();
 
     m_ShootCargoForCargoShip      = new ShootCargoForCargoShip();
     m_ShootCargoForLevelOneRocket = new ShootCargoForLevelOneRocket();
@@ -84,6 +83,10 @@ Robot::Robot() {
     // Allocate and initialize commands -
     m_ReadyCreeperArm = new ReadyCreeperArm();
     m_ClimbStep       = new ClimbStep();
+
+    // Allocate and initialize commands - Hatch
+    m_GrabHatchFromLoadingStation  = new GrabHatchFromLoadingStation();
+    m_ReadyHatch                   = new ReadyHatch();
 }
 
 void Robot::RobotInit() {
@@ -150,6 +153,7 @@ void Robot::DisabledInit() {
 
     GetCreeperClimb().Disable();
     GetCargoIntake().Disable();
+    GetHatchMechanism().Disable();
 
     // Clear pending commands out of scheduler.
     // frc::Scheduler::GetInstance()->ResetAll();
@@ -165,6 +169,11 @@ void Robot::AutonomousInit() {
     GetDriveTrain().RunReset();
     GetCreeperClimb().RunReset();
     GetCargoIntake().RunReset();
+
+    // Some positions of hatch mechanism are compliant, so disable
+    // PID/rotation instead of RunReset.
+    GetHatchMechanism().StopRotation();
+
     m_CanSandstormStepDrive = true;
 
     m_OI.ClearButtonBuffer();
@@ -242,16 +251,33 @@ void Robot::CompetitionJoystickInput() {
         m_Bling.SetBling(m_Bling.CargoIntakePattern);
     }
 
-    if (console.GetHatchGrabReleased() || console.GetHatchReleaseReleased()) {
-        std::cout << "Comp Joy Input: Stop Hatch Rotate" << std::endl;
-        GetHatchMechanism().StopRotation();
-    } else if (console.GetHatchGrabPressed()) {
+    if (console.GetHatchGrabPressed()) {
         std::cout << "Comp Joy Input: Console: Hatch Grab Pressed" << std::endl;
-        GetHatchMechanism().RaiseHatch();
+        m_RaiseHatch
+          ->Until([]() { return Robot::m_OI.GetOperatorConsole().GetHatchGrabReleased(); })
+          ->Start();
         m_Bling.SetBling(m_Bling.HatchPattern);
     } else if (console.GetHatchReleasePressed()) {
         std::cout << "Comp Joy Input: Console: Hatch Release Pressed" << std::endl;
-        GetHatchMechanism().LowerHatch();
+        m_LowerHatch
+          ->Until([]() { return Robot::m_OI.GetOperatorConsole().GetHatchReleaseReleased(); })
+          ->Start();
+        m_Bling.SetBling(m_Bling.HatchPattern);
+    }
+
+    if (console.GetHatchTopPositionPressed()) {
+        std::cout << "Comp Joy Input: Console: Hatch Top Position Pressed" << std::endl;
+        m_GrabHatchFromLoadingStation->Start();
+        m_Bling.SetBling(m_Bling.HatchPattern);
+    } else if (console.GetHatchMidPositionPressed()) {
+        std::cout << "Comp Joy Input: Console: Hatch Mid Position Pressed" << std::endl;
+        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();
         m_Bling.SetBling(m_Bling.HatchPattern);
     }
 

src/main/cpp/commands/GrabHatchFromDispenser.cpp

@@ -0,0 +1,63 @@
+#include "commands/GrabHatchFromLoadingStation.h"
+#include "Robot.h"
+
+#include <iostream>
+using std::cout;
+using std::endl;
+
+/* GOAL:
+ *
+ * Lift hatch mechanism to top position and grip hatch when arm rises above
+ * threshold.
+ *
+ * Command start:
+ * - Lift hatch to top position with PID.
+ *
+ * Command execute:
+ * - When top position exceeds "grip threshold", then grab hatch. This ensures
+ *   the hatch is fully seated inside gripper.
+ *
+ * Command finished when:
+ *  - Arm reaches top position.
+ *
+ * Command end:
+ *  - Leave PID enabled to hold position.  Arm lifts to grab 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.
+ */
+
+GrabHatchFromLoadingStation::GrabHatchFromLoadingStation() {
+    Requires(&Robot::GetHatchMechanism());
+}
+
+void GrabHatchFromLoadingStation::Initialize() {
+    // Start moving to top position.
+    Robot::GetHatchMechanism().RotateToTopPosition();
+    // Assume gripper is open.
+    m_IsHatchGrabbed = false;
+}
+
+void GrabHatchFromLoadingStation::Execute() {
+    HatchMechanism &hatch = Robot::GetHatchMechanism();
+
+    if (!m_IsHatchGrabbed && hatch.IsAboveGripThreshold()) {
+        // Only grab once.
+        hatch.GrabHatch();
+        m_IsHatchGrabbed = true;
+    }
+}
+
+bool GrabHatchFromLoadingStation::IsFinished() {
+    return Robot::GetHatchMechanism().IsArmRotationDone();
+}
+
+void GrabHatchFromLoadingStation::End() {}
+
+void GrabHatchFromLoadingStation::Interrupted() {
+    Robot::GetHatchMechanism().StopRotation();
+}

src/main/cpp/commands/GrabHatchFromFloor.cpp

@@ -0,0 +1,53 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "commands/LowerHatch.h"
+#include "Robot.h"
+
+#include <iostream>
+
+LowerHatch::LowerHatch() {
+    Requires(&Robot::GetHatchMechanism());
+    m_IsFinishedCallback = nullptr;
+}
+
+// Called just before this Command runs the first time
+void LowerHatch::Initialize() {}
+
+// Called repeatedly when this Command is scheduled to run
+void LowerHatch::Execute() {
+    if (nullptr == m_IsFinishedCallback) {
+        std::cerr << "LowerHatch command activated without IsFinished callback." << std::endl;
+        return;
+    }
+
+    HatchMechanism& hatch = Robot::GetHatchMechanism();
+
+    hatch.LowerHatch();
+    hatch.ReleaseHatch();
+}
+
+// Make this return true when this Command no longer needs to run execute()
+bool LowerHatch::IsFinished() {
+    return nullptr == m_IsFinishedCallback || m_IsFinishedCallback();
+}
+
+// Called once after isFinished returns true
+void LowerHatch::End() {
+    Robot::GetHatchMechanism().StopRotation();
+}
+
+// Called when another command which requires one or more of the same
+// subsystems is scheduled to run
+void LowerHatch::Interrupted() {
+    Robot::GetHatchMechanism().StopRotation();
+}
+
+LowerHatch* LowerHatch::Until(std::function<bool(void)> callback) {
+    m_IsFinishedCallback = callback;
+    return this;
+}

src/main/cpp/commands/GrabHatchFromLoadingStation.cpp

@@ -0,0 +1,53 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "commands/RaiseHatch.h"
+#include "Robot.h"
+
+#include <iostream>
+
+RaiseHatch::RaiseHatch() {
+    Requires(&Robot::GetHatchMechanism());
+    m_IsFinishedCallback = nullptr;
+}
+
+// Called just before this Command runs the first time
+void RaiseHatch::Initialize() {}
+
+// Called repeatedly when this Command is scheduled to run
+void RaiseHatch::Execute() {
+    if (nullptr == m_IsFinishedCallback) {
+        std::cerr << "RaiseHatch command activated without IsFinished callback." << std::endl;
+        return;
+    }
+
+    HatchMechanism& hatch = Robot::GetHatchMechanism();
+
+    hatch.RaiseHatch();
+    hatch.GrabHatch();
+}
+
+// Make this return true when this Command no longer needs to run execute()
+bool RaiseHatch::IsFinished() {
+    return nullptr == m_IsFinishedCallback || m_IsFinishedCallback();
+}
+
+// Called once after isFinished returns true
+void RaiseHatch::End() {
+    Robot::GetHatchMechanism().StopRotation();
+}
+
+// Called when another command which requires one or more of the same
+// subsystems is scheduled to run
+void RaiseHatch::Interrupted() {
+    Robot::GetHatchMechanism().StopRotation();
+}
+
+RaiseHatch* RaiseHatch::Until(std::function<bool(void)> callback) {
+    m_IsFinishedCallback = callback;
+    return this;
+}