Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 08:48:07 -06:00
Code Issues Packages Projects Releases Wiki Activity

softlims

Browse Source
This commit is contained in:
aarav18
2022-03-18 14:13:37 -06:00
parent 1d9940f81b
commit 6db9636f22
4 changed files with 71 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+2 -2
View File
@@ -194,9 +194,9 @@ public final class Constants {
public static final double ELBOW_GB_RATIO = 1.d;
public static final double SHOULDER_GB_RATIO = 1.d;
public static final double SHOULDER_SOFT_LIMIT_FORWARD = 0;
public static final double SHOULDER_SOFT_LIMIT_FORWARD = 53869;
public static final double SHOULDER_SOFT_LIMIT_REVERSE = 0;
public static final double ELBOW_SOFT_LIMIT_FORWARD = 0;
public static final double ELBOW_SOFT_LIMIT_FORWARD = 281717;
public static final double ELBOW_SOFT_LIMIT_REVERSE = 0;
// PID Constants
src/main/java/frc4388/robot/Robot.java
+1
View File
@@ -124,6 +124,7 @@ public class Robot extends TimedRobot {
});
desmosServer.start();
m_robotContainer.m_robotVisionOdometry.setLEDs(false);
// DesmosServer.putTable("table", "x1", new double[] {44}, "y1", new double[] {0});
}
src/main/java/frc4388/robot/RobotContainer.java
+42 -7
View File
@@ -112,10 +112,14 @@ public class RobotContainer {
/* Subsystems */
public final ClimberRewrite m_robotClimber = new ClimberRewrite(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false);
<<<<<<< Updated upstream
private final Claws m_robotClaws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw);
// Subsystems
=======
public final Claws m_robotClaws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw);
>>>>>>> Stashed changes
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro);
public final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam);
public final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor);
@@ -139,7 +143,7 @@ public class RobotContainer {
// Autonomous
private PathPlannerTrajectory loadedPathTrajectory = null;
// private final ListeningSendableChooser<File> autoChooser = new ListeningSendableChooser<>(this::loadPath);
private final ListeningSendableChooser<File> autoChooser = new ListeningSendableChooser<>(this::loadPath);
private final List<Waypoint> pathPoints = new ArrayList<>();
private final NetworkTableInstance networkTableInstance = NetworkTableInstance.getDefault();
private final NetworkTable recordingNetworkTable = networkTableInstance.getTable("Recording");
@@ -199,6 +203,7 @@ public class RobotContainer {
m_robotSerializer.setDefaultCommand(
new RunCommand(() -> m_robotSerializer.setSerializer(getOperatorController().getLeftTriggerAxis() * 0.8),//m_robotSerializer.setSerializerStateWithBeam(),
m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand"));
<<<<<<< Updated upstream
// Turret Manual
@@ -212,9 +217,15 @@ public class RobotContainer {
.until(() -> this.currentMode.equals(CurrentMode.TURRET)));
// EnableTurret()
=======
// Turret Manual
// m_robotTurret.setDefaultCommand(
// new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()),
// m_robotTurret).withName("Turret runTurretWithInput defaultCommand"));
>>>>>>> Stashed changes
m_robotHood.setDefaultCommand(
new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood));
// m_robotHood.setDefaultCommand(
// new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood));
// m_robotTurret.setDefaultCommand(
// new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry));
@@ -268,10 +279,10 @@ public class RobotContainer {
.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage));
//Toggles extender in and out
new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
.whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender));
new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
.whileHeld(new RunCommand(() -> m_robotTurret.gotoZero(), m_robotTurret))
.whenReleased(new RunCommand(() -> m_robotTurret.m_boomBoomRotateMotor.set(0), m_robotTurret));
@@ -292,7 +303,7 @@ public class RobotContainer {
// .whileHeld(new RunCommand(() -> m_robotTurret.gotoMidpoint(), m_robotTurret));
//B > Shoot with Lime
new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
.whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry))
.whenReleased(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false)))
.whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0)));
@@ -305,15 +316,18 @@ public class RobotContainer {
.whenPressed(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret))
.whenPressed(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood))
.whenPressed(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender))
.whenPressed(new InstantCommand(() -> m_robotClimber.setClimberSoftLimits(false), m_robotClimber))
.whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret))
.whenReleased(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood))
.whenReleased(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), m_robotExtender))
.whenReleased(new InstantCommand(() -> m_robotClimber.setClimberSoftLimits(true), m_robotClimber))
.whenReleased(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret))
.whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood))
.whenReleased(new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender))
.whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender));
.whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender))
.whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber));
new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value)
.whenPressed(new InstantCommand(() -> this.currentMode = CurrentMode.CLIMBER))
@@ -411,6 +425,7 @@ public class RobotContainer {
* Finally, adds the existing path files to the auto chooser
*/
private void autoInit() {
<<<<<<< Updated upstream
// try {
// WatchKey watchKey = PATHPLANNER_DIRECTORY.register(FileSystems.getDefault().newWatchService(),
// StandardWatchEventKinds.ENTRY_CREATE, StandardWatchEventKinds.ENTRY_MODIFY,
@@ -429,6 +444,26 @@ public class RobotContainer {
// .filter(file -> file.getName().endsWith(".path")).sorted(Comparator.comparingLong(File::lastModified))
// .forEachOrdered(file -> autoChooser.addOption(file.getName(), file));
// SmartDashboard.putData("Auto Chooser", autoChooser);
=======
try {
WatchKey watchKey = PATHPLANNER_DIRECTORY.register(FileSystems.getDefault().newWatchService(),
StandardWatchEventKinds.ENTRY_CREATE, StandardWatchEventKinds.ENTRY_MODIFY,
StandardWatchEventKinds.ENTRY_DELETE);
// TODO: Store this and other commands as fields so they can be rescheduled.
new NotifierCommand(() -> updateAutoChooser(watchKey), 0.5) {
@Override
public boolean runsWhenDisabled() {
return true;
}
}.withName("Path Watcher").schedule();
} catch (IOException exception) {
LOGGER.log(Level.SEVERE, "Exception with path file watcher.", exception);
}
Arrays.stream(PATHPLANNER_DIRECTORY.toFile().listFiles())
.filter(file -> file.getName().endsWith(".path")).sorted(Comparator.comparingLong(File::lastModified))
.forEachOrdered(file -> autoChooser.addOption(file.getName(), file));
SmartDashboard.putData("Auto Chooser", autoChooser);
>>>>>>> Stashed changes
}
/**
src/main/java/frc4388/robot/subsystems/ClimberRewrite.java
+26 -6
View File
@@ -17,6 +17,7 @@ import com.ctre.phoenix.sensors.WPI_Pigeon2;
import org.opencv.core.Point;
import edu.wpi.first.wpilibj.motorcontrol.Talon;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants;
import frc4388.robot.Constants.ClimberConstants;
@@ -51,16 +52,16 @@ public class ClimberRewrite extends SubsystemBase {
// shoulderStartPosition = m_shoulder.getSelectedSensorPosition();
// elbowStartPosition = m_elbow.getSelectedSensorPosition();
m_shoulder.setSelectedSensorPosition(((ClimberConstants.SHOULDER_RESTING_ANGLE * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI) * ClimberConstants.SHOULDER_GB_RATIO);
m_elbow.setSelectedSensorPosition(((ClimberConstants.ELBOW_RESTING_ANGLE * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI) * ClimberConstants.SHOULDER_GB_RATIO);
// m_shoulder.setSelectedSensorPosition(((ClimberConstants.SHOULDER_RESTING_ANGLE * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI) * ClimberConstants.SHOULDER_GB_RATIO);
// m_elbow.setSelectedSensorPosition(((ClimberConstants.ELBOW_RESTING_ANGLE * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI) * ClimberConstants.SHOULDER_GB_RATIO);
m_elbow.configForwardSoftLimitThreshold(ClimberConstants.ELBOW_SOFT_LIMIT_FORWARD);
m_elbow.configForwardSoftLimitEnable(false);
m_elbow.configReverseSoftLimitThreshold(ClimberConstants.ELBOW_SOFT_LIMIT_REVERSE);
m_elbow.configReverseSoftLimitEnable(false);
m_elbow.configForwardSoftLimitEnable(true);
// m_elbow.configReverseSoftLimitThreshold(ClimberConstants.ELBOW_SOFT_LIMIT_REVERSE);
// m_elbow.configReverseSoftLimitEnable(true);
m_shoulder.configForwardSoftLimitThreshold(ClimberConstants.SHOULDER_SOFT_LIMIT_FORWARD);
m_shoulder.configForwardSoftLimitEnable(false);
m_shoulder.configForwardSoftLimitEnable(true);
m_shoulder.configReverseSoftLimitThreshold(ClimberConstants.SHOULDER_SOFT_LIMIT_REVERSE);
m_shoulder.configReverseSoftLimitEnable(false);
@@ -174,6 +175,8 @@ public class ClimberRewrite extends SubsystemBase {
@Override
public void periodic() {
SmartDashboard.putNumber("Elbow", m_elbow.getSelectedSensorPosition());
SmartDashboard.putNumber("Shoulder", m_shoulder.getSelectedSensorPosition());
// double[] jointAngles = getTargetJointAngles(tPoint, 0.d);
// setJointAngles(jointAngles);
}
@@ -283,7 +286,24 @@ public class ClimberRewrite extends SubsystemBase {
return getClimberPosition(jointAngles[0], jointAngles[1]);
}
<<<<<<< Updated upstream
public double getCurrent() {
return (this.m_elbow.getSupplyCurrent() + this.m_shoulder.getSupplyCurrent());
}
=======
public void setClimberSoftLimits(boolean set){
m_elbow.configForwardSoftLimitEnable(set);
m_shoulder.configForwardSoftLimitEnable(set);
}
public void setEncoders(double value){
m_elbow.setSelectedSensorPosition(value);
m_shoulder.setSelectedSensorPosition(value);
}
public double getCurrent(){
return (m_shoulder.getSupplyCurrent() + m_elbow.getSupplyCurrent());
}
>>>>>>> Stashed changes
}