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

Compare commits

base: Team_2648:5f111cd4ee4d88ec76e289d25401ca43a677dbd9
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:f4908e4051cd2c1ce41df73debe5e7e2cc506c4a
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. "5f111cd4ee4d88ec76e289d25401ca43a677dbd9" and "f4908e4051cd2c1ce41df73debe5e7e2cc506c4a" have entirely different histories.
5f111cd4ee ... f4908e4051

8 changed files with 35 additions and 114 deletions
Show Stats Expand all files Collapse all files

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

@ -61,7 +61,7 @@ public class RobotContainer {
);
climberPivot.setDefaultCommand(
climberPivot.goToAngle(0, 1)
climberPivot.goToAngle(0)
);
climberRollers.setDefaultCommand(
@ -78,7 +78,7 @@ public class RobotContainer {
);
elevator.setDefaultCommand(
elevator.runElevator(operator::getLeftY)
elevator.goToSetpoint(0, 1)
);
indexer.setDefaultCommand(

2
src/main/java/frc/robot/constants/ArmConstants.java
View File

@ -5,6 +5,4 @@ public class ArmConstants {
public static final int kCANcoderID = 0;
public static final double kEncoderConversionFactor = 0;
public static final double kArmMaxVelocity = 0;
}

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

@ -11,23 +11,15 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
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;
public static final int kElevatorMotorID = 0;
public static final double kEncoderConversionFactor = 0;
public static final int kMotorAmpsMax = 0;
public static final double kPositionControllerP = 0;
public static final double kPositionControllerI = 0;
public static final double kPositionControllerD = 0;
public static final double kVelocityControllerP = 0;
public static final double kVelocityControllerI = 0;
public static final double kVelocityControllerD = 0;
public static final double kPIDControllerP = 0;
public static final double kPIDControllerI = 0;
public static final double kPIDControllerD = 0;
public static final double kFeedForwardS = 0;
public static final double kFeedForwardG = 0;

37
src/main/java/frc/robot/subsystems/Arm.java
View File

@ -1,7 +1,5 @@
package frc.robot.subsystems;
import java.util.function.DoubleSupplier;
import com.ctre.phoenix6.hardware.CANcoder;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.SparkLowLevel.MotorType;
@ -13,17 +11,15 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.constants.ArmConstants;
public class Arm extends SubsystemBase {
private SparkMax armMotor;
private SparkMax ArmMotor;
private CANcoder canCoder;
private PIDController positionController;
private PIDController velocityController;
private PIDController pidController;
private ArmFeedforward feedForward;
public Arm() {
armMotor = new SparkMax(
ArmMotor = new SparkMax(
ArmConstants.kArmMotorID,
MotorType.kBrushless
);
@ -31,26 +27,9 @@ public class Arm extends SubsystemBase {
canCoder = new CANcoder(ArmConstants.kCANcoderID);
}
//manual command that keeps ouput speed consistent no matter the direction
public Command runArm(DoubleSupplier speed) {
return run(() -> {
double realSpeedTarget = speed.getAsDouble() * ArmConstants.kArmMaxVelocity;
double voltsOut = velocityController.calculate(
rotationsToRadians(canCoder.getVelocity().getValueAsDouble()),
realSpeedTarget
) + feedForward.calculate(
rotationsToRadians(canCoder.getPosition().getValueAsDouble()),
canCoder.getVelocity().getValueAsDouble()
);
armMotor.setVoltage(voltsOut);
});
}
public Command goToSetpoint(double setpoint, double timeout) {
return run(() -> {
double voltsOut = positionController.calculate(
double voltsOut = pidController.calculate(
canCoder.getPosition().getValueAsDouble(),
setpoint
) + feedForward.calculate(
@ -58,11 +37,7 @@ public class Arm extends SubsystemBase {
canCoder.getVelocity().getValueAsDouble()
);
armMotor.setVoltage(voltsOut);
}).until(positionController::atSetpoint).withTimeout(timeout);
}
protected double rotationsToRadians(double rotations) {
return rotations * 2 * Math.PI;
ArmMotor.setVoltage(voltsOut);
}).until(pidController::atSetpoint).withTimeout(timeout);
}
}

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

@ -1,6 +1,6 @@
package frc.robot.subsystems;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.AbsoluteEncoder;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.SparkLowLevel.MotorType;
@ -13,7 +13,7 @@ import frc.robot.constants.ClimberPivotConstants;
public class ClimberPivot extends SubsystemBase {
private SparkMax pivotMotor;
private RelativeEncoder neoEncoder;
private AbsoluteEncoder neoEncoder;
private DigitalInput cageLimitSwitch;
@ -25,9 +25,9 @@ public class ClimberPivot extends SubsystemBase {
MotorType.kBrushless
);
neoEncoder = pivotMotor.getEncoder();
neoEncoder = pivotMotor.getAbsoluteEncoder();
cageLimitSwitch = new DigitalInput(ClimberPivotConstants.kClimberLimitSwitchID);
cageLimitSwitch = new DigitalInput(0);
pidController = new PIDController(
ClimberPivotConstants.kPIDControllerP,
@ -42,7 +42,7 @@ public class ClimberPivot extends SubsystemBase {
});
}
public Command goToAngle(double setpoint, double timeout) {
public Command goToAngle(double setpoint) {
return run(() -> {
pivotMotor.set(
pidController.calculate(
@ -50,7 +50,7 @@ public class ClimberPivot extends SubsystemBase {
setpoint
)
);
}).withTimeout(timeout);
});
}
public boolean getCageLimitSwitch() {

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

@ -1,44 +1,29 @@
package frc.robot.subsystems;
import java.util.function.DoubleSupplier;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.SparkBase.PersistMode;
import com.revrobotics.spark.SparkBase.ResetMode;
import com.revrobotics.spark.SparkBase.PersistMode;
import com.revrobotics.spark.SparkBase.ResetMode;
import com.revrobotics.spark.SparkLowLevel.MotorType;
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;
import frc.robot.constants.ElevatorConstants;
public class Elevator extends SubsystemBase {
protected SparkMax elevatorMotor1;
protected SparkMax elevatorMotor2;
protected RelativeEncoder encoder;
private DigitalInput topLimitSwitch;
private DigitalInput bottomLimitSwitch;
private PIDController positionController;
private PIDController velocityController;
private PIDController pidController;
private ElevatorFeedforward feedForward;
public Elevator() {
elevatorMotor1 = new SparkMax(
ElevatorConstants.kElevatorMotor1ID,
MotorType.kBrushless
);
elevatorMotor2 = new SparkMax(
ElevatorConstants.kElevatorMotor2ID,
ElevatorConstants.kElevatorMotorID,
MotorType.kBrushless
);
@ -48,32 +33,12 @@ public class Elevator extends SubsystemBase {
PersistMode.kPersistParameters
);
elevatorMotor2.configure(
ElevatorConstants.motorConfig.follow(ElevatorConstants.kElevatorMotor1ID),
ResetMode.kResetSafeParameters,
PersistMode.kPersistParameters
);
encoder = elevatorMotor1.getEncoder();
topLimitSwitch = new DigitalInput(
ElevatorConstants.kTopLimitSwitchID
);
bottomLimitSwitch = new DigitalInput(
ElevatorConstants.kBottomLimitSwitchID
);
positionController = new PIDController(
ElevatorConstants.kPositionControllerP,
ElevatorConstants.kPositionControllerI,
ElevatorConstants.kPositionControllerD
);
velocityController = new PIDController(
ElevatorConstants.kVelocityControllerP,
ElevatorConstants.kVelocityControllerI,
ElevatorConstants.kVelocityControllerD
pidController = new PIDController(
ElevatorConstants.kPIDControllerP,
ElevatorConstants.kPIDControllerI,
ElevatorConstants.kPIDControllerD
);
feedForward = new ElevatorFeedforward(
@ -83,32 +48,20 @@ public class Elevator extends SubsystemBase {
);
}
//manual command that keeps ouput speed consistent no matter the direction
public Command runElevator(DoubleSupplier speed) {
public Command runElevator(double speed) {
return run(() -> {
double realSpeedTarget = speed.getAsDouble() * ElevatorConstants.kElevatorMaxVelocity;
double voltsOut = velocityController.calculate(
encoder.getVelocity(),
realSpeedTarget
) + feedForward.calculate(realSpeedTarget);
elevatorMotor1.setVoltage(voltsOut);
}).until(() -> topLimitSwitch.get() || bottomLimitSwitch.get());
elevatorMotor1.set(speed);
});
}
//go to setpoint command
public Command goToSetpoint(double setpoint, double timeout) {
return run(() -> {
double voltsOut = positionController.calculate(
double voltsOut = pidController.calculate(
encoder.getPosition(),
setpoint
) + feedForward.calculate(0);
elevatorMotor1.setVoltage(voltsOut);
}).until(
() -> positionController.atSetpoint() || topLimitSwitch.get() || bottomLimitSwitch.get()
).withTimeout(timeout);
}).until(pidController::atSetpoint).withTimeout(timeout);
}
}

3
src/main/java/frc/robot/subsystems/Indexer.java
View File

@ -31,6 +31,7 @@ public class Indexer extends SubsystemBase {
public Command indexCoral(double speed) {
return run(() -> {
indexerMotor.set(speed);
}).until(indexerBeamBreak::get);
})
.until(indexerBeamBreak::get);
}
}

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

@ -33,12 +33,14 @@ public class Manipulator extends SubsystemBase {
public Command runUntilCollected(double speed, boolean coral) {
return run(() -> {
manipulatorMotor.set(coral ? speed : speed * -1);
}).until(() -> coralBeamBreak.get() || algaeBeamBreak.get());
})
.until(() -> coralBeamBreak.get() || algaeBeamBreak.get());
}
public Command indexCoral(double speed) {
return run(() -> {
manipulatorMotor.set(speed);
}).until(coralBeamBreak::get);
})
.until(coralBeamBreak::get);
}
}