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

test code

Browse Source
This commit is contained in:
aarav18
2022-03-16 22:32:57 -06:00
parent a7378ef38a
commit d5ccd5e46e
5 changed files with 77 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+4
View File
@@ -207,6 +207,10 @@ public final class Constants {
public static final double CALIBRATION_SPEED = 0; public static final double CALIBRATION_SPEED = 0;
public static final double CURRENT_LIMIT = 0.0; // TODO: set actual current limit; public static final double CURRENT_LIMIT = 0.0; // TODO: set actual current limit;
public static final int TOP_LIMIT = 1800;
public static final int BOTTOM_LIMIT = 1200;
} }
/** /**
* The OIConstants class contains the ID for the XBox controllers * The OIConstants class contains the ID for the XBox controllers
src/main/java/frc4388/robot/RobotContainer.java
+5 -5
View File
@@ -137,9 +137,9 @@ public class RobotContainer {
/* Default Commands */ /* Default Commands */
// moves climber in xy space with two-axis input from the operator controller // moves climber in xy space with two-axis input from the operator controller
m_robotClimber.setDefaultCommand( // m_robotClimber.setDefaultCommand(
new RunCommand(() -> m_robotClimber.setMotors(getOperatorController().getLeftX() * 0.4, getOperatorController().getLeftY() * 0.4), // new RunCommand(() -> m_robotClimber.setMotors(getOperatorController().getLeftX() * 0.4, getOperatorController().getLeftY() * 0.4),
m_robotClimber)); // m_robotClimber));
// IK command // IK command
@@ -235,10 +235,10 @@ public class RobotContainer {
// .whenReleased(new RunCommand(() -> m_robotClaws.runClaw(ClawType.RIGHT, 0.0))); // .whenReleased(new RunCommand(() -> m_robotClaws.runClaw(ClawType.RIGHT, 0.0)));
new JoystickButton(getOperatorController(), XboxController.Button.kY.value) new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
.whenPressed(new RunCommand(() -> m_robotClaws.setOpen(true))); .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true)));
new JoystickButton(getOperatorController(), XboxController.Button.kB.value) new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
.whenPressed(new RunCommand(() -> m_robotClaws.setOpen(false))); .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(false)));
} }
/* /*
src/main/java/frc4388/robot/commands/RunClaw.java
+1 -1
View File
@@ -37,7 +37,7 @@ public class RunClaw extends CommandBase {
// Called every time the scheduler runs while the command is scheduled. // Called every time the scheduler runs while the command is scheduled.
@Override @Override
public void execute() { public void execute() {
m_claws.runClaw(clawType, open); // m_claws.runClaw(clawType, open);
} }
// Called once the command ends or is interrupted. // Called once the command ends or is interrupted.
src/main/java/frc4388/robot/commands/climber/RunClimberPath.java
+15 -14
View File
@@ -42,23 +42,23 @@ public class RunClimberPath extends CommandBase {
// Called every time the scheduler runs while the command is scheduled. // Called every time the scheduler runs while the command is scheduled.
@Override @Override
public void execute() { public void execute() {
if(!claws.fullyOpen()) // if(!claws.fullyOpen())
return; return;
Point climberPos = ClimberRewrite.getClimberPosition(climber.getJointAngles()); // Point climberPos = ClimberRewrite.getClimberPosition(climber.getJointAngles());
Vector2D dir = new Vector2D(path[nextIndex]); // Vector2D dir = new Vector2D(path[nextIndex]);
dir.subtract(new Vector2D(climberPos)); // dir.subtract(new Vector2D(climberPos));
if(!endPath && dir.magnitude() < ClimberConstants.THRESHOLD && nextIndex < path.length-1) // if(!endPath && dir.magnitude() < ClimberConstants.THRESHOLD && nextIndex < path.length-1)
nextIndex++; // nextIndex++;
else if(!endPath && dir.magnitude() < ClimberConstants.THRESHOLD) { // else if(!endPath && dir.magnitude() < ClimberConstants.THRESHOLD) {
endPath = true; // endPath = true;
claws.setOpen(false); // claws.setOpen(false);
} else if(!endPath) { // } else if(!endPath) {
dir = dir.unit(); // dir = dir.unit();
climber.controlWithInput(dir.x, dir.y); // climber.controlWithInput(dir.x, dir.y);
} // }
} }
// Called once the command ends or is interrupted. // Called once the command ends or is interrupted.
@@ -68,6 +68,7 @@ public class RunClimberPath extends CommandBase {
// Returns true when the command should end. // Returns true when the command should end.
@Override @Override
public boolean isFinished() { public boolean isFinished() {
return endPath && claws.fullyClosed(); // return endPath && claws.fullyClosed();
return false;
} }
} }
src/main/java/frc4388/robot/subsystems/Claws.java
+52 -46
View File
@@ -8,6 +8,7 @@ import com.revrobotics.SparkMaxLimitSwitch;
import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.ClawConstants; import frc4388.robot.Constants.ClawConstants;
import frc4388.robot.Constants.ClimberConstants; import frc4388.robot.Constants.ClimberConstants;
@@ -59,71 +60,76 @@ public class Claws extends SubsystemBase {
* @param which Which claw to run. * @param which Which claw to run.
* @param open Whether to open or close the claw. * @param open Whether to open or close the claw.
*/ */
public void runClaw(ClawType which, boolean open) { // public void runClaw(ClawType which, boolean open) {
int direction = open ? 1 : -1; // int direction = open ? 1 : -1;
if (which == Claws.ClawType.LEFT) { // if (which == Claws.ClawType.LEFT) {
// double setPos = open ? ClawsConstants.OPEN_POSITION + m_leftOffset : ClawsConstants.CLOSE_POSITION + m_leftOffset; // // double setPos = open ? ClawsConstants.OPEN_POSITION + m_leftOffset : ClawsConstants.CLOSE_POSITION + m_leftOffset;
// m_leftClaw.getEncoder().setPosition(setPos); // // m_leftClaw.getEncoder().setPosition(setPos);
m_leftClaw.setSpeed(direction * 0.1); // m_leftClaw.setSpeed(direction * 0.1);
} else if (which == Claws.ClawType.RIGHT) { // } else if (which == Claws.ClawType.RIGHT) {
// double setPos = open ? ClawsConstants.OPEN_POSITION + m_rightOffset : ClawsConstants.CLOSE_POSITION + m_rightOffset; // // double setPos = open ? ClawsConstants.OPEN_POSITION + m_rightOffset : ClawsConstants.CLOSE_POSITION + m_rightOffset;
// m_rightClaw.getEncoder().setPosition(setPos); // // m_rightClaw.getEncoder().setPosition(setPos);
m_rightClaw.setSpeed(direction * 0.1); // m_rightClaw.setSpeed(direction * 0.1);
} // }
m_open = open; // m_open = open;
} // }
public void runClaw(ClawType which, double input) { // public void runClaw(ClawType which, double input) {
if (which == Claws.ClawType.LEFT) { // if (which == Claws.ClawType.LEFT) {
m_leftClaw.setSpeed(input); // m_leftClaw.setSpeed(input);
} else if (which == Claws.ClawType.RIGHT) { // } else if (which == Claws.ClawType.RIGHT) {
m_rightClaw.setSpeed(input); // m_rightClaw.setSpeed(input);
} // }
} // }
public void runClaws(double input) // public void runClaws(double input)
{ // {
m_leftClaw.setSpeed(input); // m_leftClaw.setSpeed(input);
m_rightClaw.setSpeed(input); // m_rightClaw.setSpeed(input);
} // }
/** /**
* Sets the state of both hooks * Sets the state of both hooks
* @param open The state of the hooks * @param open The state of the hooks
*/ */
public void setOpen(boolean open) { public void setOpen(boolean open) {
SmartDashboard.putBoolean("open", open);
if(open) { if(open) {
// m_leftClaw.getEncoder().setPosition(ClawsConstants.OPEN_POSITION + m_leftOffset); // m_leftClaw.getEncoder().setPosition(ClawsConstants.OPEN_POSITION + m_leftOffset);
// m_rightClaw.getEncoder().setPosition(ClawsConstants.OPEN_POSITION + m_rightOffset); // m_rightClaw.getEncoder().setPosition(ClawsConstants.OPEN_POSITION + m_rightOffset);
m_leftClaw.set(-ClawConstants.OPEN_POSITION); // m_leftClaw.setPosition(.7);
m_rightClaw.set(ClawConstants.OPEN_POSITION); // m_rightClaw.setPosition(.7);
m_leftClaw.setRaw(ClawConstants.BOTTOM_LIMIT);//ClawConstants.OPEN_POSITION);
m_rightClaw.setRaw(ClawConstants.TOP_LIMIT);//ClawConstants.OPEN_POSITION);
} else { } else {
// m_leftClaw.getEncoder().setPosition(ClawsConstants.CLOSE_POSITION + m_leftOffset); // m_leftClaw.getEncoder().setPosition(ClawsConstants.CLOSE_POSITION + m_leftOffset);
// m_rightClaw.getEncoder().setPosition(ClawsConstants.CLOSE_POSITION + m_rightOffset); // m_rightClaw.getEncoder().setPosition(ClawsConstants.CLOSE_POSITION + m_rightOffset);
m_leftClaw.set(-ClawConstants.CLOSE_POSITION); // m_leftClaw.setPosition(.3);
m_rightClaw.set(ClawConstants.CLOSE_POSITION); // m_rightClaw.setPosition(.3);
m_leftClaw.setRaw(ClawConstants.TOP_LIMIT);
m_rightClaw.setRaw(ClawConstants.BOTTOM_LIMIT);
} }
} }
public double[] getOffsets() { // public double[] getOffsets() {
return new double[] {m_leftOffset, m_rightOffset}; // return new double[] {m_leftOffset, m_rightOffset};
} // }
public boolean fullyOpen() { // public boolean fullyOpen() {
return Math.abs(m_leftClaw.getPosition() - ClawConstants.OPEN_POSITION) < ClawConstants.THRESHOLD && // return Math.abs(m_leftClaw.getPosition() - ClawConstants.OPEN_POSITION) < ClawConstants.THRESHOLD &&
Math.abs(m_rightClaw.getPosition() - ClawConstants.OPEN_POSITION) < ClawConstants.THRESHOLD; } // Math.abs(m_rightClaw.getPosition() - ClawConstants.OPEN_POSITION) < ClawConstants.THRESHOLD; }
public boolean fullyClosed() { // public boolean fullyClosed() {
return Math.abs(m_leftClaw.getPosition() - ClawConstants.CLOSE_POSITION) < ClawConstants.THRESHOLD && // return Math.abs(m_leftClaw.getPosition() - ClawConstants.CLOSE_POSITION) < ClawConstants.THRESHOLD &&
Math.abs(m_rightClaw.getPosition() - ClawConstants.CLOSE_POSITION) < ClawConstants.THRESHOLD; // Math.abs(m_rightClaw.getPosition() - ClawConstants.CLOSE_POSITION) < ClawConstants.THRESHOLD;
} // }
/** /**
* Check if a limit switch is pressed or current limit exceeded for a claw. * Check if a limit switch is pressed or current limit exceeded for a claw.
@@ -145,12 +151,12 @@ public class Claws extends SubsystemBase {
// return false; // return false;
// } // }
@Override // @Override
public void periodic() { // public void periodic() {
if (fullyOpen() || fullyClosed()) { // if (fullyOpen() || fullyClosed()) {
m_leftClaw.setSpeed(0.0); // m_leftClaw.setSpeed(0.0);
m_rightClaw.setSpeed(0.0); // m_rightClaw.setSpeed(0.0);
} // }
// if(m_leftLimitSwitchF.isPressed() || m_leftLimitSwitchR.isPressed()) // if(m_leftLimitSwitchF.isPressed() || m_leftLimitSwitchR.isPressed())
// // m_leftOffset = m_leftClaw.getEncoder().getPosition(); // // m_leftOffset = m_leftClaw.getEncoder().getPosition();
@@ -159,5 +165,5 @@ public class Claws extends SubsystemBase {
// if(m_rightLimitSwitchF.isPressed() || m_rightLimitSwitchR.isPressed()) // if(m_rightLimitSwitchF.isPressed() || m_rightLimitSwitchR.isPressed())
// // m_rightOffset = m_rightClaw.getEncoder().getPosition(); // // m_rightOffset = m_rightClaw.getEncoder().getPosition();
// m_rightOffset = m_rightClaw.getPosition(); // m_rightOffset = m_rightClaw.getPosition();
} // }
} }