Skip to content

Commit

Permalink
fix: valor controller stores pidf and profiles are setable. new eleva…
Browse files Browse the repository at this point in the history 
…rm auto command to set arm pidf
  • Loading branch information
@AntarcticaByToto
AntarcticaByToto committed Mar 9, 2023
1 parent e6b48b0 commit 1f48060
Show file tree
Hide file tree
Showing 11 changed files with 87 additions and 64 deletions.
2 changes: 1 addition & 1 deletion src/main/cpp/Robot.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down
38 changes: 21 additions & 17 deletions src/main/cpp/auto/ValorAuto.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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];
Expand Down Expand Up @@ -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){
Expand Down Expand Up @@ -430,8 +436,6 @@ frc2::SequentialCommandGroup* ValorAuto::makeAuto(std::string filename, bool blu
);
}

combinedGroup->AddCommands(std::move(*elevarm->getRotatePIDSetterCommand(false)));

return combinedGroup;
}

Expand Down
13 changes: 9 additions & 4 deletions src/main/cpp/auto/ValorAutoAction.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -153,11 +153,16 @@ ValorAutoAction::ValorAutoAction(std::string line, std::map<std::string, frc::Tr
error_message = "received " + std::to_string(items.size());
return;
}

values = {items[1], items[2], items[3]};

parallel = false;
if (items.size() >= 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]);
Expand Down
28 changes: 18 additions & 10 deletions src/main/cpp/controllers/ValorFalconController.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()));
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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);
}
Expand All @@ -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()
Expand Down
29 changes: 13 additions & 16 deletions src/main/cpp/controllers/ValorNeoController.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
}
Expand All @@ -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()));
Expand Down Expand Up @@ -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);
}

/**
Expand Down Expand Up @@ -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)
*/
Expand All @@ -126,20 +123,20 @@ 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;
}

/**
* Set the speed in units per second (specified by conversion). Example: inches per second
*/
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)
Expand Down
15 changes: 8 additions & 7 deletions src/main/cpp/subsystems/Elevarm.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down Expand Up @@ -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");
Expand Down Expand Up @@ -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
[&](){
Expand Down
1 change: 1 addition & 0 deletions src/main/include/auto/ValorAutoAction.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,7 @@ struct ValorAutoAction {
double vel;
double maxAccel;
double accelMultiplier;
double slot;

bool parallel;

Expand Down
12 changes: 12 additions & 0 deletions src/main/include/controllers/ValorController.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -278,6 +278,14 @@ class ValorController : public wpi::Sendable, public wpi::SendableHelper<ValorCo
*/
virtual void setProfile(int slot) = 0;

/**
* Get the PIDF profile number of the motor
*/
int getProfile()
{
return currentPIDSlot;
}

virtual void setNeutralMode(ValorNeutralMode mode) = 0;

virtual void InitSendable(wpi::SendableBuilder& builder) = 0;
Expand All @@ -294,4 +302,8 @@ class ValorController : public wpi::Sendable, public wpi::SendableHelper<ValorCo
bool inverted;

ValorNeutralMode neutralMode;

std::unordered_map<int, ValorPIDF> pidfs;

int currentPIDSlot;
};
5 changes: 2 additions & 3 deletions src/main/include/controllers/ValorFalconController.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,8 @@ class ValorFalconController : public ValorController<WPI_TalonFX>

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);
Expand All @@ -53,6 +54,4 @@ class ValorFalconController : public ValorController<WPI_TalonFX>
void setNeutralMode(ValorNeutralMode mode);

void InitSendable(wpi::SendableBuilder& builder);
private:
ValorPIDF pidf;
};
4 changes: 0 additions & 4 deletions src/main/include/controllers/ValorNeoController.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,8 +28,6 @@ class ValorNeoController : public ValorController<rev::CANSparkMax>
double getPosition();
double getSpeed();

int getProfile();

void setEncoderPosition(double position);

void setPosition(double);
Expand All @@ -56,6 +54,4 @@ class ValorNeoController : public ValorController<rev::CANSparkMax>
rev::SparkMaxPIDController pidController;
rev::SparkMaxRelativeEncoder encoder;
rev::SparkMaxAbsoluteEncoder extEncoder;

int currentPidSlot;
};
4 changes: 2 additions & 2 deletions src/main/include/subsystems/Elevarm.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::string, ElevarmPieceState> stringToPieceMap = {
{"cone", ElevarmPieceState::ELEVARM_CONE},
Expand Down Expand Up @@ -162,7 +162,7 @@ class Elevarm : public ValorSubsystem
return stringToPositionMap.at(name);
}

void setArmPIDF(bool);
void setArmPIDF(int);

private:

Expand Down

0 comments on commit 1f48060

Please sign in to comment.