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

did small amounts of stuff. can't remember

Browse Source
This commit is contained in:
NoahLacks63 2025-01-18 15:47:18 +00:00
parent 5325920b42
commit 91cd13f87a
3 changed files with 18 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1,5 +1,7 @@
package frc.robot.constants; package frc.robot.constants;
import com.ctre.phoenix6.configs.CANcoderConfiguration;
public class ArmConstants { public class ArmConstants {
public static final int kArmMotorID = 0; public static final int kArmMotorID = 0;
public static final int kCANcoderID = 0; public static final int kCANcoderID = 0;
@ -16,4 +18,8 @@ public class ArmConstants {
public static final double kArmL2AlgaePosition = 0; public static final double kArmL2AlgaePosition = 0;
public static final double kArmL3AlgaePosition = 0; public static final double kArmL3AlgaePosition = 0;
public static final double kArmSafeStowPosition = 0; public static final double kArmSafeStowPosition = 0;
public static final CANcoderConfiguration canCoderConfig = new CANcoderConfiguration();
} }

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

@ -8,6 +8,7 @@ import com.revrobotics.spark.SparkLowLevel.MotorType;
import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.ArmFeedforward;
import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.constants.ArmConstants; import frc.robot.constants.ArmConstants;
@ -58,11 +59,11 @@ public class Arm extends SubsystemBase {
double realSpeedTarget = speed.getAsDouble() * ArmConstants.kArmMaxVelocity; double realSpeedTarget = speed.getAsDouble() * ArmConstants.kArmMaxVelocity;
double voltsOut = velocityController.calculate( double voltsOut = velocityController.calculate(
rotationsToRadians(canCoder.getVelocity().getValueAsDouble()), getEncoderVelocity(),
realSpeedTarget realSpeedTarget
) + feedForward.calculate( ) + feedForward.calculate(
rotationsToRadians(canCoder.getPosition().getValueAsDouble()), getEncoderPosition(),
canCoder.getVelocity().getValueAsDouble() getEncoderVelocity()
); );
armMotor.setVoltage(voltsOut); armMotor.setVoltage(voltsOut);
@ -72,11 +73,11 @@ public class Arm extends SubsystemBase {
public Command goToSetpoint(double setpoint, double timeout) { public Command goToSetpoint(double setpoint, double timeout) {
return run(() -> { return run(() -> {
double voltsOut = positionController.calculate( double voltsOut = positionController.calculate(
canCoder.getPosition().getValueAsDouble(), getEncoderPosition(),
setpoint setpoint
) + feedForward.calculate( ) + feedForward.calculate(
canCoder.getPosition().getValueAsDouble(), getEncoderPosition(),
canCoder.getVelocity().getValueAsDouble() getEncoderVelocity()
); );
armMotor.setVoltage(voltsOut); armMotor.setVoltage(voltsOut);
@ -84,10 +85,10 @@ public class Arm extends SubsystemBase {
} }
public double getEncoderPosition() { public double getEncoderPosition() {
return canCoder.getPosition().getValueAsDouble(); return Units.rotationsToRadians(canCoder.getPosition().getValueAsDouble());
} }
protected double rotationsToRadians(double rotations) { public double getEncoderVelocity() {
return rotations * 2 * Math.PI; return Units.rotationsToRadians(canCoder.getVelocity().getValueAsDouble());
} }
} }

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

@ -21,7 +21,6 @@ public class Elevator extends SubsystemBase {
protected RelativeEncoder encoder; protected RelativeEncoder encoder;
private DigitalInput topLimitSwitch;
private DigitalInput bottomLimitSwitch; private DigitalInput bottomLimitSwitch;
private PIDController positionController; private PIDController positionController;
@ -54,10 +53,6 @@ public class Elevator extends SubsystemBase {
encoder = elevatorMotor1.getEncoder(); encoder = elevatorMotor1.getEncoder();
topLimitSwitch = new DigitalInput(
ElevatorConstants.kTopLimitSwitchID
);
bottomLimitSwitch = new DigitalInput( bottomLimitSwitch = new DigitalInput(
ElevatorConstants.kBottomLimitSwitchID ElevatorConstants.kBottomLimitSwitchID
); );
@ -113,7 +108,7 @@ public class Elevator extends SubsystemBase {
) + feedForward.calculate(realSpeedTarget); ) + feedForward.calculate(realSpeedTarget);
elevatorMotor1.setVoltage(voltsOut); elevatorMotor1.setVoltage(voltsOut);
}).until(() -> topLimitSwitch.get() || bottomLimitSwitch.get()); }).until(bottomLimitSwitch::get);
} }
@ -127,7 +122,7 @@ public class Elevator extends SubsystemBase {
elevatorMotor1.setVoltage(voltsOut); elevatorMotor1.setVoltage(voltsOut);
}).until( }).until(
() -> positionController.atSetpoint() || topLimitSwitch.get() || bottomLimitSwitch.get() () -> positionController.atSetpoint() || bottomLimitSwitch.get()
).withTimeout(timeout); ).withTimeout(timeout);
} }