Team4388/2023WayOfTheRobot
1
0
Fork 0
mirror of https://github.com/Team4388/2023WayOfTheRobot.git synced 2026-06-08 16:28:00 -06:00
Code Issues Packages Projects Releases Wiki Activity

new claw servo working

Browse Source
This commit is contained in:
Aarav
2023-03-23 15:45:08 -06:00
parent 731f14ecb8
commit 947b65481a
6 changed files with 165 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+38 -14
View File
@@ -27,12 +27,14 @@ import frc4388.robot.subsystems.Claw;
import frc4388.robot.subsystems.Limelight;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.commands.BooleanCommand;
import frc4388.robot.commands.TimedCommand;
import frc4388.robot.commands.Arm.PivotCommand;
import frc4388.robot.commands.Arm.TeleCommand;
import frc4388.robot.commands.Autos.AutoBalance;
import frc4388.robot.commands.Autos.PlaybackChooser;
import frc4388.robot.commands.Placement.AprilRotAlign;
import frc4388.robot.commands.Placement.LimeAlign;
import frc4388.robot.commands.Swerve.JoystickRecorder;
import frc4388.robot.commands.Swerve.RotateToAngle;
import frc4388.utility.controller.DeadbandedXboxController;
import frc4388.utility.controller.XboxController;
@@ -93,9 +95,9 @@ public class RobotContainer {
private SequentialCommandGroup alignToPole =
new SequentialCommandGroup(
new RotateToAngle(m_robotSwerveDrive, 0.0),
new InstantCommand(() -> m_robotLimeLight.setDriverMode(false), m_robotLimeLight),
new InstantCommand(() -> m_robotLimeLight.setToLimePipeline(), m_robotLimeLight),
new RotateToAngle(m_robotSwerveDrive, 0.0),
new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getLowestTape().getYaw(), 0.04),
new RotateToAngle(m_robotSwerveDrive, 0.0),
new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive)
@@ -113,9 +115,9 @@ public class RobotContainer {
private SequentialCommandGroup alignToShelf =
new SequentialCommandGroup(
new AprilRotAlign(m_robotSwerveDrive, m_robotLimeLight),
new InstantCommand(() -> m_robotLimeLight.setDriverMode(false), m_robotLimeLight),
new InstantCommand(() -> m_robotLimeLight.setToAprilPipeline(), m_robotLimeLight),
new AprilRotAlign(m_robotSwerveDrive, m_robotLimeLight),
new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getAprilPoint().getYaw(), 0.04),
new AprilRotAlign(m_robotSwerveDrive, m_robotLimeLight),
new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive)
@@ -144,6 +146,15 @@ public class RobotContainer {
armToHome.asProxy()
);
private SequentialCommandGroup placeCubeLow = new SequentialCommandGroup(
new TimedCommand(() -> new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.2), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive), 0.7),
new PivotCommand(m_robotArm, 70 + 135),
new InstantCommand(() -> m_robotArm.setRotVel(0)),
new TeleCommand(m_robotArm, 28000),
toggleClaw.asProxy(),
armToHome.asProxy()
);
private SequentialCommandGroup placeConeHigh = new SequentialCommandGroup(
new PivotCommand(m_robotArm, 0),
new TeleCommand(m_robotArm, 0),
@@ -221,6 +232,12 @@ public class RobotContainer {
* passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
private void configureButtonBindings() {
// * TEST BUTTONS
// new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); // final
// * Driver Buttons
new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); // final
@@ -255,14 +272,15 @@ public class RobotContainer {
new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
.onTrue(interruptCommand.asProxy()); // final
// new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
// .whileTrue(new JoystickRecorder(m_robotSwerveDrive,
// () -> getDeadbandedDriverController().getLeftX(),
// () -> getDeadbandedDriverController().getLeftY(),
// () -> getDeadbandedDriverController().getRightX(),
// () -> getDeadbandedDriverController().getRightY(),
// "Blue1Path.txt"))
// .onFalse(new InstantCommand());
new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_TRIGGER_AXIS)
.whileTrue(new JoystickRecorder(m_robotSwerveDrive,
() -> getDeadbandedDriverController().getLeftX(),
() -> getDeadbandedDriverController().getLeftY(),
() -> getDeadbandedDriverController().getRightX(),
() -> getDeadbandedDriverController().getRightY(),
"Blue1Path.txt"))
.onFalse(new InstantCommand());
// new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
// .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt"))
@@ -288,19 +306,21 @@ public class RobotContainer {
new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) // final
.onTrue(new InstantCommand(() -> m_robotArm.killSoftLimits()));
// outtake
new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_TRIGGER_AXIS) // final
.onTrue (new InstantCommand(() -> m_robotClaw.reversespinnyspin()))
.onFalse (new InstantCommand(() -> m_robotClaw.nospinnyspin()));
new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_TRIGGER_AXIS) // final
// intake
new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_TRIGGER_AXIS) // final
.onTrue (new InstantCommand(() -> m_robotClaw.yesspinnyspin()))
.onFalse (new InstantCommand(() -> m_robotClaw.nospinnyspin()));
//Arm to Home
// arm to Home
new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) // final
.onTrue(armToHome.asProxy());
//Interupt Button
// interupt
new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) // final
.onTrue(interruptCommand.asProxy());
@@ -337,9 +357,13 @@ public class RobotContainer {
// () -> readyForPlacement == true)
// );
// // place low
// new POVButton(getDeadbandedOperatorController(), 180)
// .onTrue(new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeLow)), emptyCommand.asProxy(), () -> readyForPlacement == true));
// place low
new POVButton(getDeadbandedOperatorController(), 180)
.onTrue(new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeLow)), emptyCommand.asProxy(), () -> readyForPlacement == true));
.onTrue(new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeCubeLow)), emptyCommand.asProxy(), () -> readyForPlacement == true));
// confirm
new POVButton(getDeadbandedOperatorController(), 90)
src/main/java/frc4388/robot/RobotMap.java
+5 -2
View File
@@ -15,6 +15,7 @@ import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import edu.wpi.first.wpilibj.PWM;
import edu.wpi.first.wpilibj.Servo;
import frc4388.robot.Constants.ArmConstants;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.robot.subsystems.SwerveModule;
@@ -170,7 +171,9 @@ public class RobotMap {
}
// claw stuff (WHAT IS A SOAP ENGINEER)
PWM leftClaw = new PWM(0);
PWM rightClaw = new PWM(1);
// PWM leftClaw = new PWM(0);
// PWM rightClaw = new PWM(1);
Servo leftClaw = new Servo(0);
Servo rightClaw = new Servo(1);
CANSparkMax spinnyspin = new CANSparkMax(18, MotorType.kBrushless);
}
src/main/java/frc4388/robot/commands/Placement/AprilRotAlign.java
+4 -1
View File
@@ -26,11 +26,14 @@ public class AprilRotAlign extends PelvicInflammatoryDisease {
@Override
public double getError() {
System.out.println("Pipeline: " + lime.getCamera().getPipelineIndex());
double err = 0.0;
try {
err = lime.getAprilSkew();
} catch (NullPointerException ex) {}
} catch (NullPointerException ex) {
System.out.println("CANT SEE APRIL POINT");
}
return err;
}
src/main/java/frc4388/robot/commands/TimedCommand.java
+61
View File
@@ -0,0 +1,61 @@
// Copyright (c) FIRST and other WPILib contributors.
// 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;
import java.util.function.Supplier;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj2.command.Subsystem;
import frc4388.utility.RobotTime;
public class TimedCommand extends CommandBase {
Supplier<Command> cs;
double duration;
long startTime;
long deltaTime;
/** Creates a new TimedCommand. Duration is in seconds. */
public TimedCommand(Supplier<Command> cs, double duration) {
this.cs = cs;
this.duration = duration;
Object[] reqs_obj = cs.get().getRequirements().toArray();
Subsystem[] reqs = new Subsystem[reqs_obj.length];
for (int i = 0; i < reqs.length; i++)
reqs[i] = (Subsystem) reqs_obj[i];
addRequirements(reqs);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
cs.get().initialize();
startTime = System.currentTimeMillis();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
cs.get().execute();
deltaTime = System.currentTimeMillis() - startTime;
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return (deltaTime / 1000.0) >= duration;
}
}
src/main/java/frc4388/robot/subsystems/Claw.java
+37 -16
View File
@@ -6,41 +6,62 @@ import java.util.TimerTask;
import com.revrobotics.CANSparkMax;
import edu.wpi.first.wpilibj.PWM;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class Claw extends SubsystemBase {
private final PWM m_leftMotor;
private final PWM m_rightMotor;
// private final PWM m_leftMotor;
// private final PWM m_rightMotor;
private final Servo m_leftMotor;
private final Servo m_rightMotor;
private final CANSparkMax m_spinnyspin;
private boolean m_open = false;
// Opens claw
public Claw(PWM leftMotor, PWM rightMotor, CANSparkMax spinnyspin) {
// public Claw(PWM leftMotor, PWM rightMotor, CANSparkMax spinnyspin) {
// m_leftMotor = leftMotor;
// m_rightMotor = rightMotor;
// m_spinnyspin = spinnyspin;
// setClaw(false);
// }
public Claw(Servo leftMotor, Servo rightMotor, CANSparkMax spinnyspin) {
m_leftMotor = leftMotor;
m_rightMotor = rightMotor;
m_spinnyspin = spinnyspin;
setClaw(true);
setClaw(false);
}
public void setClaw(boolean open) {
// Open claw
m_open = open;
m_leftMotor.setRaw(m_open ? 1000 : 2000);
m_rightMotor.setRaw(m_open ? 2000 : 1000);
// m_leftMotor.setRaw(m_open ? 1000 : 2000);
// m_rightMotor.setRaw(m_open ? 2000 : 1000);
if (!m_open) {
m_spinnyspin.set(-0.2);
new Timer().schedule(new TimerTask() {
@Override
public void run() {
nospinnyspin();
}
}, 750);
}
// m_leftMotor.setBounds(4000, 20000, 2000, 0, 0);
m_leftMotor.setAngle(m_open ? 0 : 180);
m_rightMotor.setAngle(m_open ? 180 : 0);
// m_leftMotor.setRaw(m_open ? 0 : 4000);
// m_rightMotor.setRaw(m_open ? 4000 : 0);
// m_leftMotor.setRaw(m_open ? 1500 : 2000);
// m_rightMotor.setRaw(m_open ? 1500 : 1000);
if (m_open)
m_spinnyspin.set(0.2);
else
m_spinnyspin.set(-0.2);
new Timer().schedule(new TimerTask() {
@Override
public void run() {
nospinnyspin();
}
}, 750);
}
public void toggle() {
src/main/java/frc4388/robot/subsystems/Limelight.java
+20 -5
View File
@@ -28,6 +28,10 @@ public class Limelight extends SubsystemBase {
cam.setDriverMode(true);
}
public PhotonCamera getCamera() {
return cam;
}
public void setLEDs(boolean on) {
lightOn = on;
cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff);
@@ -44,12 +48,14 @@ public class Limelight extends SubsystemBase {
public void setToLimePipeline() {
cam.setPipelineIndex(1);
setLEDs(true);
// setLEDs(true);
cam.setLED(VisionLEDMode.kOn);
}
public void setToAprilPipeline() {
cam.setPipelineIndex(0);
setLEDs(false);
// setLEDs(false);
cam.setLED(VisionLEDMode.kOff);
}
public PhotonTrackedTarget getAprilPoint() {
@@ -63,11 +69,17 @@ public class Limelight extends SubsystemBase {
}
private List<TargetCorner> getAprilCorners() {
if (!cam.isConnected()) return null;
if (!cam.isConnected()) {
System.out.println("Camera is not connected");
return null;
}
PhotonPipelineResult result = cam.getLatestResult();
if (!result.hasTargets()) return null;
if (!result.hasTargets()) {
System.out.println("Camera does not have targets");
return null;
}
return result.getBestTarget().getDetectedCorners();
}
@@ -76,7 +88,10 @@ public class Limelight extends SubsystemBase {
List<TargetCorner> corners = getAprilCorners();
ArrayList<TargetCorner> bottomSide = getAprilBottomSide(corners);
if (bottomSide == null) return 0;
if (bottomSide == null) {
// System.out.println("CANT SEE APRIL TAG");
return 0;
}
TargetCorner bottomRight = bottomSide.get(0).x > bottomSide.get(1).x ? bottomSide.get(0) : bottomSide.get(1);
TargetCorner bottomLeft = bottomRight.x == bottomSide.get(0).x ? bottomSide.get(1) : bottomSide.get(0);