diff --git a/src/main/java/com/spartronics4915/frc/Constants.java b/src/main/java/com/spartronics4915/frc/Constants.java index 4d3c02d..e45665a 100644 --- a/src/main/java/com/spartronics4915/frc/Constants.java +++ b/src/main/java/com/spartronics4915/frc/Constants.java @@ -1,5 +1,7 @@ package com.spartronics4915.frc; +import edu.wpi.first.wpilibj.Joystick; + /** * The Constants class provides a convenient place for teams to hold robot-wide * numerical or boolean constants. This class should not be used for any other @@ -13,4 +15,51 @@ public final class Constants { public static final int kTestMotorId = 1; public static final double kTestMotorSpeed = 0.3; + + public static final class OI + { + public static final int kJoystickId = 0; + public static final int kDriveStraightDriveStickButton = 8; + + // this block also copied from ir2020 (magic) + public static final int kButtonBoardId = 1; + public static class DeviceSpec + { + public String name; + public int portId; + public int numButtons; + public Joystick joystick; + public DeviceSpec(String nm, int id, int nbut) + { + this.name = nm; + this.portId = id; + this.numButtons = nbut; + this.joystick = null; + } + } + public static DeviceSpec[] deviceList; + static + { + deviceList = new DeviceSpec[2]; + deviceList[0] = new DeviceSpec("Logitech Attack 3", kJoystickId, 12); + deviceList[1] = new DeviceSpec("ButtonBoard", kButtonBoardId, 18); + } + } + + // from atlas 2020 + // public static final class DriveStraightConstants + // { + // public static final double kP = 0; // Was 0.4 + // public static final double kI = 0.2; + // public static final double kD = 0; + // public static final double kAllowedError = 0; // In degrees + // } + + public static final class JoystickMotorConstants + { + public static final double kMotorSpeedMultiplier = 0.5; + public static final double kMotorSlowSpeed = 0.3; + public static final double kJoystickDeadzone = 0.1; + public static final double kJoystickResponseCurve = 5.0/3.0; + } } diff --git a/src/main/java/com/spartronics4915/frc/RobotContainer.java b/src/main/java/com/spartronics4915/frc/RobotContainer.java index 5b344e4..027c973 100644 --- a/src/main/java/com/spartronics4915/frc/RobotContainer.java +++ b/src/main/java/com/spartronics4915/frc/RobotContainer.java @@ -1,13 +1,22 @@ package com.spartronics4915.frc; +import com.spartronics4915.frc.Constants; import com.spartronics4915.frc.commands.ExampleCommand; +import com.spartronics4915.frc.commands.JoystickMotorCommand; import com.spartronics4915.frc.subsystems.ExampleSubsystem; +import com.spartronics4915.frc.subsystems.JoystickMotor; import com.spartronics4915.lib.subsystems.SpartronicsSubsystem; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj2.command.PIDCommand; +import edu.wpi.first.wpilibj2.command.ScheduleCommand; +import edu.wpi.first.wpilibj.controller.PIDController; +import edu.wpi.first.wpilibj.RobotBase; // @@ -21,22 +30,38 @@ public class RobotContainer { // The robot's subsystems and commands are defined here... - public final ExampleSubsystem mExampleSubsystem; - public final ExampleCommand mAutoCommand; + // public final ExampleSubsystem mExampleSubsystem; + public final JoystickMotor mJoystickMotor; + + // public final ExampleCommand mAutoCommand; + public final JoystickMotorCommand mJoystickMotorCommands; + + // private final int kJoystickPort = Constants.OI.kJoystickId; + public static final Joystick mDriverController = new Joystick(Constants.OI.kJoystickId); /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { + + // ...and constructed here. - mExampleSubsystem = ExampleSubsystem.getInstance(); - mAutoCommand = new ExampleCommand(mExampleSubsystem); + // mExampleSubsystem = ExampleSubsystem.getInstance(); + // mAutoCommand = new ExampleCommand(mExampleSubsystem); + + mJoystickMotor = JoystickMotor.getInstance(); + mJoystickMotorCommands = new JoystickMotorCommand(mJoystickMotor); configureButtonBindings(); SmartDashboard.putString("Container","Completed"); + + mJoystickMotor.setDefaultCommand(new JoystickMotorCommand(mJoystickMotor)); } /** Use this method to define your button ==> command mappings. */ - private void configureButtonBindings() {} + private void configureButtonBindings() + { + + } /** * Use this to pass the autonomous command to the main {@link Robot} class. @@ -45,6 +70,6 @@ private void configureButtonBindings() {} */ public Command getAutonomousCommand() { - return mAutoCommand; + return null; } } diff --git a/src/main/java/com/spartronics4915/frc/commands/JoystickMotorCommand.java b/src/main/java/com/spartronics4915/frc/commands/JoystickMotorCommand.java new file mode 100644 index 0000000..a2e2ba7 --- /dev/null +++ b/src/main/java/com/spartronics4915/frc/commands/JoystickMotorCommand.java @@ -0,0 +1,54 @@ +package com.spartronics4915.frc.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj.Joystick; + +import com.spartronics4915.frc.Constants.*; +import com.spartronics4915.frc.subsystems.JoystickMotor; + +public class JoystickMotorCommand extends CommandBase +{ + private final JoystickMotor mJoystickMotor; + private final Joystick mJoystick; + private boolean mInverted; + private boolean mSlow; + + public JoystickMotorCommand(JoystickMotor motor) + { + mJoystickMotor = motor; + mJoystick = motor.getJoystick(); + mInverted = false; + mSlow = false; + addRequirements(mJoystickMotor); + } + + @Override + public void execute() + { + double y = -1 * mJoystick.getY(); + + if (mSlow) + { + y *= JoystickMotorConstants.kMotorSlowSpeed; + } + + if (mInverted) + { + y *= -1; + } + + // copied from 2020-InfiniteRecharge, no idea how it works (magic) + y = Math.copySign(Math.pow(Math.abs(y), JoystickMotorConstants.kJoystickResponseCurve), y); // apply response curve + mJoystickMotor.set(applyDeadzone(y, JoystickMotorConstants.kJoystickDeadzone)); + } + + // also copied from ir2020 + private double applyDeadzone(double val, double deadzone) + { + if (Math.abs(val) < deadzone) + { + return 0.0; + } + return val; + } +} diff --git a/src/main/java/com/spartronics4915/frc/subsystems/JoystickMotor.java b/src/main/java/com/spartronics4915/frc/subsystems/JoystickMotor.java new file mode 100644 index 0000000..94f50f2 --- /dev/null +++ b/src/main/java/com/spartronics4915/frc/subsystems/JoystickMotor.java @@ -0,0 +1,61 @@ +package com.spartronics4915.frc.subsystems; + +import com.spartronics4915.frc.Constants; +import com.spartronics4915.lib.subsystems.SpartronicsSubsystem; + +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +import com.revrobotics.CANError; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + +public class JoystickMotor extends SpartronicsSubsystem +{ + private static JoystickMotor sInstance = null; + private static CANSparkMax mMotor; + private static Joystick mJoystick; + + public JoystickMotor() + { + boolean success = true; + try + { + mMotor = new CANSparkMax(Constants.kTestMotorId, MotorType.kBrushless); + mJoystick = new Joystick(Constants.OI.kJoystickId); + } + catch (Exception exception) + { + logException("Could not construct hardware: ", exception); + success = false; + } + logInitialized(success); + } + + public static JoystickMotor getInstance() + { + if (sInstance == null) { + sInstance = new JoystickMotor(); + } + return sInstance; + } + + public void set(double speed) + { + SmartDashboard.putString("Motor","Setting Speed"); + mMotor.set(speed); + SmartDashboard.putNumber("Speed",mMotor.get()); + } + + public double getSpeed() + { + SmartDashboard.putString("Motor","Getting Speed"); + return mMotor.get(); + } + + public Joystick getJoystick() + { + return mJoystick; + } +}