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

Merge branch 'testRoboReveal' of https://github.com/Team4388/2022NoWayHome into testRoboReveal

Browse Source
This commit is contained in:
aarav18
2022-03-18 15:16:17 -06:00
parent 1693d320e5 517053ea50
commit 73f4aa12e4
7 changed files with 78 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+8 -4
View File
@@ -173,6 +173,10 @@ public final class Constants {
public static final int SHOULDER_ID = 30;
public static final int ELBOW_ID = 31;
public static final int GYRO_ID = 14;
public static final double INPUT_MULTIPLIER = 0.7;
public static final double ELBOW_SOFT_LIMIT_TOLERANCE = 20000.0;
public static final double SHOULDER_SOFT_LIMIT_TOLERANCE = 12000.0;
// TODO Update this stuff too
public static final double UPPER_ARM_LENGTH = 26; // Units should be in cm
@@ -192,10 +196,10 @@ 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 = 53869;
public static final double SHOULDER_SOFT_LIMIT_REVERSE = 0;
public static final double ELBOW_SOFT_LIMIT_FORWARD = 281717;
public static final double ELBOW_SOFT_LIMIT_REVERSE = 0;
public static final double SHOULDER_FORWARD_SOFT_LIMIT = 53869;
public static final double SHOULDER_REVERSE_SOFT_LIMIT = 0;
public static final double ELBOW_FORWARD_SOFT_LIMIT = 281717;
public static final double ELBOW_REVERSE_SOFT_LIMIT = 0;
// PID Constants
public static final int SHOULDER_SLOT_IDX = 0;
src/main/java/frc4388/robot/RobotContainer.java
+22 -9
View File
@@ -61,7 +61,6 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import frc4388.robot.Constants.*;
import frc4388.robot.subsystems.Claws;
import frc4388.robot.commands.RunClaw;
import frc4388.robot.subsystems.ClimberRewrite;
import frc4388.robot.subsystems.Claws.ClawType;
import frc4388.robot.Constants.OIConstants;
@@ -69,6 +68,7 @@ import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.Constants.StorageConstants;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.robot.commands.ButtonBoxCommands.RunMiddleSwitch;
import frc4388.robot.commands.ClimberCommands.RunClaw;
// import frc4388.robot.commands.ButtonBoxCommands.TurretManual;
import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup;
import frc4388.robot.commands.ShooterCommands.AimToCenter;
@@ -158,9 +158,9 @@ public class RobotContainer {
/* Default Commands */
// moves climber in xy space with two-axis input from the operator controller
m_robotClimber.setDefaultCommand(
new RunCommand(() -> m_robotClimber.setMotors(-getOperatorController().getRightX() * 0.7, -getOperatorController().getRightY() * 0.7),
m_robotClimber));
// m_robotClimber.setDefaultCommand(
// new RunCommand(() -> m_robotClimber.setMotors(-getOperatorController().getRightX() * 0.7, -getOperatorController().getRightY() * 0.7),
// m_robotClimber));
// IK command
@@ -206,8 +206,8 @@ public class RobotContainer {
new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()),
m_robotTurret).withName("Turret runTurretWithInput defaultCommand"));
// 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));
@@ -318,8 +318,22 @@ public class RobotContainer {
.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)
// .whileHeld(new TurretManual(m_robotTurret));
new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value)
.whenPressed(new InstantCommand(() -> m_robotTurret.setDefaultCommand(null)))
.whenPressed(new InstantCommand(() -> m_robotHood.setDefaultCommand(null)))
.whenPressed(new InstantCommand(() -> m_robotClimber.setDefaultCommand(
new RunCommand(() -> m_robotClimber.setMotors(-getOperatorController().getLeftY(), -getOperatorController().getRightY()), m_robotClimber))))
.whenReleased(new InstantCommand(() -> m_robotClimber.setDefaultCommand(null)))
.whenReleased(new InstantCommand(() -> m_robotTurret.setDefaultCommand(
new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), m_robotTurret))))
.whenReleased(new InstantCommand(() -> m_robotHood.setDefaultCommand(
new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood))));
// .whenPressed(new InstantCommand(() -> this.currentMode = CurrentMode.CLIMBER))
// .whenReleased(new InstantCommand(() -> this.currentMode = CurrentMode.TURRET));
// .whenReleased(EnableClimber()));
// control turret manual mode
// new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value)
@@ -407,7 +421,6 @@ public class RobotContainer {
* Finally, adds the existing path files to the auto chooser
*/
private void autoInit() {
try {
WatchKey watchKey = PATHPLANNER_DIRECTORY.register(FileSystems.getDefault().newWatchService(),
StandardWatchEventKinds.ENTRY_CREATE, StandardWatchEventKinds.ENTRY_MODIFY,
src/main/java/frc4388/robot/commands/RunClaw.java → src/main/java/frc4388/robot/commands/ClimberCommands/RunClaw.java
+1 -1
View File
@@ -2,7 +2,7 @@
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc4388.robot.commands;
package frc4388.robot.commands.ClimberCommands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.ClawConstants;
src/main/java/frc4388/robot/commands/climber/RunClimberPath.java → src/main/java/frc4388/robot/commands/ClimberCommands/RunClimberPath.java
+1 -1
View File
@@ -2,7 +2,7 @@
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc4388.robot.commands.climber;
package frc4388.robot.commands.ClimberCommands;
import org.opencv.core.Point;
src/main/java/frc4388/robot/subsystems/ClimberRewrite.java
+42 -5
View File
@@ -37,6 +37,8 @@ public class ClimberRewrite extends SubsystemBase {
private Point tPoint;
private boolean groundRelative;
private double shoulderSpeedLimiter;
private double elbowSpeedLimiter;
/** Creates a new ClimberRewrite. */
public ClimberRewrite(WPI_TalonFX shoulder, WPI_TalonFX elbow, WPI_Pigeon2 gyro, boolean _groundRelative) {
@@ -55,14 +57,14 @@ public class ClimberRewrite extends SubsystemBase {
// 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.configForwardSoftLimitThreshold(ClimberConstants.ELBOW_FORWARD_SOFT_LIMIT);
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.configForwardSoftLimitThreshold(ClimberConstants.SHOULDER_FORWARD_SOFT_LIMIT);
m_shoulder.configForwardSoftLimitEnable(true);
m_shoulder.configReverseSoftLimitThreshold(ClimberConstants.SHOULDER_SOFT_LIMIT_REVERSE);
m_shoulder.configReverseSoftLimitThreshold(ClimberConstants.SHOULDER_REVERSE_SOFT_LIMIT);
m_shoulder.configReverseSoftLimitEnable(false);
m_shoulder.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
@@ -78,6 +80,7 @@ public class ClimberRewrite extends SubsystemBase {
m_gyro = gyro;
groundRelative = _groundRelative;
this.elbowSpeedLimiter = 1.0;
}
public void setClimberGains() {
@@ -129,8 +132,8 @@ public class ClimberRewrite extends SubsystemBase {
}
public void setMotors(double shoulderOutput, double elbowOutput) {
m_shoulder.set(shoulderOutput);
m_elbow.set(elbowOutput);
m_shoulder.set(shoulderOutput * ClimberConstants.INPUT_MULTIPLIER * this.shoulderSpeedLimiter);
m_elbow.set(elbowOutput * ClimberConstants.INPUT_MULTIPLIER * this.elbowSpeedLimiter);
}
public double[] getJointAngles() {
@@ -179,6 +182,40 @@ public class ClimberRewrite extends SubsystemBase {
SmartDashboard.putNumber("Shoulder", m_shoulder.getSelectedSensorPosition());
// double[] jointAngles = getTargetJointAngles(tPoint, 0.d);
// setJointAngles(jointAngles);
// * speed limiting near ELBOW soft limits. tolerance (distance when ramping starts) is 20000 rotations. speed at hard limits is 0.2 (percent output).
double currentElbowPos = this.m_elbow.getSelectedSensorPosition();
double forwardElbowDistance = Math.abs(currentElbowPos - ClimberConstants.ELBOW_FORWARD_SOFT_LIMIT);
double reverseElbowDistance = Math.abs(currentElbowPos - ClimberConstants.ELBOW_REVERSE_SOFT_LIMIT);
if (forwardElbowDistance < ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE) {
this.elbowSpeedLimiter = 0.15 + (forwardElbowDistance * (1 / ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE));
}
if (reverseElbowDistance < ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE) {
this.elbowSpeedLimiter = 0.15 + (reverseElbowDistance * (1 / ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE));
}
if ((forwardElbowDistance > ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE) && (reverseElbowDistance > ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE)) {
this.elbowSpeedLimiter = 1.0;
}
// * speed limiting near SHOULDER soft limits. tolerance (distance when ramping starts) is 20000 rotations. speed at hard limits is 0.2 (percent output).
double currentShoulderPos = this.m_shoulder.getSelectedSensorPosition();
double forwardShoulderDistance = Math.abs(currentShoulderPos - ClimberConstants.SHOULDER_FORWARD_SOFT_LIMIT);
double reverseShoulderDistance = Math.abs(currentShoulderPos - ClimberConstants.SHOULDER_REVERSE_SOFT_LIMIT);
if (forwardShoulderDistance < ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE) {
this.shoulderSpeedLimiter = 0.15 + (forwardShoulderDistance * (1 / ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE));
}
if (reverseShoulderDistance < ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE) {
this.shoulderSpeedLimiter = 0.15 + (reverseShoulderDistance * (1 / ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE));
}
if ((forwardShoulderDistance > ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE) && (reverseShoulderDistance > ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE)) {
this.shoulderSpeedLimiter = 1.0;
}
}
/**
src/main/java/frc4388/robot/subsystems/Hood.java
+2 -5
View File
@@ -68,19 +68,16 @@ public class Hood extends SubsystemBase {
double reverseDistance = Math.abs(currentPos - ShooterConstants.HOOD_REVERSE_SOFT_LIMIT);
if (forwardDistance < ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) {
this.speedLimiter = 0.2 + (forwardDistance * 0.05);
this.speedLimiter = 0.2 + (forwardDistance * (1 / ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE));
}
if (reverseDistance < ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) {
this.speedLimiter = 0.2 + (reverseDistance * 0.05);
this.speedLimiter = 0.2 + (reverseDistance * (1 / ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE));
}
if ((forwardDistance > ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) && (reverseDistance > ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE)) {
this.speedLimiter = 1.0;
}
double hoodCurrent = m_angleAdjusterMotor.getOutputCurrent();
}
/**
src/main/java/frc4388/robot/subsystems/Turret.java
+2 -2
View File
@@ -152,11 +152,11 @@ public class Turret extends SubsystemBase {
double reverseDistance = Math.abs(currentPos - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT);
if (forwardDistance < ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) {
this.speedLimiter = 0.2 + (forwardDistance * 0.05);
this.speedLimiter = 0.2 + (forwardDistance * (1 / ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE));
}
if (reverseDistance < ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) {
this.speedLimiter = 0.2 + (reverseDistance * 0.05);
this.speedLimiter = 0.2 + (reverseDistance * (1 / ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE));
}
if ((forwardDistance > ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) && (reverseDistance > ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE)) {