Team_2648/2025_Robot_Code
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Many attempt at tuning Elevator values at 2/15 build session

Browse Source
This commit is contained in:
Team 2648 2025-02-15 18:20:59 -05:00
parent 5a53c5fe07
commit 9fc597bd30
7 changed files with 114 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
.DataLogTool/datalogtool.json Normal file
View File

@ -0,0 +1,6 @@
{
"download": {
"localDir": "C:\\Users\\infin\\Downloads",
"serverTeam": "2648"
}
}

1
.SysId/sysid.json Normal file
View File

@ -0,0 +1 @@
{}

43
src/main/java/frc/robot/RobotContainer.java
View File

@ -9,12 +9,15 @@ import frc.robot.constants.ClimberPivotConstants;
import frc.robot.constants.ElevatorConstants;
import frc.robot.constants.OIConstants;
import frc.robot.subsystems.ManipulatorPivot;
import frc.robot.subsystems.sysid.ElevatorSysID;
import frc.robot.subsystems.ClimberPivot;
import frc.robot.subsystems.ClimberRollers;
import frc.robot.subsystems.Drivetrain;
import frc.robot.subsystems.Elevator;
import frc.robot.subsystems.Manipulator;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.auto.NamedCommands;
@ -35,6 +38,7 @@ public class RobotContainer {
private Drivetrain drivetrain;
private Elevator elevator;
//private ElevatorSysID elevator;
private Manipulator manipulator;
@ -53,6 +57,7 @@ public class RobotContainer {
drivetrain = new Drivetrain();
elevator = new Elevator();
//elevator = new ElevatorSysID();
manipulator = new Manipulator();
@ -64,12 +69,22 @@ public class RobotContainer {
autoChooser = AutoBuilder.buildAutoChooser();
configureButtonBindings();
//elevatorSysIDBindings();
configureNamedCommands();
configureShuffleboard();
}
/*private void elevatorSysIDBindings() {
elevator.setDefaultCommand(elevator.maintainPosition());
operator.a().whileTrue(elevator.sysIdQuasistatic(Direction.kForward));
operator.b().whileTrue(elevator.sysIdQuasistatic(Direction.kReverse));
operator.x().whileTrue(elevator.sysIdDynamic(Direction.kForward));
operator.y().whileTrue(elevator.sysIdDynamic(Direction.kReverse));
}*/
private void configureButtonBindings() {
//Default commands
climberPivot.setDefaultCommand(
@ -91,7 +106,7 @@ public class RobotContainer {
elevator.setDefaultCommand(
elevator.runManualElevator(
() -> -operator.getLeftY()
() -> -operator.getLeftY() * .5
)
);
@ -134,8 +149,12 @@ public class RobotContainer {
driver.povRight().whileTrue(climberRollers.runRoller(-0.5));
*/
operator.povUp().whileTrue(
elevator.goToSetpoint(20)
operator.povUp().onTrue(
elevator.goToSetpoint(() -> 20)
);
operator.povDown().onTrue(
elevator.goToSetpoint(() -> 0)
);
/*
@ -233,6 +252,10 @@ public class RobotContainer {
.withSize(1, 1)
.withPosition(4, 1)
.withWidget(BuiltInWidgets.kBooleanBox);
sensorTab.addDouble("ElevMotor1", elevator::getMotor1);
sensorTab.addDouble("ElevMotor2", elevator::getMotor2);
}
public Command getAutonomousCommand() {
@ -317,13 +340,13 @@ public class RobotContainer {
return Commands.either(
Commands.either(
elevator.goToSetpoint(elevatorPosition).andThen(manipulatorPivot.goToSetpoint(armPosition)),
elevator.goToSetpoint(elevatorPosition).alongWith(manipulatorPivot.goToSetpoint(armPosition)),
elevator.goToSetpoint(() -> elevatorPosition).andThen(manipulatorPivot.goToSetpoint(armPosition)),
elevator.goToSetpoint(() -> elevatorPosition).alongWith(manipulatorPivot.goToSetpoint(armPosition)),
() -> sequential
),
Commands.either(
manipulatorPivot.goToSetpoint(armPosition).andThen(elevator.goToSetpoint(elevatorPosition)),
manipulatorPivot.goToSetpoint(armPosition).alongWith(elevator.goToSetpoint(elevatorPosition)),
manipulatorPivot.goToSetpoint(armPosition).andThen(elevator.goToSetpoint(() -> elevatorPosition)),
manipulatorPivot.goToSetpoint(armPosition).alongWith(elevator.goToSetpoint(() -> elevatorPosition)),
() -> sequential
),
() -> elevatorFirst
@ -335,14 +358,14 @@ public class RobotContainer {
if(!isL4){
return Commands.sequence(
manipulatorPivot.goToSetpoint(ManipulatorPivotConstants.kArmSafeStowPosition),
elevator.goToSetpoint(elevatorPosition),
elevator.goToSetpoint(() -> elevatorPosition),
manipulatorPivot.goToSetpoint(armPosition));
}else{
return Commands.sequence(
manipulatorPivot.goToSetpoint(ManipulatorPivotConstants.kArmSafeStowPosition),
elevator.goToSetpoint(elevatorPosition).until(() -> elevator.getEncoderPosition() > ElevatorConstants.kL4TransitionPosition),
Commands.parallel( manipulatorPivot.goToSetpoint(armPosition)), elevator.goToSetpoint(elevatorPosition));
elevator.goToSetpoint(() -> elevatorPosition).until(() -> elevator.getEncoderPosition() > ElevatorConstants.kL4TransitionPosition),
Commands.parallel( manipulatorPivot.goToSetpoint(armPosition)), elevator.goToSetpoint(() -> elevatorPosition));
}
}

34
src/main/java/frc/robot/constants/ElevatorConstants.java
View File

@ -21,24 +21,18 @@ public class ElevatorConstants {
public static final int kCurrentLimit = 40;
public static final double kPositionControllerP = 0.1;
public static final double kPositionControllerP = 3; //
public static final double kPositionControllerI = 0;
public static final double kPositionControllerD = 0;
public static final double kPositionControllerD = 0.1;//0.35
public static final double kAllowedError = 0.5;
/*
public static final double kVelocityControllerP = 0;
public static final double kVelocityControllerI = 0;
public static final double kVelocityControllerD = 0;
*/
public static final double kAllowedError = 0.75;
public static final double kFeedForwardS = 0;
public static final double kFeedForwardG = 0.53; // calculated value
public static final double kFeedForwardV = 4.78; // calculated value
public static final double kFeedForwardG = .7;//1; // calculated value .6
public static final double kFeedForwardV = 0.12; // calculated value .12 INCHES NOT METERS
public static final double kMaxVelocity = 120.0; // 120 inches per second (COOKING) calculated max is 184 in/s
public static final double kMaxAcceleration = 500.0; // 400 inches per second^2 (also COOKING) calculated max is 600 in/s^2
public static final double kMaxAcceleration = 240; // 400 inches per second^2 (also COOKING) calculated max is 600 in/s^2
public static final double kCoralIntakePosition = 0;
public static final double kL1Position = 0;
@ -51,11 +45,11 @@ public class ElevatorConstants {
public static final double kProcessorPosition = 4.0;
/**The position of the top of the elevator brace */
public static final double kBracePosition = 5.5;
public static final double kMaxHeight = 53.0;
public static final double kMaxHeight = 47.5; //actual is 53
// 1, 7, 10 are the defaults for these, change as necessary
public static final double kSysIDRampRate = 1;
public static final double kSysIDStepVolts = 7;
public static final double kSysIDRampRate = .25;
public static final double kSysIDStepVolts = 3;
public static final double kSysIDTimeout = 10;
public static final IdleMode kIdleMode = IdleMode.kBrake;
@ -76,10 +70,9 @@ public class ElevatorConstants {
.idleMode(kIdleMode)
.inverted(true);
motorConfig.encoder
//.inverted(true)
.positionConversionFactor(kEncoderConversionFactor);
/*
motorConfig.closedLoop
.positionConversionFactor(kEncoderConversionFactor)
.velocityConversionFactor(kEncoderConversionFactor);
/*motorConfig.closedLoop
.p(kPositionControllerP)
.i(kPositionControllerI)
.d(kPositionControllerD)
@ -87,8 +80,7 @@ public class ElevatorConstants {
motorConfig.closedLoop.maxMotion
.maxAcceleration(kMaxAcceleration)
.maxVelocity(kMaxVelocity)
.allowedClosedLoopError(kAllowedError);
*/
.allowedClosedLoopError(kAllowedError);*/
}
public static final SparkMaxConfig motorConfig2 = new SparkMaxConfig();

56
src/main/java/frc/robot/subsystems/Elevator.java
View File

@ -15,6 +15,8 @@ import com.revrobotics.spark.SparkLowLevel.MotorType;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.ElevatorFeedforward;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
@ -30,7 +32,7 @@ public class Elevator extends SubsystemBase {
private DigitalInput bottomLimitSwitch;
private PIDController pidController;
private ProfiledPIDController pidController;
private ElevatorFeedforward feedForward;
@ -52,7 +54,7 @@ public class Elevator extends SubsystemBase {
);
elevatorMotor2.configure(
ElevatorConstants.motorConfig2.follow(ElevatorConstants.kElevatorMotor1ID),
ElevatorConstants.motorConfig.follow(ElevatorConstants.kElevatorMotor1ID),
ResetMode.kResetSafeParameters,
PersistMode.kPersistParameters
);
@ -65,10 +67,14 @@ public class Elevator extends SubsystemBase {
ElevatorConstants.kBottomLimitSwitchID
);
pidController = new PIDController(
pidController = new ProfiledPIDController(
ElevatorConstants.kPositionControllerP,
ElevatorConstants.kPositionControllerI,
ElevatorConstants.kPositionControllerD
ElevatorConstants.kPositionControllerD,
new TrapezoidProfile.Constraints(
ElevatorConstants.kMaxVelocity,
ElevatorConstants.kMaxAcceleration
)
);
pidController.setTolerance(ElevatorConstants.kAllowedError);
@ -115,8 +121,15 @@ public class Elevator extends SubsystemBase {
*/
public Command runManualElevator(DoubleSupplier speed) {
return run(() -> {
//elevatorClosedLoop.setReference(speed.getAsDouble(), ControlType.kDutyCycle);
elevatorMotor1.set(speed.getAsDouble());
elevatorMotor1.set(
speed.getAsDouble()
);
});
}
public Command maintainPosition() {
return run(() -> {
elevatorMotor1.setVoltage(feedForward.calculate(0));
});
}
@ -127,27 +140,34 @@ public class Elevator extends SubsystemBase {
* @param timeout Time to achieve the setpoint before quitting
* @return Sets motor voltage to achieve the target destination
*/
public Command goToSetpoint(double setpoint) {
public Command goToSetpoint(DoubleSupplier setpoint) {
double clampedSetpoint = MathUtil.clamp(
setpoint,
setpoint.getAsDouble(),
0,
ElevatorConstants.kMaxHeight
);
return run(() -> {
/*
if(pidController.atGoal()) {
elevatorMotor1.setVoltage(feedForward.calculate(0));
} else {
*/
elevatorMotor1.setVoltage(
pidController.calculate(
encoder.getPosition(),
clampedSetpoint
) + feedForward.calculate(0)
) + feedForward.calculate(pidController.getSetpoint().velocity)
);
/*
elevatorClosedLoop.setReference(clampedSetpoint,
SparkBase.ControlType.kMAXMotionPositionControl,
/*elevatorClosedLoop.setReference(
clampedSetpoint,
ControlType.kMAXMotionPositionControl,
ClosedLoopSlot.kSlot0,
feedForward.calculate(0)
);
*/
);*/
});
}
@ -170,4 +190,12 @@ public class Elevator extends SubsystemBase {
public boolean getBottomLimitSwitch() {
return bottomLimitSwitch.get();
}
public double getMotor1() {
return elevatorMotor1.getOutputCurrent();
}
public double getMotor2() {
return elevatorMotor2.getOutputCurrent();
}
}

2
src/main/java/frc/robot/subsystems/ManipulatorPivot.java
View File

@ -92,7 +92,7 @@ public class ManipulatorPivot extends SubsystemBase {
return run(() -> {
pivotClosedLoopController.setReference(clampedSetpoint,
SparkBase.ControlType.kMAXMotionPositionControl,
ClosedLoopSlot.kSlot0,
ClosedLoopSlot.kSlot1,
feedForward.calculate(absoluteEncoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, absoluteEncoder.getVelocity()));
});
}

25
src/main/java/frc/robot/subsystems/sysid/ElevatorSysID.java
View File

@ -1,8 +1,8 @@
package frc.robot.subsystems.sysid;
import static edu.wpi.first.units.Units.Meters;
import static edu.wpi.first.units.Units.Inches;
import static edu.wpi.first.units.Units.Volts;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.InchesPerSecond;
import edu.wpi.first.units.measure.MutDistance;
import edu.wpi.first.units.measure.MutLinearVelocity;
@ -14,23 +14,17 @@ import frc.robot.constants.ElevatorConstants;
import frc.robot.subsystems.Elevator;
public class ElevatorSysID extends Elevator {
private MutVoltage appliedVoltage;
private MutVoltage appliedVoltage = Volts.mutable(0);;
private MutDistance elevatorPosition;
private MutDistance elevatorPosition = Inches.mutable(0);;
private MutLinearVelocity elevatorVelocity;
private MutLinearVelocity elevatorVelocity = InchesPerSecond.mutable(0);
private SysIdRoutine routine;
public ElevatorSysID() {
super();
appliedVoltage = Volts.mutable(0);
elevatorPosition = Meters.mutable(0);
elevatorVelocity = MetersPerSecond.mutable(0);
routine = new SysIdRoutine(
ElevatorConstants.kSysIDConfig,
new SysIdRoutine.Mechanism(
@ -41,10 +35,10 @@ public class ElevatorSysID extends Elevator {
elevatorMotor1.get() * RobotController.getBatteryVoltage(), Volts
))
.linearPosition(elevatorPosition.mut_replace(
encoder.getPosition(), Meters
encoder.getPosition(), Inches
))
.linearVelocity(elevatorVelocity.mut_replace(
encoder.getVelocity(), MetersPerSecond
encoder.getVelocity(), InchesPerSecond
));
},
this
@ -52,6 +46,11 @@ public class ElevatorSysID extends Elevator {
);
}
@Override
public void periodic() {
}
public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
return routine.quasistatic(direction);
}