Skip to content

Commit

Permalink
Fix timer for simulator, update number of motors, current to shuffleb…
Browse files Browse the repository at this point in the history 
…oard, update constants to ReCalc numbers
  • Loading branch information
@srgage
srgage committed Jan 18, 2025
1 parent dfc342d commit 82caf7b
Show file tree
Hide file tree
Showing 2 changed files with 35 additions and 24 deletions.
20 changes: 14 additions & 6 deletions src/main/cpp/subsystems/Elevator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,16 +16,16 @@ ElevatorSubsystem::ElevatorSubsystem()
ElevatorConstants::kP, ElevatorConstants::kI, ElevatorConstants::kD,
frc::TrapezoidProfile<units::meter>::Constraints(ElevatorConstants::kMaxVelocity,
ElevatorConstants::kMaxAcceleration),
5_ms))
20_ms))
, m_motor(ElevatorConstants::kMotorId, rev::CANSparkLowLevel::MotorType::kBrushless)
, m_encoder{m_motor.GetEncoder(rev::SparkRelativeEncoder::Type::kHallSensor,
ElevatorConstants::kEncoderPulsePerRev)}
, m_feedforwardElevator{ElevatorConstants::kFFks, ElevatorConstants::kFFkg,
ElevatorConstants::kFFkV, ElevatorConstants::kFFkA}
, m_elevatorSim(frc::DCMotor::NeoVortex(1), ElevatorConstants::kElevatorGearing,
ElevatorConstants::kCarriageMass, ElevatorConstants::kElevatorDrumRadius,
ElevatorConstants::simLowerLimit, ElevatorConstants::simUpperLimit, true, 0_m,
{0.01})
, m_elevatorSim(frc::DCMotor::NeoVortex(ElevatorConstants::kNumMotors),
ElevatorConstants::kElevatorGearing, ElevatorConstants::kCarriageMass,
ElevatorConstants::kElevatorDrumRadius, ElevatorConstants::simLowerLimit,
ElevatorConstants::simUpperLimit, false, 0_m, {0.001})
{
wpi::log::DataLog& log = frc::DataLogManager::GetLog();
m_HeightLog = wpi::log::DoubleLogEntry(log, "/Elevator/Angle");
Expand Down Expand Up @@ -114,6 +114,7 @@ void ElevatorSubsystem::printLog()
GetController().GetGoal().position.value());
frc::SmartDashboard::PutNumber("/Elevator/ELEVATOR_setpoint",
GetController().GetSetpoint().position.value());
frc::SmartDashboard::PutNumber("/Elevator/SimCurrent", m_elevatorSim.GetCurrentDraw().value());
m_HeightLog.Append(GetMeasurement().value());
m_SetPointLog.Append(GetController().GetSetpoint().position.value());
m_StateLog.Append(m_ElevatorState);
Expand Down Expand Up @@ -156,9 +157,16 @@ void ElevatorSubsystem::handle_Setpoint()
*/
void ElevatorSubsystem::Emergency_Stop() {}

void ElevatorSubsystem::SimulationInit()
{
m_simTimer.Restart();
}

void ElevatorSubsystem::SimulationPeriodic()
{
m_elevatorSim.Update(5_ms);
// m_elevatorSim.Update(m_simTimer.Get());
m_elevatorSim.Update(ElevatorConstants::kDt);
m_simTimer.Reset();
}
void ElevatorSubsystem::UseOutput(double output, State setpoint)
{
Expand Down
39 changes: 21 additions & 18 deletions src/main/include/subsystems/Elevator.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,38 +35,41 @@ enum ElevatorState
MANUAL
};

const auto upperLimit = 3_m;
const auto lowerLimit = 0_m;
const auto simUpperLimit = 3.05_m;
const auto simLowerLimit = -0.05_m;
static constexpr units::meters_per_second_t kMaxVelocity = 2.0_mps;
static constexpr units::meters_per_second_squared_t kMaxAcceleration = 1.0_mps_sq;
static constexpr double kP = 100.0; // 0.6
static constexpr double kI = 50.0; // 0.0
static constexpr double kD = 0.0;
static constexpr units::volt_t kS = 0.2_V; // minimum voltage to move motor
static constexpr units::meter_t upperLimit = 72_in;
static constexpr units::meter_t lowerLimit = 0_in;
static constexpr units::meter_t simUpperLimit = 72.1_in;
static constexpr units::meter_t simLowerLimit = -0.1_in;
static constexpr units::meters_per_second_t kMaxVelocity = 61.55_in / 1_s;
static constexpr units::meters_per_second_squared_t kMaxAcceleration =
460_in / (1_s * 1_s); // 12.0_V * 2.0_in / (1_s * 1_s * 1_V);
static constexpr double kP = 15.0; // 0.6
static constexpr double kI = 0.0; // 0.0
static constexpr double kD = 0.0;
static constexpr units::volt_t kS = 0.2_V; // minimum voltage to move motor

static constexpr units::meter_t kTolerancePos = 0.01_m;
static constexpr units::meters_per_second_t kToleranceVel = 0.05_mps;

const int kMotorId = 6;
const int kEncoderPulsePerRev = 42;

static constexpr auto kFFks = 0.23_V; // Volts static (motor)
static constexpr auto kFFkg = 0.0_V; // Volts
static constexpr auto kFFkV = 3.0_V / 1.0_mps; // volts*s/meters //1.01
static constexpr auto kFFkA = 1.0_V / 1.0_mps_sq; // volts*s^2/meters //0.1
static constexpr auto kFFks = 0.23_V; // Volts static (motor)
static constexpr auto kFFkg = 0.0_V; // Volts
static constexpr auto kFFkV = 0.5 * 2.32_V / 1.0_mps; // volts*s/meters //1.01
static constexpr auto kFFkA = 0.05_V / 1.0_mps_sq; // volts*s^2/meters //0.1

static constexpr units::second_t kDt = 20_ms;

// number of motors
static constexpr int kNumMotors = 2;
// gearing between motor and drum
static constexpr double kElevatorGearing = 10;
static constexpr double kElevatorGearing = 5;
// effective carriage mass: carriage mass = m
// maths: 1 stage = 1/1 * m1, 2 stage = 1/2 * m1 + 2/2 * m2, 3 stage = 1/3 * m1 + 2/3 * m2 + 3/3 *
// m3 highest number stage = carriage
static constexpr auto kCarriageMass = 5_kg;
static constexpr units::kilogram_t kCarriageMass = (17.5_lb + 0.5 * 5_lb);
// effective drum radius = radius of first stage * number of stages
static constexpr auto kElevatorDrumRadius = 0.1_m;
static constexpr units::meter_t kElevatorDrumRadius = 1.432_in * 2;
}

class ElevatorSubsystem : public frc2::ProfiledPIDSubsystem<units::meter>
Expand All @@ -78,6 +81,7 @@ class ElevatorSubsystem : public frc2::ProfiledPIDSubsystem<units::meter>
void printLog();
// void handle_Setpoint();
void Emergency_Stop();
void SimulationInit();
void SimulationPeriodic();
double GetHeight();
void UseOutput(double output, State setpoint) override;
Expand All @@ -99,7 +103,6 @@ class ElevatorSubsystem : public frc2::ProfiledPIDSubsystem<units::meter>
wpi::log::IntegerLogEntry m_StateLog;
wpi::log::DoubleLogEntry m_MotorCurrentLog;
wpi::log::DoubleLogEntry m_MotorVoltageLog;
frc::Timer* m_timer;
rev::SparkRelativeEncoder m_encoder;

frc::Timer m_simTimer;
Expand Down

0 comments on commit 82caf7b

Please sign in to comment.