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

removed velocity controllers on position mechanisms and added controller PID for elevator

Browse Source
This commit is contained in:
Tylr-J42 2025-02-08 03:27:59 -05:00
parent 6fa4377e52
commit 56980d3772
4 changed files with 60 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -89,17 +89,17 @@ public class RobotContainer {
)
);
elevator.setDefaultCommand(
elevator.runAssistedElevator(operator::getLeftY)
);
//elevator.setDefaultCommand(
//elevator.runAssistedElevator(operator::getLeftY)
// );
manipulator.setDefaultCommand(
manipulator.defaultCommand()
);
manipulatorPivot.setDefaultCommand(
manipulatorPivot.runAssistedPivot(operator::getRightY)
);
//manipulatorPivot.setDefaultCommand(
// manipulatorPivot.runAssistedPivot(operator::getRightY)
//);
//Driver inputs
driver.start().whileTrue(

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

@ -14,38 +14,42 @@ public class ElevatorConstants {
public static final int kElevatorMotor1ID = 0;
public static final int kElevatorMotor2ID = 0;
public static final int kTopLimitSwitchID = 0;
public static final int kBottomLimitSwitchID = 0;
// 60/12 gearing multiplied by circumference of sprocket multiplied by 2 for carriage position
public static final double kEncoderConversionFactor = 11/60 * Math.PI * 1.75 * 2;
// 60/11 gearing multiplied by circumference of sprocket multiplied by 2 for carriage position
public static final double kEncoderConversionFactor = 11/60 * (22*0.25) * 2;
public static final int kMotorAmpsMax = 40;
public static final int kCurrentLimit = 40;
public static final double kPositionControllerP = 0;
public static final double kPositionControllerI = 0;
public static final double kPositionControllerD = 0;
public static final double kAllowedError = 0.25;
/*
public static final double kVelocityControllerP = 0;
public static final double kVelocityControllerI = 0;
public static final double kVelocityControllerD = 0;
*/
public static final double kFeedForwardS = 0;
public static final double kFeedForwardG = 0.41; // calculated value
public static final double kFeedForwardV = 1.75; // calculated value
public static final double kFeedForwardG = 0.53; // calculated value
public static final double kFeedForwardV = 4.78; // calculated value
public static final double kMaxVelocity = 0;
public static final double kMaxAcceleration = 0;
public static final double kCoralIntakePosition = 0;
public static final double kL1Position = 0;
public static final double kL2Position = 0;
public static final double kL3Position = 0;
public static final double kL4Position = 0;
public static final double kL2Position = 14.5;
public static final double kL3Position = 29.0;
public static final double kL4Position = 53.0;
public static final double kL2AlgaePosition = 0;
public static final double kL3AlgaePosition = 0;
/**The position of the top of the elevator brace */
public static final double kBracePosition = 0;
public static final double kMaxHeight = 0;
public static final double kMaxHeight = 53.0;
// 1, 7, 10 are the defaults for these, change as necessary
public static final double kSysIDRampRate = 1;
@ -66,10 +70,19 @@ public class ElevatorConstants {
static {
motorConfig
.smartCurrentLimit(kMotorAmpsMax)
.smartCurrentLimit(kCurrentLimit)
.idleMode(kIdleMode);
motorConfig.encoder
.positionConversionFactor(kEncoderConversionFactor)
.velocityConversionFactor(kEncoderConversionFactor / 60.0);
motorConfig.closedLoop
.p(kPositionControllerP)
.i(kPositionControllerI)
.d(kPositionControllerD)
.velocityFF(0.0); // keep at zero for position pid
motorConfig.closedLoop.maxMotion
.maxAcceleration(kMaxAcceleration)
.maxVelocity(kMaxVelocity)
.allowedClosedLoopError(kAllowedError);
}
}

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

@ -3,6 +3,9 @@ package frc.robot.subsystems;
import java.util.function.DoubleSupplier;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.spark.ClosedLoopSlot;
import com.revrobotics.spark.SparkBase;
import com.revrobotics.spark.SparkClosedLoopController;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.SparkBase.PersistMode;
import com.revrobotics.spark.SparkBase.ResetMode;
@ -21,12 +24,14 @@ public class Elevator extends SubsystemBase {
protected SparkMax elevatorMotor1;
protected SparkMax elevatorMotor2;
private SparkClosedLoopController elevatorClosedLoop;
protected RelativeEncoder encoder;
private DigitalInput bottomLimitSwitch;
private PIDController positionController;
private PIDController velocityController;
//private PIDController velocityController;
private ElevatorFeedforward feedForward;
@ -41,6 +46,8 @@ public class Elevator extends SubsystemBase {
MotorType.kBrushless
);
elevatorClosedLoop = elevatorMotor1.getClosedLoopController();
elevatorMotor1.configure(
ElevatorConstants.motorConfig,
ResetMode.kResetSafeParameters,
@ -65,11 +72,13 @@ public class Elevator extends SubsystemBase {
ElevatorConstants.kPositionControllerD
);
/*
velocityController = new PIDController(
ElevatorConstants.kVelocityControllerP,
ElevatorConstants.kVelocityControllerI,
ElevatorConstants.kVelocityControllerD
);
*/
feedForward = new ElevatorFeedforward(
ElevatorConstants.kFeedForwardS,
@ -123,7 +132,7 @@ public class Elevator extends SubsystemBase {
*
* @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;
@ -137,6 +146,7 @@ public class Elevator extends SubsystemBase {
}).until(
() -> bottomLimitSwitch.get() || encoder.getPosition() >= ElevatorConstants.kMaxHeight);
}
*/
/**
* Moves the elevator to a target destination (setpoint).
@ -153,6 +163,16 @@ public class Elevator extends SubsystemBase {
ElevatorConstants.kMaxHeight
);
return run(() -> {
elevatorClosedLoop.setReference(clampedSetpoint,
SparkBase.ControlType.kMAXMotionPositionControl,
ClosedLoopSlot.kSlot0,
feedForward.calculate(0)
);
});
/*
if (clampedSetpoint == 0) {
return run(() -> {
double voltsOut = positionController.calculate(
@ -181,6 +201,8 @@ public class Elevator extends SubsystemBase {
() -> positionController.atSetpoint() || bottomLimitSwitch.get()
).withTimeout(timeout);
}
*/
}
/**

7
src/main/java/frc/robot/subsystems/Vision.java
View File

@ -11,6 +11,8 @@ public class Vision{
private DoubleSubscriber cam1ClosestTag;
private BooleanSubscriber cam1TagDetected;
private DoubleSubscriber cam1TimeStamp;
private DoubleSubscriber framerate;
public Vision(){
@ -22,7 +24,6 @@ public class Vision{
cam1ClosestTag = visionTable.getDoubleTopic("cam1ClosestTag").subscribe(0.0);
cam1TagDetected = visionTable.getBooleanTopic("cam1_visible").subscribe(false);
// cam2TagDetected = visionTable.getBooleanTopic("cam2_visible").subscribe(false);
framerate = visionTable.getDoubleTopic("fps").subscribe(0.0);
}
@ -39,6 +40,10 @@ public class Vision{
return cam1TagDetected.get();
}
public double getCam1TimeStamp(){
return cam1GlobalPose.getLastChange();
}
public double getFPS(){
return framerate.get();
}