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

began adding elevator and arm methods

Browse Source
This commit is contained in:
NoahLacks63 2025-01-16 15:26:54 +00:00
parent f391d1540b
commit f4cfd2874b
5 changed files with 67 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -4,6 +4,8 @@
package frc.robot;
import frc.robot.constants.ArmConstants;
import frc.robot.constants.ElevatorConstants;
import frc.robot.constants.OIConstants;
import frc.robot.subsystems.Arm;
import frc.robot.subsystems.ClimberPivot;
@ -100,23 +102,58 @@ public class RobotContainer {
//Operator inputs
operator.povUp().onTrue(
elevator.goToSetpoint(0, 0)
moveManipulator(
ElevatorConstants.kElevatorL4Position,
ArmConstants.kArmL4Position
)
);
operator.povRight().onTrue(
elevator.goToSetpoint(0, 0)
moveManipulator(
ElevatorConstants.kElevatorL3Position,
ArmConstants.kArmL3Position
)
);
operator.povLeft().onTrue(
elevator.goToSetpoint(0, 0)
moveManipulator(
ElevatorConstants.kElevatorL2Position,
ArmConstants.kArmL2Position
)
);
operator.povDown().onTrue(
elevator.goToSetpoint(0, 0)
moveManipulator(
ElevatorConstants.kElevatorL1Position,
ArmConstants.kArmL1Position
)
);
operator.a().onTrue(
coralIntakeRoutine()
);
}
public Command getAutonomousCommand() {
return new PrintCommand("NO AUTO DEFINED");
}
//teleop routines
private Command coralIntakeRoutine() {
return moveManipulator(
ElevatorConstants.kElevatorCoralIntakePosition,
ArmConstants.kArmCoralIntakePosition
)
.andThen(manipulator.indexCoral(.5));
}
private Command moveManipulator(double elevatorPosition, double armPosition) {
if ((arm.getEncoderPosition() < ArmConstants.kArmSafeStowPosition && armPosition < ArmConstants.kArmSafeStowPosition) || (elevator.getEncoderPosition() > ElevatorConstants.kElevatorBracePosition && elevatorPosition > ElevatorConstants.kElevatorBracePosition)) {
return arm.goToSetpoint(armPosition, 2)
.alongWith(elevator.goToSetpoint(elevatorPosition, 2));
} else {
return arm.goToSetpoint(armPosition, 2)
.andThen(elevator.goToSetpoint(elevatorPosition, 2));
}
}
}

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

@ -7,4 +7,13 @@ public class ArmConstants {
public static final double kEncoderConversionFactor = 0;
public static final double kArmMaxVelocity = 0;
public static final double kArmCoralIntakePosition = 0;
public static final double kArmL1Position = 0;
public static final double kArmL2Position = 0;
public static final double kArmL3Position = 0;
public static final double kArmL4Position = 0;
public static final double kArmL2AlgaePosition = 0;
public static final double kArmL3AlgaePosition = 0;
public static final double kArmSafeStowPosition = 0;
}

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

@ -35,6 +35,15 @@ public class ElevatorConstants {
public static final double kElevatorMaxVelocity = 0;
public static final double kElevatorCoralIntakePosition = 0;
public static final double kElevatorL1Position = 0;
public static final double kElevatorL2Position = 0;
public static final double kElevatorL3Position = 0;
public static final double kElevatorL4Position = 0;
public static final double kElevatorL2AlgaePosition = 0;
public static final double kElevatorL3AlgaePosition = 0;
public static final double kElevatorBracePosition = 0;
// 1, 7, 10 are the defaults for these, change as necessary
public static final double kSysIDRampRate = 1;
public static final double kSysIDStepVolts = 7;

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

@ -62,6 +62,10 @@ public class Arm extends SubsystemBase {
}).until(positionController::atSetpoint).withTimeout(timeout);
}
public double getEncoderPosition() {
return canCoder.getPosition().getValueAsDouble();
}
protected double rotationsToRadians(double rotations) {
return rotations * 2 * Math.PI;
}

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

@ -6,8 +6,6 @@ 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;
@ -111,4 +109,8 @@ public class Elevator extends SubsystemBase {
() -> positionController.atSetpoint() || topLimitSwitch.get() || bottomLimitSwitch.get()
).withTimeout(timeout);
}
public double getEncoderPosition() {
return encoder.getPosition();
}
}