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

safe travels command and constants

Browse Source
This commit is contained in:
Tylr-J42 2025-02-11 14:27:29 -05:00
parent 619b3f4b7f
commit aff9a4f2cb
3 changed files with 29 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -95,6 +95,8 @@ public class RobotContainer {
elevator.setDefaultCommand(moveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kCoralIntakePosition));
manipulatorPivot.setDefaultCommand(moveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kCoralIntakePosition));
manipulator.setDefaultCommand(
manipulator.defaultCommand()
);
@ -295,6 +297,22 @@ public class RobotContainer {
);
}
private Command manipulatorSafeTravel(double elevatorPosition, double armPosition, boolean isL4){
if(!isL4){
return Commands.sequence(
manipulatorPivot.goToSetpoint(ManipulatorPivotConstants.kArmSafeStowPosition),
elevator.goToSetpoint(elevatorPosition),
manipulatorPivot.goToSetpoint(armPosition));
}else{
return Commands.sequence(
manipulatorPivot.goToSetpoint(ManipulatorPivotConstants.kArmSafeStowPosition),
elevator.goToSetpoint(elevatorPosition).until(() -> elevator.getEncoderPosition() > ElevatorConstants.kL4TransitionPosition),
Commands.parallel( manipulatorPivot.goToSetpoint(armPosition)), elevator.goToSetpoint(elevatorPosition));
}
}
/**
* Moves the arm and elevator in a safe way.
*

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

@ -45,10 +45,12 @@ public class ElevatorConstants {
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;
public static final double kL4TransitionPosition = 40.0;
public static final double kL2AlgaePosition = 22.0;
public static final double kL3AlgaePosition = 39.0;
public static final double kProcessorPosition = 4.0;
/**The position of the top of the elevator brace */
public static final double kBracePosition = 0;
public static final double kBracePosition = 5.5;
public static final double kMaxHeight = 53.0;
// 1, 7, 10 are the defaults for these, change as necessary

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

@ -4,7 +4,6 @@ import static edu.wpi.first.units.Units.Volts;
import static edu.wpi.first.units.Units.Second;
import static edu.wpi.first.units.Units.Seconds;
import com.ctre.phoenix6.configs.CANcoderConfiguration;
import com.ctre.phoenix6.signals.SensorDirectionValue;
import com.revrobotics.spark.config.SparkMaxConfig;
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
@ -15,9 +14,8 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
public class ManipulatorPivotConstants {
public static final int kArmMotorID = 0;
public static final int kCANcoderID = 0;
public static final int kMotorAmpsMax = 40;
public static final int kMotorCurrentMax = 40;
public static final double kPivotConversion = 12/60 * 20/60 * 12/28;
@ -41,9 +39,10 @@ public class ManipulatorPivotConstants {
public static final double kL1Position = Units.degreesToRadians(0.0);
public static final double kL2Position = Units.degreesToRadians(25.0);
public static final double kL3Position = Units.degreesToRadians(60.0);
public static final double kL4Position = 0;
public static final double kL2AlgaePosition = 0;
public static final double kL3AlgaePosition = 0;
public static final double kL4Position = Units.degreesToRadians(45.0);
public static final double kL2AlgaePosition = Units.degreesToRadians(175.0);
public static final double kL3AlgaePosition = Units.degreesToRadians(175.0);
public static final double kProcesserPosition = Units.degreesToRadians(175.0);
/**The closest position to the elevator brace without hitting it */
public static final double kArmSafeStowPosition = Units.degreesToRadians(60.0);
/**The forward rotation limit of the arm */
@ -65,14 +64,12 @@ public class ManipulatorPivotConstants {
Seconds.of(kSysIDTimeout)
);
public static final CANcoderConfiguration canCoderConfig = new CANcoderConfiguration();
public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
static {
motorConfig
.smartCurrentLimit(kMotorAmpsMax)
.smartCurrentLimit(kMotorCurrentMax)
.idleMode(kIdleMode);
motorConfig.absoluteEncoder.positionConversionFactor(kPivotConversion);
motorConfig.closedLoop