From b05d9ad8574f916010fe7aba35e4eb2fc730646d Mon Sep 17 00:00:00 2001 From: ss0g <55930267+ss0g@users.noreply.github.com> Date: Fri, 14 Jan 2022 16:25:42 -0800 Subject: [PATCH 1/4] 1st iteration of joystickmotor test --- .../com/spartronics4915/frc/Constants.java | 38 ++++++++++++ .../spartronics4915/frc/RobotContainer.java | 24 ++++++-- .../frc/commands/JoystickMotorCommands.java | 61 +++++++++++++++++++ .../frc/subsystems/JoystickMotor.java | 61 +++++++++++++++++++ 4 files changed, 179 insertions(+), 5 deletions(-) create mode 100644 src/main/java/com/spartronics4915/frc/commands/JoystickMotorCommands.java create mode 100644 src/main/java/com/spartronics4915/frc/subsystems/JoystickMotor.java diff --git a/src/main/java/com/spartronics4915/frc/Constants.java b/src/main/java/com/spartronics4915/frc/Constants.java index 4d3c02d..c76ba96 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,40 @@ 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; + + // 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); + } + } + + public static final class JoystickMotorConstants + { + public static final double kMotorSpeedMultiplier = 1; + public static final double kMotorSlowSpeed = 0.3; + public static final double kJoystickDeadzone = 0.1; + } } diff --git a/src/main/java/com/spartronics4915/frc/RobotContainer.java b/src/main/java/com/spartronics4915/frc/RobotContainer.java index 5b344e4..d8402ad 100644 --- a/src/main/java/com/spartronics4915/frc/RobotContainer.java +++ b/src/main/java/com/spartronics4915/frc/RobotContainer.java @@ -1,13 +1,17 @@ package com.spartronics4915.frc; import com.spartronics4915.frc.commands.ExampleCommand; +import com.spartronics4915.frc.commands.JoystickMotorCommands; 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.wpilibj.RobotBase; // @@ -21,15 +25,25 @@ 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 JoystickMotorCommands mJoystickMotorCommands; + + private final int mJoystickPort = 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 JoystickMotorCommands(mJoystickMotor); configureButtonBindings(); SmartDashboard.putString("Container","Completed"); @@ -45,6 +59,6 @@ private void configureButtonBindings() {} */ public Command getAutonomousCommand() { - return mAutoCommand; + return null; } } diff --git a/src/main/java/com/spartronics4915/frc/commands/JoystickMotorCommands.java b/src/main/java/com/spartronics4915/frc/commands/JoystickMotorCommands.java new file mode 100644 index 0000000..ddef593 --- /dev/null +++ b/src/main/java/com/spartronics4915/frc/commands/JoystickMotorCommands.java @@ -0,0 +1,61 @@ +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 JoystickMotorCommands +{ + private final JoystickMotor mMotor; + private final Joystick mJoystick; + private boolean mInverted; + private boolean mSlow; + + public JoystickMotorCommands(JoystickMotor motor) + { + mMotor = motor; + mJoystick = motor.getJoystick(); + mInverted = false; + mSlow = false; + } + + public class TeleOpCommand extends CommandBase + { + public TeleOpCommand() + { + addRequirements(mMotor); + } + + @Override + public void execute() + { + double y = -1 * mJoystick.getY(); + + if (mSlow) + { + y *= Constants.JoystickMotorConstants.kMotorSlowSpeed; + } + + if (mInverted) + { + y *= -1; + } + + // copied from 2020-InfiniteRecharge, no idea how it works (magic) + y = Math.copySign(Math.pow(Math.abs(y), 5.0/3.0), y); // apply response curve + mMotor.set(applyDeadzone(y, Constants.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; + } +} From 9958e57b5f23ab383e567d30d6329eb8d5bf7e25 Mon Sep 17 00:00:00 2001 From: ss0g <55930267+ss0g@users.noreply.github.com> Date: Fri, 14 Jan 2022 17:37:55 -0800 Subject: [PATCH 2/4] joystickmotor 2 --- .../java/com/spartronics4915/frc/Constants.java | 10 ++++++++++ .../com/spartronics4915/frc/RobotContainer.java | 15 +++++++++++++-- .../frc/commands/JoystickMotorCommands.java | 10 +++++----- 3 files changed, 28 insertions(+), 7 deletions(-) diff --git a/src/main/java/com/spartronics4915/frc/Constants.java b/src/main/java/com/spartronics4915/frc/Constants.java index c76ba96..2391a0f 100644 --- a/src/main/java/com/spartronics4915/frc/Constants.java +++ b/src/main/java/com/spartronics4915/frc/Constants.java @@ -19,6 +19,7 @@ public final class Constants 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; @@ -45,6 +46,15 @@ public DeviceSpec(String nm, int id, int nbut) } } + // 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 = 1; diff --git a/src/main/java/com/spartronics4915/frc/RobotContainer.java b/src/main/java/com/spartronics4915/frc/RobotContainer.java index d8402ad..03bef79 100644 --- a/src/main/java/com/spartronics4915/frc/RobotContainer.java +++ b/src/main/java/com/spartronics4915/frc/RobotContainer.java @@ -1,5 +1,6 @@ package com.spartronics4915.frc; +import com.spartronics4915.frc.Constants; import com.spartronics4915.frc.commands.ExampleCommand; import com.spartronics4915.frc.commands.JoystickMotorCommands; import com.spartronics4915.frc.subsystems.ExampleSubsystem; @@ -11,6 +12,10 @@ 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; // @@ -31,7 +36,8 @@ public class RobotContainer // public final ExampleCommand mAutoCommand; public final JoystickMotorCommands mJoystickMotorCommands; - private final int mJoystickPort = Constants.OI.kJoystickId; + // 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() @@ -47,10 +53,15 @@ public RobotContainer() configureButtonBindings(); SmartDashboard.putString("Container","Completed"); + + mJoystickMotor.setDefaultCommand(new JoystickMotorCommands(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. diff --git a/src/main/java/com/spartronics4915/frc/commands/JoystickMotorCommands.java b/src/main/java/com/spartronics4915/frc/commands/JoystickMotorCommands.java index ddef593..00b55d1 100644 --- a/src/main/java/com/spartronics4915/frc/commands/JoystickMotorCommands.java +++ b/src/main/java/com/spartronics4915/frc/commands/JoystickMotorCommands.java @@ -6,16 +6,16 @@ import com.spartronics4915.frc.Constants; import com.spartronics4915.frc.subsystems.JoystickMotor; -public class JoystickMotorCommands +public class JoystickMotorCommands extends CommandBase { - private final JoystickMotor mMotor; + private final JoystickMotor mJoystickMotor; private final Joystick mJoystick; private boolean mInverted; private boolean mSlow; public JoystickMotorCommands(JoystickMotor motor) { - mMotor = motor; + mJoystickMotor = motor; mJoystick = motor.getJoystick(); mInverted = false; mSlow = false; @@ -25,7 +25,7 @@ public class TeleOpCommand extends CommandBase { public TeleOpCommand() { - addRequirements(mMotor); + addRequirements(mJoystickMotor); } @Override @@ -45,7 +45,7 @@ public void execute() // copied from 2020-InfiniteRecharge, no idea how it works (magic) y = Math.copySign(Math.pow(Math.abs(y), 5.0/3.0), y); // apply response curve - mMotor.set(applyDeadzone(y, Constants.JoystickMotorConstants.kJoystickDeadzone)); + mJoystickMotor.set(applyDeadzone(y, Constants.JoystickMotorConstants.kJoystickDeadzone)); } // also copied from ir2020 From 01f1c2de5cf3048188b82e9ca165c78e64f49f9d Mon Sep 17 00:00:00 2001 From: ss0g <55930267+ss0g@users.noreply.github.com> Date: Fri, 14 Jan 2022 17:58:38 -0800 Subject: [PATCH 3/4] got joystick to work --- .../spartronics4915/frc/RobotContainer.java | 8 +-- .../frc/commands/JoystickMotorCommand.java | 54 ++++++++++++++++ .../frc/commands/JoystickMotorCommands.java | 61 ------------------- 3 files changed, 58 insertions(+), 65 deletions(-) create mode 100644 src/main/java/com/spartronics4915/frc/commands/JoystickMotorCommand.java delete mode 100644 src/main/java/com/spartronics4915/frc/commands/JoystickMotorCommands.java diff --git a/src/main/java/com/spartronics4915/frc/RobotContainer.java b/src/main/java/com/spartronics4915/frc/RobotContainer.java index 03bef79..027c973 100644 --- a/src/main/java/com/spartronics4915/frc/RobotContainer.java +++ b/src/main/java/com/spartronics4915/frc/RobotContainer.java @@ -2,7 +2,7 @@ import com.spartronics4915.frc.Constants; import com.spartronics4915.frc.commands.ExampleCommand; -import com.spartronics4915.frc.commands.JoystickMotorCommands; +import com.spartronics4915.frc.commands.JoystickMotorCommand; import com.spartronics4915.frc.subsystems.ExampleSubsystem; import com.spartronics4915.frc.subsystems.JoystickMotor; import com.spartronics4915.lib.subsystems.SpartronicsSubsystem; @@ -34,7 +34,7 @@ public class RobotContainer public final JoystickMotor mJoystickMotor; // public final ExampleCommand mAutoCommand; - public final JoystickMotorCommands mJoystickMotorCommands; + public final JoystickMotorCommand mJoystickMotorCommands; // private final int kJoystickPort = Constants.OI.kJoystickId; public static final Joystick mDriverController = new Joystick(Constants.OI.kJoystickId); @@ -49,12 +49,12 @@ public RobotContainer() // mAutoCommand = new ExampleCommand(mExampleSubsystem); mJoystickMotor = JoystickMotor.getInstance(); - mJoystickMotorCommands = new JoystickMotorCommands(mJoystickMotor); + mJoystickMotorCommands = new JoystickMotorCommand(mJoystickMotor); configureButtonBindings(); SmartDashboard.putString("Container","Completed"); - mJoystickMotor.setDefaultCommand(new JoystickMotorCommands(mJoystickMotor)); + mJoystickMotor.setDefaultCommand(new JoystickMotorCommand(mJoystickMotor)); } /** Use this method to define your button ==> command mappings. */ 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..a372a47 --- /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 *= Constants.JoystickMotorConstants.kMotorSlowSpeed; + } + + if (mInverted) + { + y *= -1; + } + + // copied from 2020-InfiniteRecharge, no idea how it works (magic) + y = Math.copySign(Math.pow(Math.abs(y), 5.0/3.0), y); // apply response curve + mJoystickMotor.set(applyDeadzone(y, Constants.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/commands/JoystickMotorCommands.java b/src/main/java/com/spartronics4915/frc/commands/JoystickMotorCommands.java deleted file mode 100644 index 00b55d1..0000000 --- a/src/main/java/com/spartronics4915/frc/commands/JoystickMotorCommands.java +++ /dev/null @@ -1,61 +0,0 @@ -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 JoystickMotorCommands extends CommandBase -{ - private final JoystickMotor mJoystickMotor; - private final Joystick mJoystick; - private boolean mInverted; - private boolean mSlow; - - public JoystickMotorCommands(JoystickMotor motor) - { - mJoystickMotor = motor; - mJoystick = motor.getJoystick(); - mInverted = false; - mSlow = false; - } - - public class TeleOpCommand extends CommandBase - { - public TeleOpCommand() - { - addRequirements(mJoystickMotor); - } - - @Override - public void execute() - { - double y = -1 * mJoystick.getY(); - - if (mSlow) - { - y *= Constants.JoystickMotorConstants.kMotorSlowSpeed; - } - - if (mInverted) - { - y *= -1; - } - - // copied from 2020-InfiniteRecharge, no idea how it works (magic) - y = Math.copySign(Math.pow(Math.abs(y), 5.0/3.0), y); // apply response curve - mJoystickMotor.set(applyDeadzone(y, Constants.JoystickMotorConstants.kJoystickDeadzone)); - } - - // also copied from ir2020 - private double applyDeadzone(double val, double deadzone) - { - if (Math.abs(val) < deadzone) - { - return 0.0; - } - return val; - } - } -} From 5c37552b8fe03c7de2c990626d30832f08e69b47 Mon Sep 17 00:00:00 2001 From: ss0g <55930267+ss0g@users.noreply.github.com> Date: Fri, 14 Jan 2022 18:55:47 -0800 Subject: [PATCH 4/4] idk what really changed --- .../java/com/spartronics4915/frc/Constants.java | 17 +++++++++-------- .../frc/commands/JoystickMotorCommand.java | 8 ++++---- 2 files changed, 13 insertions(+), 12 deletions(-) diff --git a/src/main/java/com/spartronics4915/frc/Constants.java b/src/main/java/com/spartronics4915/frc/Constants.java index 2391a0f..e45665a 100644 --- a/src/main/java/com/spartronics4915/frc/Constants.java +++ b/src/main/java/com/spartronics4915/frc/Constants.java @@ -47,18 +47,19 @@ public DeviceSpec(String nm, int id, int nbut) } // 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 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 = 1; + 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/commands/JoystickMotorCommand.java b/src/main/java/com/spartronics4915/frc/commands/JoystickMotorCommand.java index a372a47..a2e2ba7 100644 --- a/src/main/java/com/spartronics4915/frc/commands/JoystickMotorCommand.java +++ b/src/main/java/com/spartronics4915/frc/commands/JoystickMotorCommand.java @@ -3,7 +3,7 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpilibj.Joystick; -import com.spartronics4915.frc.Constants; +import com.spartronics4915.frc.Constants.*; import com.spartronics4915.frc.subsystems.JoystickMotor; public class JoystickMotorCommand extends CommandBase @@ -29,7 +29,7 @@ public void execute() if (mSlow) { - y *= Constants.JoystickMotorConstants.kMotorSlowSpeed; + y *= JoystickMotorConstants.kMotorSlowSpeed; } if (mInverted) @@ -38,8 +38,8 @@ public void execute() } // copied from 2020-InfiniteRecharge, no idea how it works (magic) - y = Math.copySign(Math.pow(Math.abs(y), 5.0/3.0), y); // apply response curve - mJoystickMotor.set(applyDeadzone(y, Constants.JoystickMotorConstants.kJoystickDeadzone)); + y = Math.copySign(Math.pow(Math.abs(y), JoystickMotorConstants.kJoystickResponseCurve), y); // apply response curve + mJoystickMotor.set(applyDeadzone(y, JoystickMotorConstants.kJoystickDeadzone)); } // also copied from ir2020