Merge pull request #89 from PhyXTGears-programming/competition/indy-robot-inv

boxofrox · web-flow · commit f2be3386e6b5 · 2019-07-24T12:52:14.000-04:00
Competition/indy robot inv
diff --git a/src/main/cpp/Robot.cpp b/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();
 }
@@ -228,7 +230,7 @@ void Robot::TestPeriodic() {}
 
 void Robot::CompetitionJoystickInput() {
     // DRIVER CONTROLS
-    if (m_OI.GetDriverJoystick().GetBButton()) {
+    if (m_OI.GetDriverJoystick().GetBButtonPressed()) {
         std::cout << "Comp Joy Input: Driver: B Button Down" << std::endl;
         GetDriveTrain().ToggleIdleMode();
     }
@@ -337,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);
     }
 
diff --git a/src/main/cpp/commands/ReadyTakeHatch.cpp b/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();
+}
diff --git a/src/main/cpp/subsystems/DriveTrain.cpp b/src/main/cpp/subsystems/DriveTrain.cpp
@@ -39,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"];
@@ -147,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();
 
@@ -182,14 +162,14 @@ void DriveTrain::ArcadeDrive(double xSpeed, double zRotation, bool squareInputs)
         double allowedLeft = ComputeNextOutput(
             currentLeft,
             desiredLeft,
-            maxLeftAccel,
+            minLeftAccel,
             timeDelta
         );
 
         double allowedRight = ComputeNextOutput(
             currentRight,
             desiredRight,
-            maxRightAccel,
+            minRightAccel,
             timeDelta
         );
 
@@ -318,7 +298,7 @@ void DriveTrain::UseDukesSpeedLimit() {
     #endif
 }
 
-double DriveTrain::ComputeNextOutput(double iVel, double fVel, double maxAccel, double timeDelta) {
-    double deltaVel = fVel - iVel;
-    return iVel + deltaVel / 2.0;
+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);
 }
diff --git a/src/main/cpp/subsystems/HatchMechanism.cpp b/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();
diff --git a/src/main/deploy/compbot-config.json b/src/main/deploy/compbot-config.json
@@ -28,13 +28,13 @@
         "rotation": {
             "zero-point": 0,
             "angles": {
-                "home":                   0.73,
-                "cargo-ship-shoot":       0.73,
-                "rocket-1-shoot":         0.41,
-                "rocket-2-shoot":         0.73,
-                "cargo-floor-pickup":     0.02,
-                "hatch-dispenser-pickup": 0.73,
-                "hatch-floor-pickup":     0.73
+                "home":                   0.65,
+                "cargo-ship-shoot":       0.65,
+                "rocket-1-shoot":         0.33,
+                "rocket-2-shoot":         0.65,
+                "cargo-floor-pickup":     0.00,
+                "hatch-dispenser-pickup": 0.65,
+                "hatch-floor-pickup":     0.65
             }
         },
         "roller-speed": {
@@ -45,7 +45,7 @@
         }
     },
     "drive": {
-        "max-acceleration": 12.0,
+        "min-acceleration": 1.0,
         "max-normal-speed": 1,
         "sandstorm-step-speed": 0.45,
         "turn-factor": 1
@@ -60,7 +60,7 @@
             "zero-point": 0,
             "angles": {
                 "home":      80,
-                "arm-ready": 161,
+                "arm-ready": 163,
                 "arm-climb": 230
             }
         }
@@ -75,7 +75,8 @@
         "rotation": {
             "grip-threshold": 0.95,
             "top-position": 0.69,
-            "mid-position": 1.50
+            "mid-position": 1.50,
+            "bottom-position": 2.40
         }
     }
 }
diff --git a/src/main/include/Bling.h b/src/main/include/Bling.h
@@ -22,7 +22,7 @@ class Bling {
 
     private:
         const double m_PwmMap [SizeValue] = {
-            [Default]                   = -0.91, // Rainbow Forest Palette
+            [Default]                   = -0.99, // Rainbow Forest Palette
             [CargoIntakePattern]        =  0.65, // Orange
             [CargoShootShipPattern]     = -0.39, // Color Waves, Lava Palette
             [CargoShootRocketPattern]   = -0.57, // Large Fire
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
@@ -33,6 +33,7 @@
 #include "commands/RaiseHatch.h"
 #include "commands/ReadyCreeperArm.h"
 #include "commands/ReadyHatch.h"
+#include "commands/ReadyTakeHatch.h"
 #include "commands/RotateCargoForCargoShip.h"
 #include "commands/RotateCargoForLevelOneRocket.h"
 #include "commands/ShootCargoForCargoShip.h"
@@ -88,6 +89,7 @@ class Robot : public frc::TimedRobot {
 
         static GrabHatchFromLoadingStation*   m_GrabHatchFromLoadingStation;
         static ReadyHatch*                    m_ReadyHatch;
+        static ReadyTakeHatch*                m_ReadyTakeHatch;
 
         // JSON Reader for Config (this should probably be moved later)
         static wpi::json m_JsonConfig;
diff --git a/src/main/include/commands/ReadyTakeHatch.h b/src/main/include/commands/ReadyTakeHatch.h
@@ -0,0 +1,13 @@
+#pragma once
+
+#include <frc/commands/Command.h>
+
+class ReadyTakeHatch : public frc::Command {
+    public:
+        ReadyTakeHatch();
+        void Initialize() override;
+        void Execute() override;
+        bool IsFinished() override;
+        void End() override;
+        void Interrupted() override;
+};
diff --git a/src/main/include/subsystems/DriveTrain.h b/src/main/include/subsystems/DriveTrain.h
@@ -60,7 +60,7 @@ class DriveTrain : public frc::Subsystem, public frc::RobotDriveBase {
         MOTORTYPE m_MotorLeft2 {kLeftMotor2, rev::CANSparkMax::MotorType::kBrushless};
         MOTORTYPE m_MotorLeft3 {kLeftMotor3, rev::CANSparkMax::MotorType::kBrushless};
 
-        IdleMode m_IdleMode {IdleMode::kCoast};
+        IdleMode m_IdleMode {IdleMode::kBrake};
 
         // Group the motors into their sides and then combine them into the drivetrain
         frc::SpeedControllerGroup m_RightMotors {m_MotorRight1, m_MotorRight2, m_MotorRight3};
diff --git a/src/main/include/subsystems/HatchMechanism.h b/src/main/include/subsystems/HatchMechanism.h
@@ -23,6 +23,7 @@ class HatchMechanism : public frc::Subsystem {
         // Use PID to hold position.
         void RotateToTopPosition();
         void RotateToMidPosition();
+        void RotateToBottomPosition();
 
         // Stop moving arm.  Zero speed, disable PID.
         void StopRotation();
@@ -55,6 +56,7 @@ class HatchMechanism : public frc::Subsystem {
         struct Config {
             double TopPosition = 0.0;
             double MidPosition = 0.0;
+            double BottomPosition = 0.0;
             double GripThreshold = 0.0;
         };
 
