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

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 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
src/main/java/frc4388/robot/RobotContainer.java
+5 -5
View File
@@ -137,9 +137,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().getLeftX() * 0.4, getOperatorController().getLeftY() * 0.4),
m_robotClimber));
// m_robotClimber.setDefaultCommand(
// new RunCommand(() -> m_robotClimber.setMotors(getOperatorController().getLeftX() * 0.4, getOperatorController().getLeftY() * 0.4),
// m_robotClimber));
// IK command
@@ -235,10 +235,10 @@ public class RobotContainer {
// .whenReleased(new RunCommand(() -> m_robotClaws.runClaw(ClawType.RIGHT, 0.0)));
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)
.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.
@Override
public void execute() {
m_claws.runClaw(clawType, open);
// m_claws.runClaw(clawType, open);
}
// 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.
@Override
public void execute() {
if(!claws.fullyOpen())
// if(!claws.fullyOpen())
return;
Point climberPos = ClimberRewrite.getClimberPosition(climber.getJointAngles());
// Point climberPos = ClimberRewrite.getClimberPosition(climber.getJointAngles());
Vector2D dir = new Vector2D(path[nextIndex]);
dir.subtract(new Vector2D(climberPos));
// Vector2D dir = new Vector2D(path[nextIndex]);
// dir.subtract(new Vector2D(climberPos));
if(!endPath && dir.magnitude() < ClimberConstants.THRESHOLD && nextIndex < path.length-1)
nextIndex++;
else if(!endPath && dir.magnitude() < ClimberConstants.THRESHOLD) {
endPath = true;
claws.setOpen(false);
} else if(!endPath) {
dir = dir.unit();
climber.controlWithInput(dir.x, dir.y);
}
// if(!endPath && dir.magnitude() < ClimberConstants.THRESHOLD && nextIndex < path.length-1)
// nextIndex++;
// else if(!endPath && dir.magnitude() < ClimberConstants.THRESHOLD) {
// endPath = true;
// claws.setOpen(false);
// } else if(!endPath) {
// dir = dir.unit();
// climber.controlWithInput(dir.x, dir.y);
// }
}
// Called once the command ends or is interrupted.
@@ -68,6 +68,7 @@ public class RunClimberPath extends CommandBase {
// Returns true when the command should end.
@Override
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.Servo;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.ClawConstants;
import frc4388.robot.Constants.ClimberConstants;
@@ -59,71 +60,76 @@ public class Claws extends SubsystemBase {
* @param which Which claw to run.
* @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;
// m_leftClaw.getEncoder().setPosition(setPos);
m_leftClaw.setSpeed(direction * 0.1);
// // double setPos = open ? ClawsConstants.OPEN_POSITION + m_leftOffset : ClawsConstants.CLOSE_POSITION + m_leftOffset;
// // m_leftClaw.getEncoder().setPosition(setPos);
// 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;
// m_rightClaw.getEncoder().setPosition(setPos);
m_rightClaw.setSpeed(direction * 0.1);
}
// // double setPos = open ? ClawsConstants.OPEN_POSITION + m_rightOffset : ClawsConstants.CLOSE_POSITION + m_rightOffset;
// // m_rightClaw.getEncoder().setPosition(setPos);
// m_rightClaw.setSpeed(direction * 0.1);
// }
m_open = open;
}
// m_open = open;
// }
public void runClaw(ClawType which, double input) {
if (which == Claws.ClawType.LEFT) {
m_leftClaw.setSpeed(input);
// public void runClaw(ClawType which, double input) {
// if (which == Claws.ClawType.LEFT) {
// m_leftClaw.setSpeed(input);
} else if (which == Claws.ClawType.RIGHT) {
m_rightClaw.setSpeed(input);
}
}
// } else if (which == Claws.ClawType.RIGHT) {
// m_rightClaw.setSpeed(input);
// }
// }
public void runClaws(double input)
{
m_leftClaw.setSpeed(input);
m_rightClaw.setSpeed(input);
}
// public void runClaws(double input)
// {
// m_leftClaw.setSpeed(input);
// m_rightClaw.setSpeed(input);
// }
/**
* Sets the state of both hooks
* @param open The state of the hooks
*/
public void setOpen(boolean open) {
SmartDashboard.putBoolean("open", open);
if(open) {
// m_leftClaw.getEncoder().setPosition(ClawsConstants.OPEN_POSITION + m_leftOffset);
// m_rightClaw.getEncoder().setPosition(ClawsConstants.OPEN_POSITION + m_rightOffset);
m_leftClaw.set(-ClawConstants.OPEN_POSITION);
m_rightClaw.set(ClawConstants.OPEN_POSITION);
// m_leftClaw.setPosition(.7);
// m_rightClaw.setPosition(.7);
m_leftClaw.setRaw(ClawConstants.BOTTOM_LIMIT);//ClawConstants.OPEN_POSITION);
m_rightClaw.setRaw(ClawConstants.TOP_LIMIT);//ClawConstants.OPEN_POSITION);
} else {
// m_leftClaw.getEncoder().setPosition(ClawsConstants.CLOSE_POSITION + m_leftOffset);
// m_rightClaw.getEncoder().setPosition(ClawsConstants.CLOSE_POSITION + m_rightOffset);
m_leftClaw.set(-ClawConstants.CLOSE_POSITION);
m_rightClaw.set(ClawConstants.CLOSE_POSITION);
// m_leftClaw.setPosition(.3);
// m_rightClaw.setPosition(.3);
m_leftClaw.setRaw(ClawConstants.TOP_LIMIT);
m_rightClaw.setRaw(ClawConstants.BOTTOM_LIMIT);
}
}
public double[] getOffsets() {
return new double[] {m_leftOffset, m_rightOffset};
}
// public double[] getOffsets() {
// return new double[] {m_leftOffset, m_rightOffset};
// }
public boolean fullyOpen() {
return Math.abs(m_leftClaw.getPosition() - ClawConstants.OPEN_POSITION) < ClawConstants.THRESHOLD &&
Math.abs(m_rightClaw.getPosition() - ClawConstants.OPEN_POSITION) < ClawConstants.THRESHOLD; }
// public boolean fullyOpen() {
// return Math.abs(m_leftClaw.getPosition() - ClawConstants.OPEN_POSITION) < ClawConstants.THRESHOLD &&
// Math.abs(m_rightClaw.getPosition() - ClawConstants.OPEN_POSITION) < ClawConstants.THRESHOLD; }
public boolean fullyClosed() {
return Math.abs(m_leftClaw.getPosition() - ClawConstants.CLOSE_POSITION) < ClawConstants.THRESHOLD &&
Math.abs(m_rightClaw.getPosition() - ClawConstants.CLOSE_POSITION) < ClawConstants.THRESHOLD;
}
// public boolean fullyClosed() {
// return Math.abs(m_leftClaw.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.
@@ -145,12 +151,12 @@ public class Claws extends SubsystemBase {
// return false;
// }
@Override
public void periodic() {
if (fullyOpen() || fullyClosed()) {
m_leftClaw.setSpeed(0.0);
m_rightClaw.setSpeed(0.0);
}
// @Override
// public void periodic() {
// if (fullyOpen() || fullyClosed()) {
// m_leftClaw.setSpeed(0.0);
// m_rightClaw.setSpeed(0.0);
// }
// if(m_leftLimitSwitchF.isPressed() || m_leftLimitSwitchR.isPressed())
// // m_leftOffset = m_leftClaw.getEncoder().getPosition();
@@ -159,5 +165,5 @@ public class Claws extends SubsystemBase {
// if(m_rightLimitSwitchF.isPressed() || m_rightLimitSwitchR.isPressed())
// // m_rightOffset = m_rightClaw.getEncoder().getPosition();
// m_rightOffset = m_rightClaw.getPosition();
}
// }
}