work in progress

src/main/cpp/subsystems/Elevator.cpp

@@ -0,0 +1,95 @@
+/*
+https://docs.wpilib.org/en/stable/docs/software/wpilib-tools/robot-simulation/physics-sim.html
+https://docs.wpilib.org/en/stable/docs/software/wpilib-tools/robotbuilder/advanced/robotbuilder-writing-pidsubsystem-code.html
+https://github.wpilib.org/allwpilib/docs/release/cpp/classfrc_1_1sim_1_1_elevator_sim.html
+*/
+
+#include "subsystems/Elevator.h"
+
+using State = frc::TrapezoidProfile<units::meters>::State;
+using mps_sq_t =
+    units::unit_t<units::compound_unit<units::velocity::mps, units::inverse<units::time::seconds>>>;
+
+ElevatorSubsystem::ElevatorSubsystem()
+    : frc2::ProfiledPIDSubsystem<units::meter>(frc::ProfiledPIDController<units::meter>(
+          ElevatorConstants::kP, ElevatorConstants::kI, ElevatorConstants::kD,
+          frc::TrapezoidProfile<units::meter>::Constraints(ElevatorConstants::kMaxVelocity,
+                                                           ElevatorConstants::kMaxAcceleration),
+          5_ms))
+    , m_motor(ElevatorConstants::kAngleMotorId, rev::CANSparkLowLevel::MotorType::kBrushless)
+    , m_feedforward(ElevatorConstants::kFFks, ElevatorConstants::kFFkg, ElevatorConstants::kFFkV,
+                    ElevatorConstants::kFFkA)
+    , Linear{1}
+    , m_encoder{m_motor.GetEncoder(rev::SparkRelativeEncoder::Type::kHallSensor,
+                                   ElevatorConstants::kAngleEncoderPulsePerRev)}
+    , m_elevatorSim(m_elevatorGearbox, ElevatorConstants::kElevatorGearing,
+                    ElevatorConstants::kCarriageMass, ElevatorConstants::kElevatorDrumRadius,
+                    ElevatorConstants::lowerLimit, ElevatorConstants::upperLimit, true, 0_m, {0.01})
+{
+    m_encoder.SetDistancePerPulse(0.0);
+    // m_encoder.Reset();
+    ElevatorConstants::m_holdHeight    = 0.0;
+    ElevatorConstants::m_ElevatorState = ElevatorConstants::ElevatorState::HOLD;
+}
+
+void Elevator::Periodic()
+{
+    // This method will be called once per scheduler run.
+    switch(m_ElevatorState)
+    {
+        case ElevatorConstants::LIFT:
+            break;
+        case ElevatorConstants::LOWER:
+            break;
+        case ElevatorConstants::HOLD:
+            break;
+    }
+}
+
+void Elevator::SetHeight(double height)
+{
+    if(m_ElevatorState != LIFT && m_ElevatorState != LOWER)
+    {
+        if(GetHeight() > height)
+        {
+            m_ElevatorState = LOWER;
+            m_controller.Reset(units::meter_t{GetHeight()});
+            m_controller.SetTolerance(kTolerancePos, kToleranceVel);
+            m_controller.SetGoal(units::meter_t{height});
+        }
+        else if(GetHeight() < height)
+        {
+            m_ElevatorState = LIFT;
+            m_controller.Reset(units::meter_t{GetHeight()});
+            m_controller.SetTolerance(kTolerancePos, kToleranceVel);
+            m_controller.SetGoal(units::meter_t{height});
+        }
+        else
+        {
+            m_ElevatorState = HOLD;
+        }
+    }
+}
+
+void Elevator::SetSpeed(double speed)
+{
+    if(m_state == MANUAL_MOVING)
+    {
+        m_linearMotor.Set(speed);
+    }
+}
+
+double Elevator::GetHeight()
+{
+    return m_linearEncoder.GetDistance() + m_offset;
+}
+
+bool Elevator::CheckGoal()
+{
+    return m_state == HOLD;
+}
+
+void Elevator::printLog() {}
+void Elevator::handle_Setpoint() {}
+void Elevator::Emergency_Stop() {}
+void Elevator::SimulationPeriodic() {}

src/main/include/subsystems/Elevator.h

@@ -0,0 +1,101 @@
+#pragma once
+
+#include "frc/DataLogManager.h"
+#include "wpi/DataLog.h"
+#include <ctre/phoenix6/Pigeon2.hpp>
+#include <ctre/phoenix6/TalonFX.hpp>
+#include <frc/DutyCycleEncoder.h>
+#include <frc/Encoder.h>
+#include <frc/controller/ArmFeedforward.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <frc2/command/ProfiledPIDSubsystem.h>
+#include <rev/CANSparkFlex.h>
+#include <units/angle.h>
+#include <units/time.h>
+
+#include "Constants.hpp"
+#include <frc/DigitalInput.h>
+#include <frc/RobotBase.h>
+#include <frc/Servo.h>
+#include <frc/Timer.h>
+#include <frc/simulation/ElevatorSim.h>
+#include <frc/simulation/SimDeviceSim.h>
+
+namespace ElevatorConstants
+{
+
+enum ElevatorState
+{
+    LIFT,
+    LOWER,
+    HOLD
+};
+
+const double                                        upperLimit       = 10; // in meters
+const double                                        lowerLimit       = 0;  // in meters
+static constexpr units::meters_per_second_t         kMaxVelocity     = 0.1_mps;
+static constexpr units::meters_per_second_squared_t kMaxAcceleration = 0.025_mps_sq;
+static constexpr double                             kP               = 10.0;
+static constexpr double                             kI               = 10.0;
+static constexpr double                             kD               = 0.0;
+static constexpr units::volt_t                      kS = 0.2_V; // minimum voltage to move motor
+static constexpr auto                               kV = 0.0_V / 0.28_mps;
+static constexpr auto                               kA = 0.0_V / 0.28_mps_sq;
+
+static constexpr units::meter_t             kTolerancePos = 0.001_m;
+static constexpr units::meters_per_second_t kToleranceVel = 0.001_mps;
+
+const auto kFFks = units::volt_t(0.23);                                    // Volts static (motor)
+const auto kFFkg = units::volt_t(0.28);                                    // Volts
+const auto kFFkV = units::unit_t<frc::ElevatorFeedforward::kv_unit>(1.01); // volts*s/rad
+const auto kFFkA = units::unit_t<frc::ElevatorFeedforward::ka_unit>(0.01); // volts*s^2/rad
+
+static constexpr units::second_t kDt = 20_ms;
+
+frc::TrapezoidProfile<units::meters>::Constraints m_constraints{kMaxVelocity, kMaxAcceleration};
+
+frc::ProfiledPIDController<units::meters>  m_controller{kP, kI, kD, m_constraints, kDt};
+frc::SimpleMotorFeedforward<units::meters> m_feedforward{kS, kV, kA};
+
+double m_holdHeight;
+
+ElevatorConstants::ElevatorState m_ElevatorState;
+
+double m_elevatorGearbox;
+double kElevatorGearing;
+double kCarriageMass;
+double kElevatorDrumRadius;
+}
+
+class ElevatorSubsystem : public frc2::ProfiledPIDSubsystem<units::meter>
+{
+    using State = frc::TrapezoidProfile<units::degrees>::State;
+
+public:
+    ElevatorSubsystem();
+    void printLog();
+    void handle_Setpoint();
+    void Emergency_Stop();
+    void SimulationPeriodic();
+    void GetHeight();
+    void SetSpeed(double speed);
+    void SetHeight(double height);
+    void CheckGoal();
+
+private:
+    rev::CANSparkFlex                 m_motor;
+    frc::ArmFeedforward               m_feedforward;
+    wpi::log::DoubleLogEntry          m_HeightLog;
+    wpi::log::DoubleLogEntry          m_SetPointLog;
+    wpi::log::IntegerLogEntry         m_StateLog;
+    wpi::log::DoubleLogEntry          m_MotorCurrentLog;
+    wpi::log::DoubleLogEntry          m_MotorVoltageLog;
+    ctre::phoenix6::hardware::Pigeon2 arm_pigeon{9}; //, "NKCANivore"};
+    frc::Timer*                       m_timer;
+    frc::PWM                          Linear;
+    rev::SparkRelativeEncoder         m_encoder;
+
+    frc::Timer m_simTimer;
+
+    frc::sim::ElevatorSim m_elevatorSim;
+};