Merge pull request #3 from Tigerbotics7125/merge-pathplanning-main

update pathfinding base
Tigerbotics7125 · Mar 16, 2024 · d265e03 · d265e03
2 parents 1789dc5 + 29492da
commit d265e03
Show file tree

Hide file tree

Showing 4 changed files with 49 additions and 25 deletions.
diff --git a/src/main/java/io/github/tigerbotics7125/Constants.java b/src/main/java/io/github/tigerbotics7125/Constants.java
@@ -81,9 +81,9 @@ public static final class Arm {
 
         public static final boolean kFollowerInverted = true;
 
-        public static final double kP = .04;
-        public static final double kI = 1e-7; // Integral term should be done with FF instead.
-        public static final double kD = .5;
+        public static final double kP = .01;
+        public static final double kI = 0; // Integral term should be done with FF instead.
+        public static final double kD = 0; // .5;
         public static final PIDController kPID = new PIDController(kP, kI, kD);
         // TODO look into sysid to characterize this.
         public static final double kS = 0;
@@ -94,14 +94,14 @@ public static final class Arm {
         public static final double kGearRatio = 1D / 48D;
         public static final double kChainRatio = 10D / 60D;
 
-        public static final double kPositionConversionFactor = kGearRatio * kChainRatio;
+        public static final double kPositionConversionFactor = kGearRatio * kChainRatio * 360;
         public static final double kVelocityConversionFactor = kPositionConversionFactor / 60D;
 
         public enum ArmState {
-            AMP(0D),
-            SPEAKER(60D),
-            SPEAKERAUTO(63D),
-            INTAKE(78D);
+            AMP(-82D),
+            SPEAKER(-10D),
+            SPEAKERAUTO(-10D),
+            INTAKE(0D);
 
             public final double kPosition;
 

diff --git a/src/main/java/io/github/tigerbotics7125/Robot.java b/src/main/java/io/github/tigerbotics7125/Robot.java
@@ -5,13 +5,16 @@
  */
 package io.github.tigerbotics7125;
 
+import com.revrobotics.CANSparkBase.IdleMode;
 import edu.wpi.first.cameraserver.CameraServer;
+import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
 import io.github.tigerbotics7125.Constants.Arm.ArmState;
 import io.github.tigerbotics7125.Constants.DriveTrain.ControlType;
 import io.github.tigerbotics7125.autos.*;
@@ -59,7 +62,11 @@ public void robotInit() {
             if (controlType.equals(ControlType.CURVE_ROCKETLEAGUE)) continue;
             m_driveControlChooser.addOption(controlType.name(), controlType);
         }
-
+        new Trigger(RobotController::getUserButton)
+                .onTrue(
+                        m_drivetrain
+                                .setIdleMode(IdleMode.kBrake)
+                                .andThen(m_arm.setIdleMode(IdleMode.kBrake)));
         m_drivetrain.setDefaultCommand(
                 Commands.select(
                         Map.of(

diff --git a/src/main/java/io/github/tigerbotics7125/subsystems/Arm.java b/src/main/java/io/github/tigerbotics7125/subsystems/Arm.java
@@ -5,11 +5,13 @@
  */
 package io.github.tigerbotics7125.subsystems;
 
+import com.revrobotics.CANSparkBase.IdleMode;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.RelativeEncoder;
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -37,14 +39,18 @@ public Arm() {
         configureMotor(m_left);
         configureMotor(m_right);
         m_right.follow(m_left, Constants.Arm.kFollowerInverted);
+
+        m_encoder.setPositionConversionFactor(Constants.Arm.kPositionConversionFactor);
+        m_encoder.setVelocityConversionFactor(Constants.Arm.kVelocityConversionFactor);
+        m_encoder.setPosition(0);
     }
 
     private void configureMotor(CANSparkMax motor) {
         motor.restoreFactoryDefaults();
         Timer.delay(.02);
 
         motor.setSmartCurrentLimit(Constants.Arm.kCurrentLimit);
-
+        motor.setIdleMode(IdleMode.kCoast);
         motor.burnFlash();
         Timer.delay(.02);
     }
@@ -64,9 +70,9 @@ public Command pidControl(ArmState state) {
                                 () -> {
                                     double pidContribution =
                                             12D * m_PID.calculate(m_encoder.getPosition());
-                                    double ffContribution =
-                                            m_feedforward.calculate(m_PID.getSetpoint(), 0);
-                                    m_left.setVoltage(pidContribution + ffContribution);
+                                    // double ffContribution =
+                                    //         m_feedforward.calculate(m_PID.getSetpoint(), 0);
+                                    m_left.setVoltage(pidContribution); // + ffContribution);
                                 }));
     }
 
@@ -82,10 +88,21 @@ public Command autoHome() {
         return Commands.none();
     }
 
+    public Command setIdleMode(IdleMode idleMode) {
+        return runOnce(
+                        () -> {
+                            m_left.setIdleMode(idleMode);
+                            m_right.setIdleMode(idleMode);
+                        })
+                .ignoringDisable(true);
+    }
+
     public Trigger atState() {
         return new Trigger(m_PID::atSetpoint);
     }
 
     @Override
-    public void periodic() {}
+    public void periodic() {
+        SmartDashboard.putNumber("arm", m_encoder.getPosition());
+    }
 }
diff --git a/src/main/java/io/github/tigerbotics7125/subsystems/Drivetrain.java b/src/main/java/io/github/tigerbotics7125/subsystems/Drivetrain.java
@@ -5,6 +5,7 @@
  */
 package io.github.tigerbotics7125.subsystems;
 
+import com.revrobotics.CANSparkBase.IdleMode;
 import com.ctre.phoenix.motorcontrol.FeedbackDevice;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 import com.kauailabs.navx.frc.AHRS;
@@ -62,7 +63,7 @@ private void configureMotor(CANSparkMax motor) {
         Timer.delay(.02);
 
         motor.setSmartCurrentLimit(Constants.DriveTrain.kCurrentLimit);
-
+        motor.setIdleMode(IdleMode.kCoast);
         motor.burnFlash();
         Timer.delay(.02);
     }
@@ -95,16 +96,15 @@ public Command curvatureDrive(
                 });
     }
 
-    public void resetOdometry(Rotation2d heading, Pose2d pose) {
-        m_odometry.resetPosition(
-                heading,
-                getLeftPositionMeters(),
-                getRightPositionMeters(),
-                pose);
-    }
-
-    public void resetGyro() {
-        m_gyro.reset();
+    public Command setIdleMode(IdleMode idleMode) {
+        return runOnce(
+                        () -> {
+                            frontLeft.setIdleMode(idleMode);
+                            frontRight.setIdleMode(idleMode);
+                            backLeft.setIdleMode(idleMode);
+                            backRight.setIdleMode(idleMode);
+                        })
+                .ignoringDisable(true);
     }
 
     @Override