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

More work tuning the elevator

Browse Source
This commit is contained in:
Team 2648 2025-02-17 18:58:43 -05:00
parent aa6a0366e6
commit 42d15ab101
5 changed files with 82 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -56,8 +56,8 @@ public class RobotContainer {
climberRollers = new ClimberRollers();
vision = new Vision(drivetrain::getGyroValue);
drivetrain = new Drivetrain(vision);
//vision = new Vision(drivetrain::getGyroValue);
drivetrain = new Drivetrain();
elevator = new Elevator();
//elevator = new ElevatorSysID();
@ -153,13 +153,15 @@ public class RobotContainer {
*/
operator.povUp().onTrue(
elevator.goToSetpoint(() -> 20)
elevator.goToSetpoint(() -> 20).until(elevator::eitherAtGoal)
);
operator.povDown().onTrue(
elevator.goToSetpoint(() -> 0)
elevator.goToSetpoint(() -> 0).until(elevator::eitherAtGoal)
);
operator.a().whileTrue(elevator.maintainPosition());
/*
operator.a().whileTrue(elevator.runManualElevator(() -> 0.2));

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

@ -21,17 +21,21 @@ public class ElevatorConstants {
public static final int kCurrentLimit = 40;
public static final double kPositionControllerP = 3; //
public static final double kPositionControllerI = 0;
public static final double kPositionControllerD = 0.1;//0.35
public static final double kUpControllerP = 4;//7; //
public static final double kUpControllerI = 0;
public static final double kUpControllerD = 0.35;//0.1;//0.35
public static final double kDownControllerP = 6;//7; //
public static final double kDownControllerI = 0;
public static final double kDownControllerD = 0.175;//0.1;//0.35
public static final double kAllowedError = 0.75;
public static final double kFeedForwardS = 0; /* kG too high - kG too low / 2 */
public static final double kFeedForwardG = .7; /* kG too high + kG too low / 2 */ // calculated value 0.6
public static final double kFeedForwardV = 0.12; // calculated value 0.12
public static final double kFeedForwardS = (0.95 - 0.2)/2*0.8; /* kG too high - kG too low / 2 0.95, 0.2 */
public static final double kFeedForwardG = (0.95 + 0.2)/2; /* kG too high + kG too low / 2 */ // calculated value 0.6
public static final double kFeedForwardV = 0.06; // calculated value 0.12
public static final double kMaxVelocity = 120.0; // 120 inches per second (COOKING) calculated max is 184 in/s
public static final double kMaxVelocity = 150.0; // 120 inches per second (COOKING) calculated max is 184 in/s
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;

5
src/main/java/frc/robot/subsystems/Drivetrain.java
View File

@ -44,7 +44,7 @@ public class Drivetrain extends SubsystemBase {
private Vision vision;
/** Creates a new DriveSubsystem. */
public Drivetrain(Vision vision) {
public Drivetrain() {
m_frontLeft = new MAXSwerveModule(
DrivetrainConstants.kFrontLeftDrivingCanId,
DrivetrainConstants.kFrontLeftTurningCanId,
@ -115,6 +115,7 @@ public class Drivetrain extends SubsystemBase {
// if the detected tags match your alliances reef tags use their pose estimates
/*
if(vision.getOrangeClosestTag() >= 6 || vision.getOrangeClosestTag() <= 11 || DriverStation.getAlliance().equals(Alliance.Red)){
m_estimator.addVisionMeasurement(vision.getOrangeGlobalPose(), vision.getOrangeTimeStamp());
@ -127,7 +128,7 @@ public class Drivetrain extends SubsystemBase {
}else if(vision.getBlackClosestTag() >= 17 || vision.getBlackClosestTag() <= 22 || DriverStation.getAlliance().equals(Alliance.Blue)){
m_estimator.addVisionMeasurement(vision.getBlackGlobalPose(), vision.getBlackTimeStamp());
}
*/
}

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

@ -25,7 +25,8 @@ public class Elevator extends SubsystemBase {
private DigitalInput bottomLimitSwitch;
private ProfiledPIDController pidController;
private ProfiledPIDController pidControllerUp;
private ProfiledPIDController pidControllerDown;
private ElevatorFeedforward feedForward;
@ -60,16 +61,28 @@ public class Elevator extends SubsystemBase {
ElevatorConstants.kBottomLimitSwitchID
);
pidController = new ProfiledPIDController(
ElevatorConstants.kPositionControllerP,
ElevatorConstants.kPositionControllerI,
ElevatorConstants.kPositionControllerD,
pidControllerUp = new ProfiledPIDController(
ElevatorConstants.kUpControllerP,
ElevatorConstants.kUpControllerI,
ElevatorConstants.kUpControllerD,
new TrapezoidProfile.Constraints(
ElevatorConstants.kMaxVelocity,
ElevatorConstants.kMaxAcceleration
)
);
pidController.setTolerance(ElevatorConstants.kAllowedError);
pidControllerDown = new ProfiledPIDController(
ElevatorConstants.kDownControllerP,
ElevatorConstants.kDownControllerI,
ElevatorConstants.kDownControllerD,
new TrapezoidProfile.Constraints(
ElevatorConstants.kMaxVelocity,
ElevatorConstants.kMaxAcceleration
)
);
pidControllerUp.setTolerance(ElevatorConstants.kAllowedError);
pidControllerDown.setTolerance(ElevatorConstants.kAllowedError);
feedForward = new ElevatorFeedforward(
ElevatorConstants.kFeedForwardS,
@ -114,9 +127,16 @@ public class Elevator extends SubsystemBase {
*/
public Command runManualElevator(DoubleSupplier speed) {
return run(() -> {
elevatorMotor1.set(
speed.getAsDouble()
);
double desired = speed.getAsDouble();
if(Math.abs(MathUtil.applyDeadband(desired, .05)) > 0) {
elevatorMotor1.set(
speed.getAsDouble()
);
} else {
elevatorMotor1.setVoltage(feedForward.calculate(0));
}
});
}
@ -132,6 +152,10 @@ public class Elevator extends SubsystemBase {
});
}
public boolean eitherAtGoal() {
return pidControllerUp.atGoal() || pidControllerDown.atGoal();
}
/**
* Moves the elevator to a target destination (setpoint).
*
@ -147,17 +171,35 @@ public class Elevator extends SubsystemBase {
);
return run(() -> {
pidController.reset(encoder.getPosition(), encoder.getVelocity());
//pidController.reset(encoder.getPosition(), encoder.getVelocity());
elevatorMotor1.setVoltage(
pidController.calculate(
pidControllerUp.calculate(
encoder.getPosition(),
clampedSetpoint
) + feedForward.calculate(pidController.getSetpoint().velocity)
) + feedForward.calculate(pidControllerUp.getSetpoint().velocity)
);
});
}
/*
if(encoder.getPosition() >= setpoint.getAsDouble()){
elevatorMotor1.setVoltage(
pidControllerUp.calculate(
encoder.getPosition(),
clampedSetpoint
) + feedForward.calculate(pidControllerUp.getSetpoint().velocity)
);
}else if(encoder.getPosition() <= setpoint.getAsDouble()){
elevatorMotor1.setVoltage(
pidControllerDown.calculate(
encoder.getPosition(),
clampedSetpoint
) + feedForward.calculate(pidControllerDown.getSetpoint().velocity)
);
}
*/
/**
* Returns the current encoder position
@ -184,7 +226,7 @@ public class Elevator extends SubsystemBase {
* @return Motor output current
*/
public double getMotor1() {
return elevatorMotor1.getOutputCurrent();
return elevatorMotor1.getAppliedOutput()*elevatorMotor1.getBusVoltage();
}
/**
@ -193,6 +235,6 @@ public class Elevator extends SubsystemBase {
* @return Motor output current
*/
public double getMotor2() {
return elevatorMotor2.getOutputCurrent();
return elevatorMotor2.getAppliedOutput()*elevatorMotor2.getBusVoltage();
}
}

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

@ -12,6 +12,7 @@ import com.revrobotics.spark.SparkLowLevel.MotorType;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.ArmFeedforward;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.constants.ManipulatorPivotConstants;
@ -40,10 +41,10 @@ public class ManipulatorPivot extends SubsystemBase {
encoder = pivotMotor.getAbsoluteEncoder();
pidController = new ProfiledPIDController(
getEncoderPosition(),
getEncoderVelocity(),
getEncoderPosition(),
null
ManipulatorPivotConstants.kPositionalP,
ManipulatorPivotConstants.kPositionalI,
ManipulatorPivotConstants.kPositionalD,
new TrapezoidProfile.Constraints(ManipulatorPivotConstants.kMaxVelocity, ManipulatorPivotConstants.kMaxAcceleration)
);
feedForward = new ArmFeedforward(