Develop

src/main/java/frc/robot/Robot.java

@@ -4,6 +4,11 @@
 
 package frc.robot;
 
+import edu.wpi.first.util.datalog.BooleanLogEntry;
+import edu.wpi.first.util.datalog.DataLog;
+import edu.wpi.first.util.datalog.DoubleLogEntry;
+import edu.wpi.first.util.datalog.StringLogEntry;
+import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
@@ -13,14 +18,25 @@ public class Robot extends TimedRobot {
 
   private RobotContainer m_robotContainer;
 
+  DoubleLogEntry rightAmps;
+  DoubleLogEntry leftAmps;
+
   @Override
   public void robotInit() {
     m_robotContainer = new RobotContainer();
+
+    DataLogManager.start();
+    DataLog log = DataLogManager.getLog();
+    rightAmps = new DoubleLogEntry(log, "left drive amps");
+    leftAmps = new DoubleLogEntry(log, "right drive amps");
   }
 
   @Override
   public void robotPeriodic() {
     CommandScheduler.getInstance().run();
+    leftAmps.append(RobotContainer.swerve.getDriveAmps()[0]);
+    rightAmps.append(RobotContainer.swerve.getDriveAmps()[1]);
+
   }
 
   @Override

src/main/java/frc/robot/RobotContainer.java

@@ -7,31 +7,40 @@
 import java.util.function.Supplier;
 
 import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.commands.ResetZeroes;
+import frc.robot.commands.RevertZeroes;
 import frc.robot.commands.TurdDrive;
-import frc.robot.subsystems.TurdPod;
+import frc.robot.constants.Constants;
 import frc.robot.subsystems.TurdSwerve;
 
 public class RobotContainer {
 
-  public static final XboxController driver = new XboxController(Constants.driverPort);
+  public static final XboxController driverRaw = new XboxController(Constants.driverPort);
+  public static final CommandXboxController driverCommand = new CommandXboxController(Constants.driverPort);
   // public static final TurdPod leftPod = new TurdPod(Constants.leftAzimuthID, Constants.leftDriveID, Constants.leftAbsoluteEncoderID, Constants.leftAzimuthInvert,Constants.rightAzimuthInvert, Constants.leftAbsoluteEncoderOffset);
   public static final TurdSwerve swerve = new TurdSwerve();
+
 
   public RobotContainer() {
+    final var Odometry = Shuffleboard.getTab("Odometry");
     configureBindings();
-    Supplier<Translation2d> driverRightJoystick = () -> new Translation2d(driver.getRightX(), driver.getRightY());
-    Supplier<Translation2d> driverLeftJoystick = () -> new Translation2d(driver.getLeftX(), driver.getLeftY());
-    Supplier<Boolean> buttonStart = () -> driver.getStartButton();
-    swerve.setDefaultCommand(new TurdDrive(swerve, driverLeftJoystick, driverRightJoystick, buttonStart));
+    Supplier<Translation2d> driverRightJoystick = () -> new Translation2d(driverRaw.getRightX(), driverRaw.getRightY());
+    Supplier<Translation2d> driverLeftJoystick = () -> new Translation2d(driverRaw.getLeftX(), driverRaw.getLeftY());
+    Supplier<Integer> DPAD = () -> driverRaw.getPOV();
+    swerve.setDefaultCommand(new TurdDrive(swerve, driverLeftJoystick, driverRightJoystick, DPAD, driverRaw::getLeftBumper));
+    swerve.addDashboardWidgets(Odometry);
   }
 
   private void configureBindings() {
-    // new JoystickButton(driver, 4).onTrue(swerve.resetPods());
+    driverCommand.rightBumper().and(driverRaw::getYButton).onTrue(new ResetZeroes(swerve));
+    driverCommand.rightBumper().and(driverRaw::getXButton).whileTrue(new RevertZeroes(swerve));
+    driverCommand.start().whileTrue(new InstantCommand(swerve::resetPods, swerve));
   }
 
   public Command getAutonomousCommand() {

src/main/java/frc/robot/commands/ResetZeroes.java

@@ -0,0 +1,41 @@
+// 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 frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.TurdSwerve;
+
+public class ResetZeroes extends Command {
+  private TurdSwerve swerve;
+  public ResetZeroes(TurdSwerve swerve) {
+    this.swerve = swerve;
+    addRequirements(swerve);
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    swerve.resetZero();
+    swerve.stop();
+  }
+
+  // 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 false;
+  }
+}

src/main/java/frc/robot/commands/RevertZeroes.java

@@ -0,0 +1,41 @@
+// 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 frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.TurdSwerve;
+
+public class RevertZeroes extends Command {
+  private TurdSwerve swerve;
+  public RevertZeroes(TurdSwerve swerve) {
+    this.swerve = swerve;
+    addRequirements(swerve);
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    swerve.revertZero();
+    swerve.stop();
+  }
+
+  // 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 false;
+  }
+}

src/main/java/frc/robot/commands/TurdDrive.java

@@ -4,50 +4,65 @@
 
 package frc.robot.commands;
 
-import java.util.function.DoubleSupplier;
 import java.util.function.Supplier;
 
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.math.kinematics.SwerveModuleState;
-import edu.wpi.first.wpilibj.AddressableLED;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystems.TurdPod;
+import frc.robot.constants.Constants;
+import frc.robot.constants.RobotMap;
 import frc.robot.subsystems.TurdSwerve;
 
 public class TurdDrive extends Command {
 
   TurdSwerve swerve;
-  Supplier<Translation2d> joystickRight;
-  Supplier<Translation2d> joystickLeft;
-  Supplier<Boolean> resetPods;
+  Supplier<Translation2d> joystickRight, joystickLeft;
+  Supplier<Integer> DPAD;
+  Supplier<Boolean> boost;
   Rotation2d rotation = new Rotation2d();
+  double maxSpeed = Constants.robotMaxSpeed;
 
-  public TurdDrive(TurdSwerve swerve, Supplier<Translation2d> joystickLeft, Supplier<Translation2d> joystickRight, Supplier<Boolean> resetPods) {
+  public TurdDrive(TurdSwerve swerve, Supplier<Translation2d> joystickLeft, Supplier<Translation2d> joystickRight, Supplier<Integer> DPAD, Supplier<Boolean> boost) {
     this.swerve = swerve;
     this.joystickRight = joystickRight;
     this.joystickLeft = joystickLeft;
-    this.resetPods = resetPods;
+    this.DPAD = DPAD;
+    this.boost = boost;
     addRequirements(swerve);
   }
 
   // Called when the command is initially scheduled.
   @Override
-  public void initialize() {}
+  public void initialize() {
+    swerve.resetGyro();
+  }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if (resetPods.get()) {
-      swerve.resetPods();
+
+    if (DPAD.get() != -1) {
+      swerve.targetAngle = -Units.degreesToRadians(DPAD.get());
+    }
+
+    if (boost.get()) {
+      swerve.setAmpLimit(Constants.driveTopAmpLimit);
+      maxSpeed = 1;
+    } else {
+      swerve.setAmpLimit(Constants.driveAmpLimit);
+      maxSpeed = Constants.robotMaxSpeed;
     }
-    boolean deadband = Math.abs(joystickRight.get().getX()) + Math.abs(joystickRight.get().getY()) < 0.1;
-    double speedX = deadband ? 0 : -joystickRight.get().getX();
-    double speedY = deadband ? 0 : joystickRight.get().getY();
-    double speedOmega = -joystickLeft.get().getX() * Math.abs(joystickLeft.get().getX());
+    boolean deadband = Math.abs(joystickRight.get().getX()) + Math.abs(joystickRight.get().getY()) < 0.05;
+    double speedX = deadband ? 0 : -joystickRight.get().getX() * maxSpeed;
+    double speedY = deadband ? 0 : joystickRight.get().getY() * maxSpeed;
+    // double speedX = deadband ? 0 : 3.0 * Math.abs(joystickRight.get().getX()) * -joystickRight.get().getX();
+    // double speedY = deadband ? 0 : 3.0 * Math.abs(joystickRight.get().getY()) * joystickRight.get().getY();
+    double speedOmega = Math.abs(joystickLeft.get().getX()) > 0.07 ? -joystickLeft.get().getX() * Math.abs(joystickLeft.get().getX())*0.3 : 0;
     ChassisSpeeds speeds = new ChassisSpeeds(speedX, speedY, speedOmega);
     swerve.setRobotSpeeds(speeds);
+
   }
 
   // Called once the command ends or is interrupted.

src/main/java/frc/robot/constants/Constants.java

@@ -0,0 +1,45 @@
+// 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 frc.robot.constants;
+
+import com.revrobotics.CANSparkBase.IdleMode;
+
+
+/** Add your docs here. */
+public final class Constants {
+    public static final int driverPort = 0;
+
+    public static final double robotMaxSpeed = 0.25;
+
+    public static final double gyroP = 2;
+    public static final double gyroI = 0.0;
+    public static final double gyroD = 0.00;
+
+
+    // Azimuth Settings
+    public static final IdleMode azimuthMode = IdleMode.kBrake;
+
+    public static final int azimuthAmpLimit = 55;
+    public static final double azimuthMaxOutput = 0.25;
+
+    public static final double azimuthkP = 0.4;
+    public static final double azimuthkI = 0.0;
+    public static final double azimuthkD = 0.003;
+    public static final double azimuthkIz = 0;
+
+    public static final double azimuthDriveSpeedMultiplier = 0;//0.5;
+
+
+    // Drive Settings
+    public static final IdleMode driveMode = IdleMode.kCoast;
+
+    public static final int driveAmpLimit = 25;
+    public static final int driveTopAmpLimit = 50;
+    public static final double driveSpeedToPower = 1.0;
+    public static final double driveMotorRampRate = 0.2;
+
+    public static final double podMaxSpeed = 1;
+
+}