From 1f480606b942f2075ffca02084f82691fe39473e Mon Sep 17 00:00:00 2001 From: j a Date: Wed, 8 Mar 2023 18:00:44 -0600 Subject: [PATCH] fix: valor controller stores pidf and profiles are setable. new elevarm auto command to set arm pidf --- src/main/cpp/Robot.cpp | 2 +- src/main/cpp/auto/ValorAuto.cpp | 38 ++++++++++--------- src/main/cpp/auto/ValorAutoAction.cpp | 13 +++++-- .../cpp/controllers/ValorFalconController.cpp | 28 +++++++++----- .../cpp/controllers/ValorNeoController.cpp | 29 +++++++------- src/main/cpp/subsystems/Elevarm.cpp | 15 ++++---- src/main/include/auto/ValorAutoAction.h | 1 + .../include/controllers/ValorController.h | 12 ++++++ .../controllers/ValorFalconController.h | 5 +-- .../include/controllers/ValorNeoController.h | 4 -- src/main/include/subsystems/Elevarm.h | 4 +- 11 files changed, 87 insertions(+), 64 deletions(-) diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index 64297829..50c3df4e 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -83,7 +83,7 @@ void Robot::AutonomousPeriodic(){ void Robot::TeleopInit() { drivetrain.pullSwerveModuleZeroReference(); drivetrain.setDriveMotorNeutralMode(ValorNeutralMode::Coast); - elevarm.setArmPIDF(false); + elevarm.setArmPIDF(0); if (autoCommand != nullptr) { autoCommand->Cancel(); diff --git a/src/main/cpp/auto/ValorAuto.cpp b/src/main/cpp/auto/ValorAuto.cpp index 7931a05f..809b2b69 100644 --- a/src/main/cpp/auto/ValorAuto.cpp +++ b/src/main/cpp/auto/ValorAuto.cpp @@ -145,8 +145,6 @@ frc2::SequentialCommandGroup* ValorAuto::makeAuto(std::string filename, bool blu drivetrain->setAutoMaxAcceleration(NULL, 1.0); - currentGroup->AddCommands(std::move(*elevarm->getRotatePIDSetterCommand(true))); - for (int i = 0; i < actions.size(); i ++){ table->PutString("Action " + std::to_string(i), commandToStringMap[actions[i].type]); ValorAutoAction & action = actions[i]; @@ -347,20 +345,28 @@ frc2::SequentialCommandGroup* ValorAuto::makeAuto(std::string filename, bool blu drivetrain->setAutoMaxAcceleration(action.maxAccel, action.accelMultiplier); } else if (action.type == ValorAutoAction::ELEVARM){ - Elevarm::ElevarmPieceState pieceState = elevarm->stringToPieceState(action.values[0]); - Elevarm::ElevarmDirectionState directionState = elevarm->stringToDirectionState(action.values[1]); - Elevarm::ElevarmPositionState positionState = elevarm->stringToPositionState(action.values[2]); + if (action.values[0] == "pidf") { + if (action.values[1] == "arm") { + currentGroup->AddCommands(std::move(*elevarm->getRotatePIDSetterCommand(action.slot))); + } + } else { + Elevarm::ElevarmPieceState pieceState = elevarm->stringToPieceState(action.values[0]); + Elevarm::ElevarmDirectionState directionState = elevarm->stringToDirectionState(action.values[1]); + Elevarm::ElevarmPositionState positionState = elevarm->stringToPositionState(action.values[2]); + + if (!elevarmTable->GetBoolean("Pit Mode", false)) + currentGroup->AddCommands( + std::move(*elevarm->getAutoCommand( + action.values[0], + action.values[1], + action.values[2], + action.parallel + )) + ); + table->PutBoolean("Pit mode enabled for elevarm", elevarmTable->GetBoolean("Pit Mode", false)); + } - if (!elevarmTable->GetBoolean("Pit Mode", false)) - currentGroup->AddCommands( - std::move(*elevarm->getAutoCommand( - action.values[0], - action.values[1], - action.values[2], - action.parallel - )) - ); - table->PutBoolean("Pit mode enabled for elevarm", elevarmTable->GetBoolean("Pit Mode", false)); + table->PutBoolean("Action " + std::to_string(i) + " parallel", action.parallel); } else if (action.type == ValorAutoAction::BALANCE){ @@ -430,8 +436,6 @@ frc2::SequentialCommandGroup* ValorAuto::makeAuto(std::string filename, bool blu ); } - combinedGroup->AddCommands(std::move(*elevarm->getRotatePIDSetterCommand(false))); - return combinedGroup; } diff --git a/src/main/cpp/auto/ValorAutoAction.cpp b/src/main/cpp/auto/ValorAutoAction.cpp index e587d3e1..6f642bad 100644 --- a/src/main/cpp/auto/ValorAutoAction.cpp +++ b/src/main/cpp/auto/ValorAutoAction.cpp @@ -153,11 +153,16 @@ ValorAutoAction::ValorAutoAction(std::string line, std::map= 5) - parallel = items[4] == "parallel"; + if (items[1] == "pidf") { + values = {items[1], items[2]}; + slot = stod(items[3]); + } else { + values = {items[1], items[2], items[3]}; + if (items.size() >= 5) + parallel = items[4] == "parallel"; + } } else if (type == ValorAutoAction::Type::ACCELERATION) { if (items.size() == 2){ maxAccel = stod(items[1]); diff --git a/src/main/cpp/controllers/ValorFalconController.cpp b/src/main/cpp/controllers/ValorFalconController.cpp index e24054db..691a3d09 100644 --- a/src/main/cpp/controllers/ValorFalconController.cpp +++ b/src/main/cpp/controllers/ValorFalconController.cpp @@ -29,6 +29,7 @@ void ValorFalconController::init() ValorPIDF motionPIDF; setPIDF(motionPIDF, 0); + setProfile(0); reset(); wpi::SendableRegistry::AddLW(this, "ValorFalconController", "ID " + std::to_string(motor->GetDeviceID())); @@ -68,17 +69,17 @@ void ValorFalconController::setReverseLimit(double reverse) motor->ConfigReverseSoftLimitEnable(true); } -void ValorFalconController::setPIDF(ValorPIDF _pidf, int slot) +void ValorFalconController::setPIDF(ValorPIDF pidf, int slot) { - pidf = _pidf; motor->Config_kP(slot, pidf.P); motor->Config_kI(slot, pidf.I); motor->Config_kD(slot, pidf.D); motor->Config_kF(slot, pidf.F * (1023.0 / 7112.0)); motor->ConfigAllowableClosedloopError(slot, pidf.error * FALCON_TICKS_PER_REV / conversion); - double vel = pidf.velocity / 10.0 * FALCON_TICKS_PER_REV / conversion; - motor->ConfigMotionCruiseVelocity(vel); - motor->ConfigMotionAcceleration(vel / pidf.acceleration); + + pidf.velocity = pidf.velocity / 10.0 * FALCON_TICKS_PER_REV / conversion; + + pidfs.insert_or_assign(slot, pidf); } void ValorFalconController::setConversion(double _conversion) @@ -111,10 +112,11 @@ void ValorFalconController::setRange(int slot, double min, double max) void ValorFalconController::setPosition(double position) { - if (pidf.aFF != 0) { - double horizontalOffset = getPosition() - pidf.aFFTarget; + + if (pidfs.at(currentPIDSlot).aFF != 0) { + double horizontalOffset = getPosition() - pidfs.at(currentPIDSlot).aFFTarget; double scalar = std::cos(horizontalOffset * M_PI / 180.0); - motor->Set(ControlMode::MotionMagic, position / conversion * FALCON_TICKS_PER_REV, DemandType_ArbitraryFeedForward, scalar * pidf.aFF); + motor->Set(ControlMode::MotionMagic, position / conversion * FALCON_TICKS_PER_REV, DemandType_ArbitraryFeedForward, scalar * pidfs.at(currentPIDSlot).aFF); } else motor->Set(ControlMode::MotionMagic, position / conversion * FALCON_TICKS_PER_REV); } @@ -129,9 +131,15 @@ void ValorFalconController::setPower(double speed) motor->Set(ControlMode::PercentOutput, speed); } -void ValorFalconController::setProfile(int profile) +void ValorFalconController::setProfile(int slot) { - motor->SelectProfileSlot(profile, 0); + currentPIDSlot = slot; + ValorPIDF pidf = pidfs.at(currentPIDSlot); + + motor->ConfigMotionCruiseVelocity(pidf.velocity); + motor->ConfigMotionAcceleration(pidf.velocity / pidf.acceleration); + + motor->SelectProfileSlot(slot, 0); } void ValorFalconController::preventBackwards() diff --git a/src/main/cpp/controllers/ValorNeoController.cpp b/src/main/cpp/controllers/ValorNeoController.cpp index efd2b621..a15a7d87 100644 --- a/src/main/cpp/controllers/ValorNeoController.cpp +++ b/src/main/cpp/controllers/ValorNeoController.cpp @@ -7,8 +7,7 @@ ValorNeoController::ValorNeoController(int canID, ValorController(new rev::CANSparkMax(canID, rev::CANSparkMax::MotorType::kBrushless), _inverted, _mode), pidController(motor->GetPIDController()), encoder(motor->GetEncoder()), - extEncoder(motor->GetAbsoluteEncoder(rev::SparkMaxAbsoluteEncoder::Type::kDutyCycle)), - currentPidSlot(0) + extEncoder(motor->GetAbsoluteEncoder(rev::SparkMaxAbsoluteEncoder::Type::kDutyCycle)) { init(); } @@ -21,6 +20,7 @@ void ValorNeoController::init() setRange(0,-1,1); ValorPIDF motionPIDF; setPIDF(motionPIDF, 0); + setProfile(0); reset(); wpi::SendableRegistry::AddLW(this, "ValorNeoController", "ID " + std::to_string(motor->GetDeviceId())); @@ -58,9 +58,14 @@ void ValorNeoController::setPIDF(ValorPIDF pidf, int slot) pidController.SetFF(pidf.F, slot); pidController.SetIZone(0, slot); - pidController.SetSmartMotionMaxVelocity(pidf.velocity * 60.0 / conversion, slot); - pidController.SetSmartMotionMaxAccel(pidf.acceleration * 60.0 / conversion, slot); + pidf.velocity = pidf.velocity * 60.0 / conversion; + pidf.acceleration = pidf.acceleration * 60.0 / conversion; + + pidController.SetSmartMotionMaxVelocity(pidf.velocity, slot); + pidController.SetSmartMotionMaxAccel(pidf.acceleration, slot); pidController.SetSmartMotionAllowedClosedLoopError(pidf.error, slot); + + pidfs.insert_or_assign(slot, pidf); } /** @@ -95,14 +100,6 @@ double ValorNeoController::getPosition() return encoder.GetPosition(); } -/** - * Get the PIDF profile number of the motor -*/ -int ValorNeoController::getProfile() -{ - return currentPidSlot; -} - /** * Get the speed in units per second (specified by conversion) */ @@ -126,12 +123,12 @@ double ValorNeoController::getAbsEncoderPosition() */ void ValorNeoController::setPosition(double position) { - pidController.SetReference(position, rev::CANSparkMax::ControlType::kSmartMotion, currentPidSlot); + pidController.SetReference(position, rev::CANSparkMax::ControlType::kSmartMotion, currentPIDSlot); } -void ValorNeoController::setProfile(int profile) +void ValorNeoController::setProfile(int slot) { - currentPidSlot = profile; + currentPIDSlot = slot; } /** @@ -139,7 +136,7 @@ void ValorNeoController::setProfile(int profile) */ void ValorNeoController::setSpeed(double speed) { - pidController.SetReference(speed * 60.0, rev::CANSparkMax::ControlType::kVelocity, currentPidSlot); + pidController.SetReference(speed * 60.0, rev::CANSparkMax::ControlType::kVelocity, currentPIDSlot); } void ValorNeoController::setPower(double power) diff --git a/src/main/cpp/subsystems/Elevarm.cpp b/src/main/cpp/subsystems/Elevarm.cpp index a22ad259..b1524e56 100644 --- a/src/main/cpp/subsystems/Elevarm.cpp +++ b/src/main/cpp/subsystems/Elevarm.cpp @@ -168,6 +168,7 @@ void Elevarm::init() armRotateMotor.setForwardLimit(ROTATE_FORWARD_LIMIT); armRotateMotor.setReverseLimit(ROTATE_REVERSE_LIMIT); armRotateMotor.setPIDF(rotatePID, 0); + armRotateMotor.setPIDF(autoRotatePID, 1); armCANcoder.ConfigAbsoluteSensorRange(AbsoluteSensorRange::Unsigned_0_to_360); armCANcoder.SetPositionToAbsolute(); @@ -456,6 +457,10 @@ frc::Pose2d Elevarm::forwardKinematics(Elevarm::Positions positions) return frc::Pose2d((units::length::meter_t)x,(units::length::meter_t)z,(units::angle::degree_t)w); } +void Elevarm::setArmPIDF(int slot) { + armRotateMotor.setProfile(slot); +} + void Elevarm::InitSendable(wpi::SendableBuilder& builder) { builder.SetSmartDashboardType("Subsystem"); @@ -596,17 +601,13 @@ frc2::FunctionalCommand * Elevarm::getAutoCommand(std::string pieceState, std::s ); } -void Elevarm::setArmPIDF(bool isAuto) { - isAuto ? armRotateMotor.setPIDF(autoRotatePID,0) : armRotateMotor.setPIDF(rotatePID,0); -} - -frc2::FunctionalCommand * Elevarm::getRotatePIDSetterCommand(bool isAuto){ +frc2::FunctionalCommand * Elevarm::getRotatePIDSetterCommand(int slot){ return new frc2::FunctionalCommand( // OnInit [&]() {}, //onExecute - [&, isAuto](){ - setArmPIDF(isAuto); + [&, slot](){ + armRotateMotor.setProfile(slot); }, [&](bool){}, // onEnd [&](){ diff --git a/src/main/include/auto/ValorAutoAction.h b/src/main/include/auto/ValorAutoAction.h index 3c8bd71d..302515ee 100644 --- a/src/main/include/auto/ValorAutoAction.h +++ b/src/main/include/auto/ValorAutoAction.h @@ -54,6 +54,7 @@ struct ValorAutoAction { double vel; double maxAccel; double accelMultiplier; + double slot; bool parallel; diff --git a/src/main/include/controllers/ValorController.h b/src/main/include/controllers/ValorController.h index 49d2faf1..906e6517 100644 --- a/src/main/include/controllers/ValorController.h +++ b/src/main/include/controllers/ValorController.h @@ -278,6 +278,14 @@ class ValorController : public wpi::Sendable, public wpi::SendableHelper pidfs; + + int currentPIDSlot; }; \ No newline at end of file diff --git a/src/main/include/controllers/ValorFalconController.h b/src/main/include/controllers/ValorFalconController.h index 7064908a..b45fa188 100644 --- a/src/main/include/controllers/ValorFalconController.h +++ b/src/main/include/controllers/ValorFalconController.h @@ -33,7 +33,8 @@ class ValorFalconController : public ValorController void setupFollower(int, bool = false); - void setPIDF(ValorPIDF pidf, int slot); + void setPIDF(ValorPIDF pidf, int slot = 0); + void setForwardLimit(double forward); void setReverseLimit(double reverse); void setRange(int slot, double min, double max); @@ -53,6 +54,4 @@ class ValorFalconController : public ValorController void setNeutralMode(ValorNeutralMode mode); void InitSendable(wpi::SendableBuilder& builder); -private: - ValorPIDF pidf; }; \ No newline at end of file diff --git a/src/main/include/controllers/ValorNeoController.h b/src/main/include/controllers/ValorNeoController.h index 8d4758cf..54284ff4 100644 --- a/src/main/include/controllers/ValorNeoController.h +++ b/src/main/include/controllers/ValorNeoController.h @@ -28,8 +28,6 @@ class ValorNeoController : public ValorController double getPosition(); double getSpeed(); - int getProfile(); - void setEncoderPosition(double position); void setPosition(double); @@ -56,6 +54,4 @@ class ValorNeoController : public ValorController rev::SparkMaxPIDController pidController; rev::SparkMaxRelativeEncoder encoder; rev::SparkMaxAbsoluteEncoder extEncoder; - - int currentPidSlot; }; \ No newline at end of file diff --git a/src/main/include/subsystems/Elevarm.h b/src/main/include/subsystems/Elevarm.h index 0296299f..7295e8db 100644 --- a/src/main/include/subsystems/Elevarm.h +++ b/src/main/include/subsystems/Elevarm.h @@ -128,7 +128,7 @@ class Elevarm : public ValorSubsystem frc2::FunctionalCommand * getAutoCommand(std::string, std::string, std::string, bool); - frc2::FunctionalCommand * getRotatePIDSetterCommand(bool); + frc2::FunctionalCommand * getRotatePIDSetterCommand(int); std::unordered_map stringToPieceMap = { {"cone", ElevarmPieceState::ELEVARM_CONE}, @@ -162,7 +162,7 @@ class Elevarm : public ValorSubsystem return stringToPositionMap.at(name); } - void setArmPIDF(bool); + void setArmPIDF(int); private: