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

motortest

Browse Source
This commit is contained in:
Connor Peach
2024-01-19 19:26:18 -07:00
parent fc6ad901ef
commit c33e4beabe
4 changed files with 42 additions and 199 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+1 -1
View File
@@ -175,4 +175,4 @@ public final class Constants {
public static final double APRIL_HEIGHT = -1.0; // TODO: find actual value
}
}
}
src/main/java/frc4388/robot/Robot.java
+2 -3
View File
@@ -84,7 +84,6 @@ public class Robot extends TimedRobot {
*/
@Override
public void autonomousInit() {
m_robotContainer.m_robotSwerveDrive.resetGyro();
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
// schedule the autonomous command (example)
@@ -103,7 +102,7 @@ public class Robot extends TimedRobot {
@Override
public void teleopInit() {
m_robotContainer.m_robotSwerveDrive.resetGyro();
// This makes sure that the autonomous stops running when
// teleop starts running. If you want the autonomous to
@@ -112,7 +111,7 @@ public class Robot extends TimedRobot {
if (m_autonomousCommand != null) {
m_autonomousCommand.cancel();
}
m_robotContainer.m_robotSwerveDrive.resetGyro();
m_robotTime.startMatchTime();
m_robotContainer.m_robotMap.restart_motor_tests();
src/main/java/frc4388/robot/RobotContainer.java
+12 -195
View File
@@ -11,6 +11,7 @@ import java.util.function.Consumer;
import edu.wpi.first.math.geometry.Translation2d;
import com.revrobotics.CANSparkMax;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.revrobotics.REVLibError;
import com.revrobotics.CANSparkMax.FaultID;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
@@ -24,6 +25,7 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import edu.wpi.first.wpilibj2.command.button.POVButton;
import frc4388.robot.Constants.*;
import frc4388.robot.subsystems.Arm;
import frc4388.robot.subsystems.MotorTest;
import frc4388.robot.subsystems.Claw;
import frc4388.robot.subsystems.Limelight;
import frc4388.robot.subsystems.SwerveDrive;
@@ -50,11 +52,7 @@ public class RobotContainer {
public final RobotMap m_robotMap = new RobotMap();
/* Subsystems */
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront,
m_robotMap.rightFront,
m_robotMap.leftBack,
m_robotMap.rightBack,
m_robotMap.gyro);
public final Arm m_robotArm = new Arm(m_robotMap.pivot, m_robotMap.tele, m_robotMap.pivotEncoder);
@@ -68,23 +66,13 @@ public class RobotContainer {
/* Autos */
public SendableChooser<Command> chooser = new SendableChooser<>();
// private Command balance = new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive);
// private Command blue1Path = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt");
// private Command blue1PathWithBalance = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt").andThen(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive));
// private Command red1Path = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt", -1);
// private Command red1PathWithBalance = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt", -1).andThen(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive));
// private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt");
// TODO: PUT PARAMETERS FOR MOTORS
MotorTest m_motortest = new MotorTest(new WPI_TalonFX(1), new WPI_TalonFX(1));
private PlaybackChooser playbackChooser;
/* Commands */
private Command emptyCommand = new InstantCommand();
private Command interruptCommand = new InstantCommand(() -> {}, m_robotArm, m_robotSwerveDrive, m_robotClaw, m_robotLimeLight);
private SequentialCommandGroup armToHome = new SequentialCommandGroup(new TeleCommand(m_robotArm, 0), new PivotCommand(m_robotArm, 135));
private Command toggleClaw = new InstantCommand(() -> m_robotClaw.toggle(), m_robotClaw);
@@ -92,78 +80,8 @@ public class RobotContainer {
private boolean readyForPlacement = false;
private Boolean isPole = null;
private SequentialCommandGroup alignToPole =
new SequentialCommandGroup(
new RotateToAngle(m_robotSwerveDrive, 0.0),
new InstantCommand(() -> m_robotLimeLight.setToLimePipeline(), m_robotLimeLight),
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)
// new DriveToLimeDistance(m_robotSwerveDrive, m_robotLimeLight, 37, () -> m_robotLimeLight.getDistanceToTape())
).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = true));
// private SequentialCommandGroup alignToShelf =
// new SequentialCommandGroup(
// new RotateToAngle(m_robotSwerveDrive, 0.0),
// new InstantCommand(() -> m_robotLimeLight.setToAprilPipeline(), m_robotLimeLight),
// new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getAprilPoint().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)
// ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = false));
private SequentialCommandGroup alignToShelf =
new SequentialCommandGroup(
new AprilRotAlign(m_robotSwerveDrive, m_robotLimeLight),
new InstantCommand(() -> m_robotLimeLight.setToAprilPipeline(), 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)
).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = false));
public SequentialCommandGroup place = null;
private Consumer<SequentialCommandGroup> queuePlacement = (scg) -> {
place = scg.andThen(new InstantCommand(() -> readyForPlacement = false), new InstantCommand(() -> isPole = null), new InstantCommand(() -> place = null));
};
// TODO: find actual values
private SequentialCommandGroup placeCubeHigh =
new SequentialCommandGroup(
// new InstantCommand(() -> System.out.println("Placing cone high")),
new PivotCommand(m_robotArm, 64 + 135),
new InstantCommand(() -> m_robotArm.setRotVel(0)),
new TeleCommand(m_robotArm, 95642),
toggleClaw.asProxy(),
armToHome.asProxy()
);
private SequentialCommandGroup placeCubeMid = new SequentialCommandGroup(
new PivotCommand(m_robotArm, 70 + 135),
new TeleCommand(m_robotArm, 32866),
toggleClaw.asProxy(),
armToHome.asProxy()
);
private SequentialCommandGroup placeConeHigh = new SequentialCommandGroup(
new PivotCommand(m_robotArm, 0),
new TeleCommand(m_robotArm, 0),
toggleClaw.asProxy(),
armToHome.asProxy()
);
private SequentialCommandGroup placeConeMid = new SequentialCommandGroup(
new PivotCommand(m_robotArm, 0),
new TeleCommand(m_robotArm, 0),
toggleClaw.asProxy(),
armToHome.asProxy()
);
private SequentialCommandGroup placeLow = new SequentialCommandGroup(
new PivotCommand(m_robotArm, 0),
new TeleCommand(m_robotArm, 0),
toggleClaw.asProxy(),
armToHome.asProxy()
);
/**
@@ -172,30 +90,14 @@ public class RobotContainer {
public RobotContainer() {
configureButtonBindings();
// * Default Commands
m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(),
getDeadbandedDriverController().getRight(),
true);
}, m_robotSwerveDrive)
.withName("SwerveDrive DefaultCommand"));
m_robotArm.setDefaultCommand(new RunCommand(() -> {
m_robotArm.setRotVel(getDeadbandedOperatorController().getLeftY());
m_robotArm.setTeleVel(getDeadbandedOperatorController().getRightY());
m_motortest.setDefaultCommand(new RunCommand(() -> {
m_motortest.motorSetSpeed(getDeadbandedDriverController().getLeftX());
}, m_robotArm)
.withName("Arm DefaultCommand"));
// * Auto Commands
chooser.setDefaultOption("NoAuto", emptyCommand);
chooser.addOption("alignToPole", alignToPole);
chooser.addOption("alignToShelf", alignToShelf);
chooser.addOption("placeConeHigh", placeConeHigh);
chooser.addOption("placeConeMid", placeConeMid);
chooser.addOption("placeCubeHigh", placeCubeHigh);
chooser.addOption("placeCubeMid", placeCubeMid);
chooser.addOption("placeLow", placeLow);
// chooser.addOption("Blue1Path", blue1Path);
// chooser.addOption("Blue1PathWithBalance", blue1PathWithBalance);
@@ -203,9 +105,7 @@ public class RobotContainer {
// chooser.addOption("Red1Path", red1Path);
// chooser.addOption("Red1PathWithBalance", red1PathWithBalance);
playbackChooser = new PlaybackChooser(m_robotSwerveDrive)
.addOption("Balance", new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive))
.buildDisplay();
}
// here be dragons - enter if you dare
@@ -222,14 +122,8 @@ public class RobotContainer {
*/
private void configureButtonBindings() {
// * Driver Buttons
new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); // final
// because closure reasons - ask Daniel Thomas
// final TapState tap = new TapState();
new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToTurbo()))
.onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast()));
// * Driver Buttons
// .onTrue(new InstantCommand(() -> {
// tap.gearTapped = true;
// tap.gearTime = System.currentTimeMillis();
@@ -247,85 +141,8 @@ public class RobotContainer {
// m_robotSwerveDrive.setToSlow();
// }));
new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToSlow()));
new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
.onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); // final
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.LEFT_BUMPER_BUTTON)
// .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt"))
// .onFalse(new InstantCommand());
// * Operator Buttons
// align (pole)
new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) // final
.onTrue(alignToPole)
.onFalse(interruptCommand.asProxy());
// align (shelf)
new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) // final
.onTrue(alignToShelf)
.onFalse(interruptCommand.asProxy());
// toggle claw
new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) // final
.onTrue(toggleClaw.asProxy());
// kill soft limits
new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) // final
.onTrue(new InstantCommand(() -> m_robotArm.killSoftLimits()));
// SmartDashboard.putBoolean("isn't SparkMAX", );
new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON)
.onTrue (new InstantCommand(() -> m_robotClaw.yesspinnyspin()))
.onFalse (new InstantCommand(() -> m_robotClaw.nospinnyspin()));
//Arm to Home
new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) // final
.onTrue(armToHome.asProxy());
//Interupt Button
new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) // final
.onTrue(interruptCommand.asProxy());
// place high
new POVButton(getDeadbandedOperatorController(), 0)
.onTrue(new ConditionalCommand(
new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeConeHigh)), new InstantCommand(() -> queuePlacement.accept(placeCubeHigh)), () -> isPole == true),
emptyCommand.asProxy(),
() -> readyForPlacement == true)
);
// place mid
new POVButton(getDeadbandedOperatorController(), 270)
.onTrue(new ConditionalCommand(
new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeConeMid)), new InstantCommand(() -> queuePlacement.accept(placeCubeMid)), () -> isPole == true),
emptyCommand.asProxy(),
() -> readyForPlacement == true)
);
// place low
new POVButton(getDeadbandedOperatorController(), 180)
.onTrue(new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeLow)), emptyCommand.asProxy(), () -> readyForPlacement == true));
// confirm
new POVButton(getDeadbandedOperatorController(), 90)
.onTrue(new BooleanCommand(() -> place, () -> emptyCommand.asProxy(), () -> place != null));
}
src/main/java/frc4388/robot/subsystems/MotorTest.java
+27
View File
@@ -0,0 +1,27 @@
package frc4388.robot.subsystems;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
public class MotorTest extends SubsystemBase {
/** Creates a new MotorTest. */
WPI_TalonFX motor1;
WPI_TalonFX motor2;
public MotorTest(WPI_TalonFX motor1, WPI_TalonFX motor2) {
this.motor1 = motor1;
this.motor2 = motor2;
}
public double motorSetSpeed(double speed) {
motor1.set(speed);
motor2.set(speed);
return speed;
}
public void stop() {
motor1.set(0);
motor2.set(0);
}
}