1st iteration of joystickmotor test

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;
+    }
 }

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;
     }
 }

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;
+        }
+    }
+}

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;
+    }
+}