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;
import com.ctre.phoenix6.configs.CANcoderConfiguration;
public class ArmConstants {
public static final int kArmMotorID = 0;
public static final int kCANcoderID = 0;
@ -16,4 +18,8 @@ public class ArmConstants {
public static final double kArmL2AlgaePosition = 0;
public static final double kArmL3AlgaePosition = 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.PIDController;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.constants.ArmConstants;
@ -58,11 +59,11 @@ public class Arm extends SubsystemBase {
double realSpeedTarget = speed.getAsDouble() * ArmConstants.kArmMaxVelocity;
double voltsOut = velocityController.calculate(
rotationsToRadians(canCoder.getVelocity().getValueAsDouble()),
getEncoderVelocity(),
realSpeedTarget
) + feedForward.calculate(
rotationsToRadians(canCoder.getPosition().getValueAsDouble()),
canCoder.getVelocity().getValueAsDouble()
getEncoderPosition(),
getEncoderVelocity()
);
armMotor.setVoltage(voltsOut);
@ -72,11 +73,11 @@ public class Arm extends SubsystemBase {
public Command goToSetpoint(double setpoint, double timeout) {
return run(() -> {
double voltsOut = positionController.calculate(
canCoder.getPosition().getValueAsDouble(),
getEncoderPosition(),
setpoint
) + feedForward.calculate(
canCoder.getPosition().getValueAsDouble(),
canCoder.getVelocity().getValueAsDouble()
getEncoderPosition(),
getEncoderVelocity()
);
armMotor.setVoltage(voltsOut);
@ -84,10 +85,10 @@ public class Arm extends SubsystemBase {
}
public double getEncoderPosition() {
return canCoder.getPosition().getValueAsDouble();
return Units.rotationsToRadians(canCoder.getPosition().getValueAsDouble());
}
protected double rotationsToRadians(double rotations) {
return rotations * 2 * Math.PI;
public double getEncoderVelocity() {
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;
private DigitalInput topLimitSwitch;
private DigitalInput bottomLimitSwitch;
private PIDController positionController;
@ -54,10 +53,6 @@ public class Elevator extends SubsystemBase {
encoder = elevatorMotor1.getEncoder();
topLimitSwitch = new DigitalInput(
ElevatorConstants.kTopLimitSwitchID
);
bottomLimitSwitch = new DigitalInput(
ElevatorConstants.kBottomLimitSwitchID
);
@ -113,7 +108,7 @@ public class Elevator extends SubsystemBase {
) + feedForward.calculate(realSpeedTarget);
elevatorMotor1.setVoltage(voltsOut);
}).until(() -> topLimitSwitch.get() || bottomLimitSwitch.get());
}).until(bottomLimitSwitch::get);
}
@ -127,7 +122,7 @@ public class Elevator extends SubsystemBase {
elevatorMotor1.setVoltage(voltsOut);
}).until(
() -> positionController.atSetpoint() || topLimitSwitch.get() || bottomLimitSwitch.get()
() -> positionController.atSetpoint() || bottomLimitSwitch.get()
).withTimeout(timeout);
}