Merge pull request #24 from OakvilleDynamics/fix/intake

Fix/intake

src/main/deploy/swerve/controllerproperties.json

@@ -1,8 +1,8 @@
 {
-  "angleJoystickRadiusDeadband": 0.25,
+  "angleJoystickRadiusDeadband": 0.05,
   "heading": {
     "p": 0.4,
     "i": 0,
-    "d": 0.01
+    "d": 0
   }
 }

src/main/deploy/swerve/modules/backleft.json

@@ -1,7 +1,7 @@
 {
   "location": {
-    "front": -13.5,
-    "left": 13.5
+    "front": -11,
+    "left": 11
   },
   "absoluteEncoderOffset": 276.15234375,
   "drive": {

src/main/deploy/swerve/modules/backright.json

@@ -1,7 +1,7 @@
 {
   "location": {
-    "front": -13.5,
-    "left": -13.5
+    "front": -11,
+    "left": -11
   },
   "absoluteEncoderOffset": 299.267578125,
   "drive": {

src/main/deploy/swerve/modules/frontleft.json

@@ -1,7 +1,7 @@
 {
   "location": {
-    "front": 13.5,
-    "left": 13.5
+    "front": 11,
+    "left": 11
   },
   "absoluteEncoderOffset": 357.978515625,
   "drive": {

src/main/deploy/swerve/modules/frontright.json

@@ -1,7 +1,7 @@
 {
   "location": {
-    "front": 13.5,
-    "left": -13.5
+    "front": 11,
+    "left": -11
   },
   "absoluteEncoderOffset": 52.91015625,
   "drive": {

src/main/deploy/swerve/modules/physicalproperties.json

@@ -11,6 +11,6 @@
   },
   "rampRate": {
     "drive": 0.25,
-    "angle": 0.25
+    "angle": 0
   }
 }

src/main/deploy/swerve/swervedrive.json

@@ -4,7 +4,7 @@
     "id": 0,
     "canbus": null
   },
-  "invertedIMU": true,
+  "invertedIMU": false,
   "modules": [
     "frontleft.json",
     "frontright.json",

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

@@ -76,14 +76,22 @@ public static class MechanismConstants {
     public static final int INTAKE_MOTOR_FRONT = 52;
     public static final boolean INTAKE_MOTOR_SUSHI_INVERTED = true;
     public static final boolean INTAKE_MOTOR_FRONT_INVERTED = false;
-    public static final double INTAKE_MOTOR_SPEED_FRONT = 0.8;
+    public static final double INTAKE_MOTOR_SPEED_FRONT = 0.2;
     public static final double INTAKE_MOTOR_SPEED_SUSHI = 0.8;
 
     // Conveyor Motors
     public static final int CONVEYOR_MOTOR_1 = 1;
     public static final int CONVEYOR_MOTOR_2 = 2;
     public static final boolean CONVEYOR_MOTOR_1_INVERTED = true;
-    public static final boolean CONVEYOR_MOTOR_2_INVERTED = true;
-    public static final double CONVEYOR_MOTOR_SPEED = 0.30;
+    public static final boolean CONVEYOR_MOTOR_2_INVERTED = false;
+    public static final double CONVEYOR_MOTOR_SPEED = 0.38;
+
+    // Flywheel Motors
+    public static final int FLYWHEEL_MOTOR_1 = 3;
+    public static final int FLYWHEEL_MOTOR_2 = 4;
+    public static final boolean FLYWHEEL_MOTOR_1_INVERTED = true;
+    public static final boolean FLYWHEEL_MOTOR_2_INVERTED = false;
+    public static final double FLYWHEEL_MOTOR_SPEED = 1.0;
+    public static final double FLYWHEEL_MOTOR_SPEED_SLOW = 0.15;
   }
 }

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

@@ -20,10 +20,12 @@
 import frc.robot.Constants.OperatorConstants;
 import frc.robot.commands.ConveyorCommand;
 import frc.robot.commands.DumpControl;
+import frc.robot.commands.FlyWCommand;
 import frc.robot.commands.IntakeCommand;
 import frc.robot.commands.swervedrive.drivebase.AbsoluteDriveAdv;
 import frc.robot.subsystems.Conveyor;
 import frc.robot.subsystems.Dump;
+import frc.robot.subsystems.FlyWheel;
 import frc.robot.subsystems.Intake;
 import frc.robot.subsystems.swervedrive.SwerveSubsystem;
 import java.io.File;
@@ -50,18 +52,20 @@ public class RobotContainer {
   private final Intake intake = new Intake();
   private final Dump dump = new Dump();
   private final Conveyor conveyor = new Conveyor();
+  private final FlyWheel flyWheel = new FlyWheel();
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
 
     intake.setDefaultCommand(new IntakeCommand(intake));
     dump.setDefaultCommand(new DumpControl(dump));
     conveyor.setDefaultCommand(new ConveyorCommand(conveyor));
+    flyWheel.setDefaultCommand(new FlyWCommand(flyWheel));
 
     // Configure the trigger bindings
     configureBindings();
 
-    AbsoluteDriveAdv closedAbsoluteDriveAdv =
+    final AbsoluteDriveAdv closedAbsoluteDriveAdv =
         new AbsoluteDriveAdv(
             drivebase,
             () -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND),
@@ -94,15 +98,15 @@ public RobotContainer() {
         drivebase.driveCommand(
             () -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND),
             () -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND),
-            () -> driverXbox.getRawAxis(2));
+            () -> driverXbox.getRightX() * 0.5);
 
     Command driveFieldOrientedDirectAngleSim =
         drivebase.simDriveCommand(
             () -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND),
             () -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND),
             () -> driverXbox.getRawAxis(2));
 
-    drivebase.setDefaultCommand(closedAbsoluteDriveAdv);
+    drivebase.setDefaultCommand(driveFieldOrientedDirectAngle);
   }
 
   /**

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

@@ -31,7 +31,7 @@ public void initialize() {}
   public void execute() {
     // TODO: Change this to the correct button
     if (DumpJoystick.getRawButton(1)) {
-      DumpSubsystem.openThenClose();
+      DumpSubsystem.open();
       // TODO: Change this to the correct button
     } else if (DumpJoystick.getRawButton(2)) {
       DumpSubsystem.close();

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

@@ -0,0 +1,50 @@
+// 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.wpilibj.Joystick;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.FlyWheel;
+
+public class FlyWCommand extends Command {
+  private final FlyWheel m_FlyWSubsystem;
+  // controller
+  // TODO: Change this to the correct controller
+  private final Joystick ConveyorJoystick = new Joystick(1);
+
+  public FlyWCommand(FlyWheel subsystem) {
+    m_FlyWSubsystem = subsystem;
+    addRequirements(m_FlyWSubsystem);
+  }
+
+  @Override
+  public void initialize() {}
+
+  @Override
+  public void execute() {
+    // TODO: Change this to the correct button
+    if (ConveyorJoystick.getRawButton(7) == true) {
+      m_FlyWSubsystem.enableflywheelfast();
+      // TODO: Change this to the correct button
+    } else if (ConveyorJoystick.getRawButton(8) == true) {
+      m_FlyWSubsystem.reverseflywheel();
+      System.out.println("Conveyor Moving in Reverse");
+    } else if (ConveyorJoystick.getRawButton(9) == true) {
+      m_FlyWSubsystem.enableflywheelslow();
+    } else {
+      m_FlyWSubsystem.disableflywheel();
+    }
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    m_FlyWSubsystem.disableflywheel();
+  }
+
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/subsystems/FlyWheel.java

@@ -0,0 +1,53 @@
+// 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.subsystems;
+
+import com.revrobotics.CANSparkLowLevel;
+import com.revrobotics.CANSparkMax;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.MechanismConstants;
+
+public class FlyWheel extends SubsystemBase {
+
+  private final CANSparkMax flywheelMotor1 =
+      new CANSparkMax(MechanismConstants.FLYWHEEL_MOTOR_1, CANSparkLowLevel.MotorType.kBrushless);
+  private final CANSparkMax flywheelMotor2 =
+      new CANSparkMax(MechanismConstants.FLYWHEEL_MOTOR_2, CANSparkLowLevel.MotorType.kBrushless);
+
+  /** Creates a new flywheel. */
+  public FlyWheel() {
+    flywheelMotor1.setInverted(MechanismConstants.FLYWHEEL_MOTOR_1_INVERTED);
+    flywheelMotor2.setInverted(MechanismConstants.FLYWHEEL_MOTOR_2_INVERTED);
+  }
+
+  /** Sets the flywheel motors to 100% power. */
+  public void enableflywheelfast() {
+    flywheelMotor1.set(MechanismConstants.FLYWHEEL_MOTOR_SPEED);
+    flywheelMotor2.set(MechanismConstants.FLYWHEEL_MOTOR_SPEED);
+  }
+
+  /** Sets the flywheel motors to slow speed */
+  public void enableflywheelslow() {
+    flywheelMotor1.set(MechanismConstants.FLYWHEEL_MOTOR_SPEED_SLOW);
+    flywheelMotor2.set(MechanismConstants.FLYWHEEL_MOTOR_SPEED_SLOW);
+  }
+
+  /** Sets the flywheel motors to 0% power. */
+  public void disableflywheel() {
+    flywheelMotor1.set(0);
+    flywheelMotor2.set(0);
+  }
+
+  /** Sets the flywheel motors to -100% power. (Reverse direction) */
+  public void reverseflywheel() {
+    flywheelMotor1.set(-MechanismConstants.FLYWHEEL_MOTOR_SPEED_SLOW);
+    flywheelMotor2.set(-MechanismConstants.FLYWHEEL_MOTOR_SPEED_SLOW);
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}