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

Robot PID testing

Browse Source
This commit is contained in:
Team 2648 2025-02-15 12:46:23 -05:00
parent 2275248f70
commit ddcf64159f
6 changed files with 89 additions and 152 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -89,24 +89,28 @@ public class RobotContainer {
)
);
//elevator.setDefaultCommand(
//elevator.runAssistedElevator(operator::getLeftY)
// );
//elevator.setDefaultCommand(moveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kCoralIntakePosition));
elevator.setDefaultCommand(elevator.manualControl(() -> 0.0));
manipulatorPivot.setDefaultCommand(moveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kCoralIntakePosition));
manipulator.setDefaultCommand(
manipulator.defaultCommand()
elevator.setDefaultCommand(
elevator.runManualElevator(
() -> -operator.getLeftY()
)
);
//manipulatorPivot.setDefaultCommand(
// manipulatorPivot.runAssistedPivot(operator::getRightY)
//);
manipulatorPivot.setDefaultCommand(
manipulatorPivot.setSpeed(
() -> 0
)
);
manipulator.setDefaultCommand(
manipulator.runUntilCollected(
() -> 0
)
);
//Driver inputs
/*
driver.start().whileTrue(
drivetrain.setXCommand()
);
@ -128,16 +132,18 @@ public class RobotContainer {
driver.povLeft().whileTrue(climberRollers.runRoller(0.5));
driver.povRight().whileTrue(climberRollers.runRoller(-0.5));
*/
operator.povUp().whileTrue(
elevator.goToSetpoint(20)
);
//operator.a().whileTrue(elevator.manualControl(() -> 0.5));
/*
operator.a().whileTrue(elevator.runManualElevator(() -> 0.2));
//operator.b().whileTrue(elevator.manualControl(() ->-0.1));
operator.b().whileTrue(elevator.runManualElevator(() -> -0.2));
/*
//Operator inputs
operator.povUp().onTrue(
moveManipulator(
@ -200,36 +206,33 @@ public class RobotContainer {
sensorTab.addDouble("Elevator Position", elevator::getEncoderPosition)
.withSize(2, 1)
.withPosition(0, 1)
.withPosition(0, 0)
.withWidget(BuiltInWidgets.kTextView);
sensorTab.addDouble("Manipulator Position", manipulatorPivot::getEncoderPosition)
.withSize(2, 1)
.withPosition(2, 1)
.withPosition(2, 0)
.withWidget(BuiltInWidgets.kTextView);
sensorTab.addDouble("Climber Pivot Position", climberPivot::getEncoderPosition)
.withSize(2, 1)
.withPosition(2, 0)
.withPosition(2, 1)
.withWidget(BuiltInWidgets.kTextView);
sensorTab.addDouble("gyro angle", drivetrain::getGyroValue)
.withSize(2, 1)
.withPosition(0, 1)
.withWidget(BuiltInWidgets.kTextView);
sensorTab.addBoolean("Coral Sensor", manipulator::getCoralBeamBreak)
.withSize(1, 1)
.withPosition(4, 1)
.withWidget(BuiltInWidgets.kBooleanBox);
sensorTab.addDouble("gyro angle", drivetrain::getGyroValue);
sensorTab.addBoolean("bottom limit switch", elevator::getBottomLimitSwitch);
sensorTab.addDouble("elevator position", elevator::getEncoderPosition);
/*
sensorTab.addBoolean("Algae Sensor", manipulator::getAlgaePhotoSwitch)
.withSize(1, 1)
.withPosition(4, 0)
.withWidget(BuiltInWidgets.kBooleanBox);
*/
sensorTab.addBoolean("bottom limit switch", elevator::getBottomLimitSwitch)
.withSize(1, 1)
.withPosition(4, 1)
.withWidget(BuiltInWidgets.kBooleanBox);
}
public Command getAutonomousCommand() {
@ -240,12 +243,13 @@ public class RobotContainer {
* Moves the elevator and arm to the coral intake position, then runs the manipulator until collected
* @return Moves the elevator and arm, then intakes coral
*/
@SuppressWarnings("unused")
private Command coralIntakeRoutine() {
return moveManipulator(
ElevatorConstants.kCoralIntakePosition,
ManipulatorPivotConstants.kCoralIntakePosition
)
.andThen(manipulator.runUntilCollected(() -> 1));
.andThen(manipulator.runUntilCollected(() -> .5));
}
/**
@ -254,6 +258,7 @@ public class RobotContainer {
* @param l2 Is the algae on L2? (True = L2, False = L3)
* @return Moves the elevator and arm then intakes algae
*/
@SuppressWarnings("unused")
private Command algaeIntakeRoutine(boolean l2) {
return moveManipulator(
l2 ? ElevatorConstants.kL2AlgaePosition : ElevatorConstants.kL3AlgaePosition,
@ -325,6 +330,7 @@ public class RobotContainer {
);
}
@SuppressWarnings("unused")
private Command manipulatorSafeTravel(double elevatorPosition, double armPosition, boolean isL4){
if(!isL4){
return Commands.sequence(
@ -354,6 +360,7 @@ public class RobotContainer {
.andThen(manipulatorPivot.goToSetpoint(armPosition));
}
@SuppressWarnings("unused")
private Command startingConfig() {
return moveManipulatorUtil(0, 0, false, true)
.alongWith(climberPivot.climb(ClimberPivotConstants.kClimberStartingPosition, .1));

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

@ -21,7 +21,7 @@ public class ElevatorConstants {
public static final int kCurrentLimit = 40;
public static final double kPositionControllerP = 0.3;
public static final double kPositionControllerP = 0.1;
public static final double kPositionControllerI = 0;
public static final double kPositionControllerD = 0;
@ -74,9 +74,11 @@ public class ElevatorConstants {
motorConfig
.smartCurrentLimit(kCurrentLimit)
.idleMode(kIdleMode)
.inverted(false);
.inverted(true);
motorConfig.encoder
.positionConversionFactor(kEncoderConversionFactor);
//.inverted(true)
.positionConversionFactor(kEncoderConversionFactor);
/*
motorConfig.closedLoop
.p(kPositionControllerP)
.i(kPositionControllerI)
@ -86,6 +88,7 @@ public class ElevatorConstants {
.maxAcceleration(kMaxAcceleration)
.maxVelocity(kMaxVelocity)
.allowedClosedLoopError(kAllowedError);
*/
}
public static final SparkMaxConfig motorConfig2 = new SparkMaxConfig();

13
src/main/java/frc/robot/subsystems/ClimberPivot.java
View File

@ -15,8 +15,6 @@ public class ClimberPivot extends SubsystemBase {
private RelativeEncoder neoEncoder;
//private DigitalInput cageLimitSwitch;
public ClimberPivot() {
pivotMotor = new SparkMax(
ClimberPivotConstants.kPivotMotorID,
@ -30,8 +28,6 @@ public class ClimberPivot extends SubsystemBase {
);
neoEncoder = pivotMotor.getEncoder();
//cageLimitSwitch = new DigitalInput(ClimberPivotConstants.kClimberLimitSwitchID);
}
public Command runPivot(double speed) {
@ -53,15 +49,6 @@ public class ClimberPivot extends SubsystemBase {
}).until(() -> neoEncoder.getPosition() >= setpoint);
}
/**
* Returns the limit switch attached to the climber. Detects if the cage is present
*
* @return Is the cage in the climber
*/
// public boolean getCageLimitSwitch() {
// return cageLimitSwitch.get();
//}
public double getEncoderPosition() {
return neoEncoder.getPosition();
}

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

@ -14,6 +14,7 @@ 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.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
@ -23,12 +24,14 @@ public class Elevator extends SubsystemBase {
protected SparkMax elevatorMotor1;
protected SparkMax elevatorMotor2;
private SparkClosedLoopController elevatorClosedLoop;
//private SparkClosedLoopController elevatorClosedLoop;
protected RelativeEncoder encoder;
private DigitalInput bottomLimitSwitch;
private PIDController pidController;
private ElevatorFeedforward feedForward;
public Elevator() {
@ -48,27 +51,26 @@ public class Elevator extends SubsystemBase {
PersistMode.kPersistParameters
);
elevatorClosedLoop = elevatorMotor1.getClosedLoopController();
elevatorMotor2.configure(
ElevatorConstants.motorConfig2.follow(ElevatorConstants.kElevatorMotor1ID),
ResetMode.kResetSafeParameters,
PersistMode.kPersistParameters
);
//elevatorClosedLoop = elevatorMotor1.getClosedLoopController();
encoder = elevatorMotor1.getEncoder();
bottomLimitSwitch = new DigitalInput(
ElevatorConstants.kBottomLimitSwitchID
);
/*
velocityController = new PIDController(
ElevatorConstants.kVelocityControllerP,
ElevatorConstants.kVelocityControllerI,
ElevatorConstants.kVelocityControllerD
pidController = new PIDController(
ElevatorConstants.kPositionControllerP,
ElevatorConstants.kPositionControllerI,
ElevatorConstants.kPositionControllerD
);
*/
pidController.setTolerance(ElevatorConstants.kAllowedError);
feedForward = new ElevatorFeedforward(
ElevatorConstants.kFeedForwardS,
@ -111,43 +113,15 @@ public class Elevator extends SubsystemBase {
* @param speed The speed at which the elevator translates
* @return Sets motor voltage to translate the elevator and maintain position
*/
public Command runManualElevator(double speed) {
public Command runManualElevator(DoubleSupplier speed) {
return run(() -> {
elevatorMotor1.set(speed);
});
}
/**
* A manual translation command that will move the elevator using a consistent velocity disregarding direction
*
* @param speed How fast the elevator moves
* @return Sets motor voltage to move the elevator relative to the speed parameter
*
public Command runAssistedElevator(DoubleSupplier speed) {
return run(() -> {
double realSpeedTarget = speed.getAsDouble() * ElevatorConstants.kMaxVelocity;
double voltsOut = velocityController.calculate(
encoder.getVelocity(),
realSpeedTarget
) + feedForward.calculate(realSpeedTarget);
elevatorMotor1.setVoltage(voltsOut);
}).until(
() -> bottomLimitSwitch.get() || encoder.getPosition() >= ElevatorConstants.kMaxHeight);
}
*/
public Command manualControl(DoubleSupplier speed){
return run(() -> {
elevatorClosedLoop.setReference(speed.getAsDouble(), ControlType.kDutyCycle);
//elevatorClosedLoop.setReference(speed.getAsDouble(), ControlType.kDutyCycle);
elevatorMotor1.set(speed.getAsDouble());
});
}
/**
* Moves the elevator to a target destination (setpoint).
* If the setpoint is 0, the elevator will creep down to hit the limit switch
*
* @param setpoint Target destination of the subsystem
* @param timeout Time to achieve the setpoint before quitting
@ -161,44 +135,20 @@ public class Elevator extends SubsystemBase {
);
return run(() -> {
elevatorMotor1.setVoltage(
pidController.calculate(
encoder.getPosition(),
clampedSetpoint
) + feedForward.calculate(0)
);
/*
elevatorClosedLoop.setReference(clampedSetpoint,
SparkBase.ControlType.kMAXMotionPositionControl,
ClosedLoopSlot.kSlot0,
feedForward.calculate(0)
);
});
/*
if (clampedSetpoint == 0) {
return run(() -> {
double voltsOut = positionController.calculate(
encoder.getPosition(),
clampedSetpoint
) + feedForward.calculate(0);
elevatorMotor1.setVoltage(voltsOut);
}).until(
() -> positionController.atSetpoint() || bottomLimitSwitch.get()
).withTimeout(timeout)
.andThen(Commands.either(
runAssistedElevator(() -> 0),
runAssistedElevator(() -> -.2),
bottomLimitSwitch::get
)).withTimeout(timeout);
} else {
return run(() -> {
double voltsOut = positionController.calculate(
encoder.getPosition(),
clampedSetpoint
) + feedForward.calculate(0);
elevatorMotor1.setVoltage(voltsOut);
}).until(
() -> positionController.atSetpoint() || bottomLimitSwitch.get()
).withTimeout(timeout);
}
*/
});
}

18
src/main/java/frc/robot/subsystems/Manipulator.java
View File

@ -16,7 +16,6 @@ public class Manipulator extends SubsystemBase {
private SparkMax manipulatorMotor;
private DigitalInput coralBeamBreak;
// private DigitalInput algaeBeamBreak;
public Manipulator() {
manipulatorMotor = new SparkMax(
@ -31,7 +30,6 @@ public class Manipulator extends SubsystemBase {
);
coralBeamBreak = new DigitalInput(ManipulatorConstants.kCoralBeamBreakID);
// algaeBeamBreak = new DigitalInput(ManipulatorConstants.kAlgaeBeamBreakID);
}
/**
@ -42,8 +40,7 @@ public class Manipulator extends SubsystemBase {
*/
public Command defaultCommand() {
return run(() -> {
//runUntilCollected(0.1);
manipulatorMotor.set(0);
runUntilCollected(() -> 0.1);
});
}
@ -77,6 +74,19 @@ public class Manipulator extends SubsystemBase {
}).until(() -> !coralBeamBreak.get());
}
/**
* Runs the manipulator in a way that will bring the coral to a reliable holding position
*
* @return Returns a command that will position the coral to a known location
*/
public Command indexCoral() {
return run(() -> {
runUntilCollected(() -> 0.5)
.andThen(runManipulator(() -> .1, false))
.until(() -> getCoralBeamBreak());
});
}
public boolean getCoralBeamBreak() {
return coralBeamBreak.get();
}

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

@ -1,6 +1,9 @@
package frc.robot.subsystems;
import com.revrobotics.spark.SparkMax;
import java.util.function.DoubleSupplier;
import com.revrobotics.spark.ClosedLoopSlot;
import com.revrobotics.spark.SparkAbsoluteEncoder;
import com.revrobotics.spark.SparkBase;
@ -66,35 +69,12 @@ public class ManipulatorPivot extends SubsystemBase {
return motionTarget > ManipulatorPivotConstants.kArmSafeStowPosition;
}
/**
* A manual rotation command that will move the elevator using a consistent velocity disregarding direction
*
* @param speed The velocity at which the arm rotates
* @return Sets motor voltage to achieve the target velocity
*/
/*
public Command runAssistedPivot(DoubleSupplier speed) {
double clampedSpeed = MathUtil.clamp(
speed.getAsDouble(),
-1,
1
);
public Command setSpeed(DoubleSupplier speed) {
return run(() -> {
double realSpeedTarget = clampedSpeed * ManipulatorPivotConstants.kPivotMaxVelocity;
double voltsOut = velocityController.calculate(
getEncoderVelocity(),
realSpeedTarget
) + feedForward.calculate(
getEncoderPosition(),
getEncoderVelocity()
);
armMotor.setVoltage(voltsOut);
armMotor.set(speed.getAsDouble());
});
}
*/
/**
* Moves the arm to a target destination (setpoint)
*