Merge pull request #9 from ss0g/main

joystick working

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

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

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

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