From c3c212b0ee78f5ffe59dd85a5978712e4aca60ca Mon Sep 17 00:00:00 2001 From: Sarah Gage Date: Thu, 2 Jan 2025 16:07:50 -0500 Subject: [PATCH] work in progress --- src/main/cpp/subsystems/Elevator.cpp | 95 +++++++++++++++++++++++ src/main/include/subsystems/Elevator.h | 101 +++++++++++++++++++++++++ 2 files changed, 196 insertions(+) create mode 100644 src/main/cpp/subsystems/Elevator.cpp create mode 100644 src/main/include/subsystems/Elevator.h diff --git a/src/main/cpp/subsystems/Elevator.cpp b/src/main/cpp/subsystems/Elevator.cpp new file mode 100644 index 0000000..b986f15 --- /dev/null +++ b/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::State; +using mps_sq_t = + units::unit_t>>; + +ElevatorSubsystem::ElevatorSubsystem() + : frc2::ProfiledPIDSubsystem(frc::ProfiledPIDController( + ElevatorConstants::kP, ElevatorConstants::kI, ElevatorConstants::kD, + frc::TrapezoidProfile::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() {} \ No newline at end of file diff --git a/src/main/include/subsystems/Elevator.h b/src/main/include/subsystems/Elevator.h new file mode 100644 index 0000000..0f10edf --- /dev/null +++ b/src/main/include/subsystems/Elevator.h @@ -0,0 +1,101 @@ +#pragma once + +#include "frc/DataLogManager.h" +#include "wpi/DataLog.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "Constants.hpp" +#include +#include +#include +#include +#include +#include + +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(1.01); // volts*s/rad +const auto kFFkA = units::unit_t(0.01); // volts*s^2/rad + +static constexpr units::second_t kDt = 20_ms; + +frc::TrapezoidProfile::Constraints m_constraints{kMaxVelocity, kMaxAcceleration}; + +frc::ProfiledPIDController m_controller{kP, kI, kD, m_constraints, kDt}; +frc::SimpleMotorFeedforward 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 +{ + using State = frc::TrapezoidProfile::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; +}; \ No newline at end of file