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

Compare commits

base: Team_2648:56980d3772caf6236c61ff03b2017c81ed1fdf87
Branches Tags
Team_2648:main
Team_2648:faster_elevator
Team_2648:maybe_fix_field_oriented
Team_2648:brad_elevator_control_rework
Team_2648:kraken_swerve
Team_2648:2025-2-1-setup
..
compare: Team_2648:89c1914d11b765bcedcd008974085f2f0c8206b2
Branches Tags
Team_2648:main
Team_2648:faster_elevator
Team_2648:maybe_fix_field_oriented
Team_2648:brad_elevator_control_rework
Team_2648:kraken_swerve
Team_2648:2025-2-1-setup

No commits in common. "56980d3772caf6236c61ff03b2017c81ed1fdf87" and "89c1914d11b765bcedcd008974085f2f0c8206b2" have entirely different histories.
56980d3772 ... 89c1914d11

6 changed files with 35 additions and 75 deletions
Show Stats Expand all files Collapse all files

20
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(
@ -192,12 +192,10 @@ public class RobotContainer {
.withPosition(4, 1)
.withWidget(BuiltInWidgets.kBooleanBox);
/*
sensorTab.addBoolean("Algae Sensor", manipulator::getAlgaePhotoSwitch)
.withSize(1, 1)
.withPosition(4, 0)
.withWidget(BuiltInWidgets.kBooleanBox);
*/
}
public Command getAutonomousCommand() {
@ -213,8 +211,8 @@ public class RobotContainer {
ElevatorConstants.kCoralIntakePosition,
ManipulatorPivotConstants.kCoralIntakePosition
)
.andThen(manipulator.runUntilCollected(1));
}
.andThen(manipulator.runUntilCollected(1, true));
}
/**
* Moves the elevator and arm to the constant setpoints and runs the manipulator until collected
@ -227,7 +225,7 @@ public class RobotContainer {
l2 ? ElevatorConstants.kL2AlgaePosition : ElevatorConstants.kL3AlgaePosition,
l2 ? ManipulatorPivotConstants.kL2AlgaePosition : ManipulatorPivotConstants.kL3AlgaePosition
)
.andThen(manipulator.runUntilCollected(1));
.andThen(manipulator.runUntilCollected(1, false));
}
/**

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

@ -14,42 +14,37 @@ 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/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 double kEncoderConversionFactor = 0;
public static final int kCurrentLimit = 40;
public static final int kMotorAmpsMax = 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.53; // calculated value
public static final double kFeedForwardV = 4.78; // calculated value
public static final double kFeedForwardG = 0.41; // calculated value
public static final double kFeedForwardV = 1.75; // 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 = 14.5;
public static final double kL3Position = 29.0;
public static final double kL4Position = 53.0;
public static final double kL2Position = 0;
public static final double kL3Position = 0;
public static final double kL4Position = 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 = 53.0;
public static final double kMaxHeight = 0;
// 1, 7, 10 are the defaults for these, change as necessary
public static final double kSysIDRampRate = 1;
@ -70,19 +65,10 @@ public class ElevatorConstants {
static {
motorConfig
.smartCurrentLimit(kCurrentLimit)
.smartCurrentLimit(kMotorAmpsMax)
.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);
}
}

3
src/main/java/frc/robot/constants/ManipulatorPivotConstants.java
View File

@ -19,8 +19,6 @@ public class ManipulatorPivotConstants {
public static final int kMotorAmpsMax = 40;
public static final double kPivotConversion = 12/60 * 20/60 * 28/12;
public static final double kPivotMaxVelocity = 0;
public static final double kPositionalP = 0;
@ -83,7 +81,6 @@ public class ManipulatorPivotConstants {
motorConfig
.smartCurrentLimit(kMotorAmpsMax)
.idleMode(kIdleMode);
motorConfig.encoder.positionConversionFactor(kPivotConversion);
}

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

@ -3,9 +3,6 @@ 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;
@ -24,14 +21,12 @@ 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;
@ -46,8 +41,6 @@ public class Elevator extends SubsystemBase {
MotorType.kBrushless
);
elevatorClosedLoop = elevatorMotor1.getClosedLoopController();
elevatorMotor1.configure(
ElevatorConstants.motorConfig,
ResetMode.kResetSafeParameters,
@ -72,13 +65,11 @@ public class Elevator extends SubsystemBase {
ElevatorConstants.kPositionControllerD
);
/*
velocityController = new PIDController(
ElevatorConstants.kVelocityControllerP,
ElevatorConstants.kVelocityControllerI,
ElevatorConstants.kVelocityControllerD
);
*/
feedForward = new ElevatorFeedforward(
ElevatorConstants.kFeedForwardS,
@ -132,7 +123,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;
@ -146,7 +137,6 @@ public class Elevator extends SubsystemBase {
}).until(
() -> bottomLimitSwitch.get() || encoder.getPosition() >= ElevatorConstants.kMaxHeight);
}
*/
/**
* Moves the elevator to a target destination (setpoint).
@ -163,16 +153,6 @@ 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(
@ -201,8 +181,6 @@ public class Elevator extends SubsystemBase {
() -> positionController.atSetpoint() || bottomLimitSwitch.get()
).withTimeout(timeout);
}
*/
}
/**

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

@ -16,7 +16,7 @@ public class Manipulator extends SubsystemBase {
private SparkMax manipulatorMotor;
private DigitalInput coralBeamBreak;
// private DigitalInput algaeBeamBreak;
private DigitalInput algaeBeamBreak;
public Manipulator() {
manipulatorMotor = new SparkMax(
@ -31,7 +31,7 @@ public class Manipulator extends SubsystemBase {
);
coralBeamBreak = new DigitalInput(ManipulatorConstants.kCoralBeamBreakID);
// algaeBeamBreak = new DigitalInput(ManipulatorConstants.kAlgaeBeamBreakID);
algaeBeamBreak = new DigitalInput(ManipulatorConstants.kAlgaeBeamBreakID);
}
/**
@ -42,8 +42,10 @@ public class Manipulator extends SubsystemBase {
*/
public Command defaultCommand() {
return run(() -> {
runUntilCollected(0.1);
});
manipulatorMotor.set(
algaeBeamBreak.get() ? 0.1 : 0
);
});
}
/**
@ -68,15 +70,19 @@ public class Manipulator extends SubsystemBase {
* @param coral Is the object a coral? (True = Coral, False = Algae)
* @return Returns a command that sets the speed of the motor
*/
public Command runUntilCollected(double speed) {
public Command runUntilCollected(double speed, boolean coral) {
return run(() -> {
manipulatorMotor.set(
speed * -1
coral ? speed : speed * -1
);
}).until(() -> coralBeamBreak.get());
}).until(() -> coralBeamBreak.get() || algaeBeamBreak.get());
}
public boolean getCoralBeamBreak() {
return coralBeamBreak.get();
}
public boolean getAlgaePhotoSwitch() {
return algaeBeamBreak.get();
}
}

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

@ -11,8 +11,6 @@ public class Vision{
private DoubleSubscriber cam1ClosestTag;
private BooleanSubscriber cam1TagDetected;
private DoubleSubscriber cam1TimeStamp;
private DoubleSubscriber framerate;
public Vision(){
@ -24,6 +22,7 @@ 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);
}
@ -40,10 +39,6 @@ public class Vision{
return cam1TagDetected.get();
}
public double getCam1TimeStamp(){
return cam1GlobalPose.getLastChange();
}
public double getFPS(){
return framerate.get();
}