commit

src/main/java/frc4388/robot/Constants.java

@@ -24,8 +24,8 @@ import frc4388.utility.LEDPatterns;
 public final class Constants {
   public static final class SwerveDriveConstants {
 
-    public static final double MAX_ROT_SPEED = -0.7;
+    public static final double MAX_ROT_SPEED = 1.5;
-    public static final double MIN_ROT_SPEED = -0.3;
+    public static final double MIN_ROT_SPEED = 0.8;
     public static       double ROTATION_SPEED = MAX_ROT_SPEED;
 
     public static final class IDs {
@@ -65,7 +65,7 @@ public final class Constants {
     public static final class Conversions {
       public static final int CANCODER_TICKS_PER_ROTATION = 4096;
 
-      public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 5.8;
+      public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 4.8;
       public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 0.8;
 
       public static final double MOTOR_REV_PER_WHEEL_REV = 5.12;

src/main/java/frc4388/robot/Robot.java

@@ -10,10 +10,16 @@ package frc4388.robot;
 import java.lang.System;
 import java.lang.reflect.Array;
 import java.util.Arrays;
+
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.DemandType;
+import com.ctre.phoenix.motorcontrol.can.MotControllerJNI;
+
 import java.io.File;
 import java.io.IOException;
 import java.io.PrintWriter;
 
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -84,13 +90,27 @@ public class Robot extends TimedRobot {
    * You can use it to reset any subsystem information you want to clear when
    * the robot is disabled.
    */
+  boolean dis = false;
   @Override
   public void disabledInit() {
+    // m_robotContainer.m_robotClaw.setClaw(false);
     m_robotTime.endMatchTime();
+    m_robotContainer.m_robotClaw.disable();
+
+    m_handle = m_robotContainer.m_robotMap.leftBackWheel.getHandle();
   }
 
+  long m_handle = 0;
   @Override
   public void disabledPeriodic() {
+    // if (dis) {
+      
+    //   MotControllerJNI.Set_4(m_handle, ControlMode.PercentOutput.value, 1, 1, DemandType.Neutral.value);
+    //   // m_robotContainer.m_robotMap.leftBackSteer.set(ControlMode.PercentOutput, 1, DemandType.Neutral, 0);
+    //   // m_robotContainer.m_robotSwerveDrive.driveWithInput(new Translation2d(.5, 0), new Translation2d(), true);
+    // }
+
+    // System.out.println("hi from disabled");
   }
 
   /**
@@ -118,6 +138,9 @@ public class Robot extends TimedRobot {
   @Override
   public void teleopInit() {
     m_robotContainer.m_robotSwerveDrive.resetGyro();
+    dis = true;
+
+    m_robotContainer.m_robotClaw.enable();
 
     // This makes sure that the autonomous stops running when
     // teleop starts running. If you want the autonomous to

src/main/java/frc4388/robot/RobotContainer.java

@@ -67,8 +67,6 @@ public class RobotContainer {
     // private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt");
 
     private PlaybackChooser playbackChooser;
-    private PWM             servo      = new PWM(0);
-    private boolean         servo_open = true;
     /**
      * The container for the robot. Contains subsystems, OI devices, and commands.
      */
@@ -117,8 +115,8 @@ public class RobotContainer {
         new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
             .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive));
         
-        // new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
+        new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
-        //     .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(), m_robotSwerveDrive));
+            .onTrue(new InstantCommand(() -> m_robotSwerveDrive.toggleGear(), m_robotSwerveDrive));
         //     // .onFalse()
 
         new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
@@ -138,17 +136,13 @@ public class RobotContainer {
             .onFalse(new InstantCommand());
 
         // * Operator Buttons
-
-        // TODO: use the claw subsystem
-        // new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
-        //     .onTrue(new InstantCommand(() -> {
-        //         servo.setRaw(servo_open ? 1000 : 2000);
-        //         servo_open = !servo_open;
-        //     }));
-
         new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
+            // .onTrue(new InstantCommand(() -> System.out.println("Claw Button")));
             .onTrue(new InstantCommand(() -> m_robotClaw.toggle()));
 
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
+            .onTrue(new InstantCommand(() -> m_robotArm.killSoftLimits()));
+        
         new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
             .onTrue(new InstantCommand(() -> m_robotArm.resetTeleSoftLimit(), m_robotArm));
 

src/main/java/frc4388/robot/RobotMap.java

@@ -116,6 +116,11 @@ public class RobotMap {
     rightBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
     
     // set neutral mode
+    leftFrontWheel.setNeutralMode(NeutralMode.Brake);
+    rightFrontWheel.setNeutralMode(NeutralMode.Brake);
+    leftBackWheel.setNeutralMode(NeutralMode.Brake);
+    rightBackWheel.setNeutralMode(NeutralMode.Brake);
+
     leftFrontSteer.setNeutralMode(NeutralMode.Brake);
     rightFrontSteer.setNeutralMode(NeutralMode.Brake);
     leftBackSteer.setNeutralMode(NeutralMode.Brake);

src/main/java/frc4388/robot/subsystems/Arm.java

@@ -10,6 +10,8 @@ import com.ctre.phoenix.sensors.CANCoderConfiguration;
 import frc4388.robot.Constants.ArmConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class Arm extends SubsystemBase {
@@ -42,6 +44,32 @@ public class Arm extends SubsystemBase {
         CANCoderConfiguration config = new CANCoderConfiguration();
         config.magnetOffsetDegrees = ArmConstants.OFFSET;
         m_pivotEncoder.configAllSettings(config);
+        // m_pivot.configFactoryDefault();
+        // m_pivot.clearStickyFaults();
+        // m_pi
+
+        SmartDashboard.putNumber("kP Pivot", 0);
+        SmartDashboard.putNumber("kI Pivot", 0);
+        SmartDashboard.putNumber("kD Pivot", 0);
+
+        SmartDashboard.putData("Set PID", new InstantCommand(() -> {
+            TalonFXConfiguration pivotConfig_ = new TalonFXConfiguration();
+            pivotConfig_.slot0.kP = SmartDashboard.getNumber("kP Pivot", 0);
+            pivotConfig_.slot0.kI = SmartDashboard.getNumber("kI Pivot", 0);
+            pivotConfig_.slot0.kD = SmartDashboard.getNumber("kD Pivot", 0);
+
+            // pivotConfig_.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID();
+            // pivotConfig_.remoteFilter0.remoteSensorSource   = RemoteSensorSource.CANCoder;
+            // pivotConfig_.configSelectedFeedbackFilter();// FeedbackDevice.RemoteSensor0;
+            m_pivot.configAllSettings(pivotConfig_);
+            m_pivot.configRemoteFeedbackFilter(encoder.getDeviceID(), RemoteSensorSource.CANCoder, 0);
+            m_pivot.configSelectedFeedbackSensor(FeedbackDevice.RemoteSensor0);
+            // m_pivot.selectProfileSlot(0, 0);
+        }));
+
+        SmartDashboard.putData("set pos 1", new RunCommand(() -> m_pivot.set(ControlMode.Position, 225), this));
+        SmartDashboard.putData("set pos 2", new RunCommand(() -> m_pivot.set(ControlMode.Position, 135), this));
+        SmartDashboard.putData("Kill Self", new InstantCommand(() -> {}, this));
     }
 
     public Arm(WPI_TalonFX pivot, WPI_TalonFX tele, CANCoder encoder) {
@@ -49,11 +77,11 @@ public class Arm extends SubsystemBase {
     }
 
     public void setRotVel(double vel) {
-        m_pivot.set(ControlMode.PercentOutput, vel / 5);
+        m_pivot.set(ControlMode.PercentOutput, .4 * vel);
     }
 
     public void setTeleVel(double vel) {
-        m_tele.set(ControlMode.PercentOutput, -0.25 * vel);
+        m_tele.set(ControlMode.PercentOutput, -0.5 * vel);
     }
 
     public void armSetRotation(double rot) {
@@ -129,7 +157,8 @@ public class Arm extends SubsystemBase {
         tele_softLimit = !tele_softLimit;
     }
 
-    boolean resetable = true;
+    boolean resetable  = true;
+    boolean tele_reset = true;
 
     @Override
     public void periodic() {
@@ -141,8 +170,8 @@ public class Arm extends SubsystemBase {
             SmartDashboard.putNumber("start pivot", pivot_soft);
             SmartDashboard.putNumber("start tele", tele_soft);
             
-            m_pivot.configForwardSoftLimitEnable(true);
+            m_pivot.configForwardSoftLimitEnable(soft_limits);
-            m_pivot.configReverseSoftLimitEnable(true);
+            m_pivot.configReverseSoftLimitEnable(soft_limits);
             SmartDashboard.putNumber("fwd err", m_pivot.configForwardSoftLimitThreshold(1200 + pivot_soft).value);
             SmartDashboard.putNumber("rvs err", m_pivot.configReverseSoftLimitThreshold(pivot_soft).value);
             resetable = false;
@@ -150,12 +179,41 @@ public class Arm extends SubsystemBase {
             resetable = true;
         }
 
+        if (m_tele.isFwdLimitSwitchClosed() == 1 && tele_reset) {
+            var tele_soft = m_tele.getSelectedSensorPosition();
+            m_tele.configForwardSoftLimitThreshold(91000 - tele_soft);
+            m_tele.configReverseSoftLimitThreshold(1000  - tele_soft);
+            m_tele.configForwardSoftLimitEnable(true);
+            m_tele.configReverseSoftLimitEnable(true);
+            tele_reset = false;
+        } else if (m_tele.isFwdLimitSwitchClosed() == 0) {
+            tele_reset = true;
+        }
+
+        SmartDashboard.putBoolean("reverse", m_tele.isFwdLimitSwitchClosed() == 1);
+
         double x = Math.cos(degrees);
-        SwerveDriveConstants.ROTATION_SPEED = SwerveDriveConstants.MIN_ROT_SPEED + x * (SwerveDriveConstants.MAX_ROT_SPEED - SwerveDriveConstants.MIN_ROT_SPEED);
+        // SwerveDriveConstants.ROTATION_SPEED = SwerveDriveConstants.MIN_ROT_SPEED + x * (SwerveDriveConstants.MAX_ROT_SPEED - SwerveDriveConstants.MIN_ROT_SPEED);
         // if (m_debug)
         //     SmartDashboard.putNumber("Arm Motor", m_tele.getSelectedSensorPosition());
         SmartDashboard.putNumber("Pivot CANCoder", m_pivotEncoder.getAbsolutePosition());
         SmartDashboard.putNumber("Pivot IntegratedSensor", m_pivot.getSelectedSensorPosition());
         SmartDashboard.putNumber("Telescope Encoder", m_tele.getSelectedSensorPosition());
     }
+
+    boolean soft_limits = true;
+    public void killSoftLimits() {
+        var pivot_soft = m_pivot.getSelectedSensorPosition();
+        var tele_soft  = m_tele.getSelectedSensorPosition();
+        
+        SmartDashboard.putNumber("start pivot", pivot_soft);
+        SmartDashboard.putNumber("start tele", tele_soft);
+        
+        m_pivot.configForwardSoftLimitEnable(!soft_limits);
+        m_pivot.configReverseSoftLimitEnable(!soft_limits);
+        SmartDashboard.putNumber("fwd err", m_pivot.configForwardSoftLimitThreshold(1200 + pivot_soft).value);
+        SmartDashboard.putNumber("rvs err", m_pivot.configReverseSoftLimitThreshold(pivot_soft).value);
+
+        soft_limits = !soft_limits;
+    }
 }

src/main/java/frc4388/robot/subsystems/Claw.java

@@ -1,10 +1,12 @@
 package frc4388.robot.subsystems;
+import edu.wpi.first.hal.PWMJNI;
 import edu.wpi.first.wpilibj.PWM;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class Claw extends SubsystemBase {
 	private PWM m_clawMotor;
-	private boolean m_open = false;
+	private boolean m_open     = false;
+	private boolean m_disabled = false;
 
     // Opens claw
 	public Claw(PWM m_clawMotor) {
@@ -13,16 +15,37 @@ public class Claw extends SubsystemBase {
 	}
 
     public void setClaw(boolean open) {
+		if (m_disabled) return;
         // Open claw
+		// m_clawMotor.setRaw(150);
 		m_open = open;
-		m_clawMotor.setRaw(open ? 0 : 2000);
+		System.out.println("setClaw()");
+		// m_clawMotor.setPosition(0.5);
+		// m_clawMotor.setRaw(0);
+		// m_clawMotor.setRaw(m_open ? 0 : 255);
+		// m_clawMotor.setSpeed(m_open ? -1 : 1);
+		PWMJNI.setPWMSpeed(m_clawMotor.getHandle(), m_open ? -1 : 1);
+		// PWMJNI.setPWMDisabled(0);
+		System.out.println("Claw Pos: " + m_clawMotor.getRaw());
     }
 
 	public void toggle() {
+		System.out.println("toggle()");
 		setClaw(!m_open);
 	}
 
 	public boolean isClawOpen() {
 		return m_open;
 	}
+
+	public void disable() {
+		m_disabled = true;
+		// PWMJNI.setPWMRaw(m_clawMotor.getHandle(), PWMJNI.getPWMRaw(m_clawMotor.getHandle()));
+		PWMJNI.setPWMSpeed(m_clawMotor.getHandle(), 0.5);
+		// PWMJNI.setPWMDisabled(m_clawMotor.getHandle());
+	}
+
+	public void enable() {
+		m_disabled = false;
+	}
 }

src/main/java/frc4388/robot/subsystems/SwerveDrive.java

@@ -82,19 +82,21 @@ public class SwerveDrive extends SubsystemBase {
 
   public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
     if (fieldRelative) {
+
+      double rot = 0;
       if (rightStick.getNorm() > 0.1) {
         rotTarget = gyro.getRotation2d();
+        rot = rightStick.getX();
+      } else {
+        rot = rotTarget.minus(gyro.getRotation2d()).getRadians();
       }
 
-      double rot = rightStick.getX();
+      // Use the left joystick to set speed. Apply a cubic curve and the set max speed.
-
-      // Use the left joystick to set speed. Apply a quadratic curve and the set max speed.
       Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
       Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
 
       // Convert field-relative speeds to robot-relative speeds.
-      chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * cubedSpeed.getX(), cubedSpeed.getY(), rot * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1));
+      chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * cubedSpeed.getX(), cubedSpeed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1));
-
     } else {
       // Create robot-relative speeds.
       chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
@@ -235,8 +237,16 @@ public class SwerveDrive extends SubsystemBase {
    * Shifts gear from high to low, or vice versa.
    * @param shift true to shift to high, false to shift to low
    */
-  public void highSpeed(boolean shift) {
+  // public void highSpeed(boolean shift) {
-    this.speedAdjust = shift ? SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST : SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
+  //   this.speedAdjust = shift ? SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST : SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
+  // }
+
+  public void toggleGear() {
+    if (this.speedAdjust == SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW) {
+      this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
+    } else {
+      this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
+    }
   }
 
 }