Merge pull request #266 from CyberCoyotes/gyro-adjustment

Gyro adjustment

src/main/deploy/pathplanner/paths/2meters.path

@@ -32,14 +32,14 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxVelocity": 2.0,
+    "maxAcceleration": 2.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 180.0,
+    "rotation": 0.0,
     "rotateFast": false
   },
   "reversed": false,

src/main/java/frc/robot/Robot.java

@@ -12,6 +12,7 @@
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.subsystems.Gyro; // TODO Added by Scoy
 
 @SuppressWarnings("unused")
 
@@ -20,6 +21,8 @@ public class Robot extends TimedRobot {
 
   private RobotContainer m_robotContainer;
 
+  private Gyro gyro = new Gyro(); // Added by Scoy
+
   @Override
   public void robotInit() {
     m_robotContainer = new RobotContainer();
@@ -54,6 +57,9 @@ public void disabledExit() {
 
   @Override
   public void autonomousInit() {
+
+    // gyro.setAutonStartingPose180(180);
+    // gyro.setAutonStartingPose(-180); // TODO Seems to impact Teleop and auton
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
 
     if (m_autonomousCommand != null) {

src/main/java/frc/robot/experimental/ArmConstants.java.txt

@@ -0,0 +1,10 @@
+/*
+Use this approach in offseason.
+See TunerConstants.java for an example of how to setup
+*/
+
+package frc.robot.subsystems;
+
+public class ArmConstants {
+
+}

src/main/java/frc/robot/experimental/GyroIO.java

@@ -0,0 +1,39 @@
+/* This is from FRC 2910, 2023 season with some 
+ * updates for this year's version of the CTRE Phoenix library.
+ */
+package frc.robot.experimental;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+// import org.littletonrobotics.junction.AutoLog;
+
+/**
+ * Connects the software to the hardware and directly receives data from the gyroscope.
+ */
+public interface GyroIO {
+    /**
+     * Reads information from sources (hardware or simulation) and updates the inputs object.
+     *
+     * @param inputs Contains the defaults for the input values listed above.
+     */
+    // default void updateInputs(GyroIOInputs inputs) {}
+
+    default BaseStatusSignal[] getSignals() {
+        return new BaseStatusSignal[0];
+    }
+
+
+    /**
+     * Holds data that can be read from the corresponding gyroscope IO implementation.
+     */
+
+    /*
+    @AutoLog
+    class GyroIOInputs {
+        public boolean connected = false;
+        public double yaw = 0;
+        public double pitch = 0;
+        public double roll = 0;
+        public double angularVelocity = 0;
+    }
+    */
+}

src/main/java/frc/robot/experimental/GyroIOPigeon2.java

@@ -0,0 +1,81 @@
+/* 
+* This is from FRC 2910, 2023 season with some 
+* updates for this year's version of the CTRE Phoenix library.
+*/
+
+package frc.robot.experimental;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.GyroTrimConfigs;
+import com.ctre.phoenix6.configs.MountPoseConfigs;
+import com.ctre.phoenix6.configs.Pigeon2Configuration;
+import com.ctre.phoenix6.hardware.Pigeon2;
+import edu.wpi.first.math.util.Units;
+import frc.robot.util.Constants;
+
+
+@SuppressWarnings("unused")
+
+/**
+ * Implementation of the gyroscope class. Uses the Pigeon2 gyroscope to receive data about the robots yaw, pitch, and roll values.
+ */
+public class GyroIOPigeon2 implements GyroIO {
+
+    /**
+     * Pigeon2 gyroscope object. Receives direct data from the gyroscope about the yaw, pitch, and roll.
+     */
+   private final Pigeon2 gyro;
+
+    /**
+     * Pigeon2 gyroIO implementation.
+     *
+     * @param canId  Is the unique identifier to the gyroscope used on the robot.
+     * @param canBus The name of the CAN bus the device is connected to.
+     */
+    private StatusSignal<Double> yawSignal;
+
+    private StatusSignal<Double> angularVelocitySignal;
+
+    private BaseStatusSignal[] signals;
+
+    public GyroIOPigeon2(int mountPose, double error, String canBus) {
+
+        gyro = new Pigeon2(mountPose, canBus);
+
+        Pigeon2Configuration config = new Pigeon2Configuration();
+        MountPoseConfigs mountPoseConfigs = new MountPoseConfigs();
+        mountPoseConfigs.MountPoseYaw = mountPose;
+        GyroTrimConfigs gyroTrimConfigs = new GyroTrimConfigs();
+        gyroTrimConfigs.GyroScalarZ = error;
+        config.MountPose = mountPoseConfigs;
+        config.GyroTrim = gyroTrimConfigs;
+        gyro.getConfigurator().apply(config);
+
+        yawSignal = gyro.getYaw();
+        // angularVelocitySignal = gyro.getAngularVelocityZ();
+        angularVelocitySignal = gyro.getAngularVelocityZDevice(); // Not sure if this is the correct method to use or ZWorld
+
+        signals = new BaseStatusSignal[2];
+        signals[0] = yawSignal;
+        signals[1] = angularVelocitySignal;
+    } 
+
+
+    /*
+    @Override
+    public void updateInputs(GyroIOInputs inputs) {
+        inputs.connected = true;
+
+        inputs.yaw = Units.degreesToRadians(
+                BaseStatusSignalValue.getLatencyCompensatedValue(yawSignal, angularVelocitySignal));
+        inputs.pitch = Units.degreesToRadians(gyro.getPitch().getValue());
+        inputs.roll = Units.degreesToRadians(gyro.getRoll().getValue());
+        inputs.angularVelocity = Units.degreesToRadians(angularVelocitySignal.getValue());
+    }
+ */
+    @Override
+    public BaseStatusSignal[] getSignals() {
+        return signals;
+    }
+}

src/main/java/frc/robot/experimental/ShooterConstants.java.txt

@@ -0,0 +1,110 @@
+package frc.robot.subsystems;
+
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.VoltageConfigs;
+import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.configs.VoltageConfigs;
+import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+import frc.robot.Constants;
+
+public class ShooterConstants {
+
+    // Declare variables for the motors to be controlled
+    private TalonFX m_primaryMotor = new TalonFX(Constants.CANIDs.RIGHT_FLYWHEEL_ID, "rio"); // Right
+    private TalonFX m_secondaryMotor = new TalonFX(Constants.CANIDs.LEFT_FLYWHEEL_ID, "rio"); // Left
+
+    // TODO Tune this value
+    public final double FLYWHEEL_VELOCITY = 100; // rotations per second (rps)
+    public final double FLYWHEEL_VELOCITY_SHORT_RANGE = 40; // Not currently in use anywhere
+    public final double FLYWHEEL_VELOCITY_LONG_RANGE = 60; // Not currently in use anywhere, Flywheel speed for long range shots only
+    public final double FLYWHEEL_IDLE_VELOCITY = FLYWHEEL_VELOCITY * 0.30; // 30% of max speed
+    public final double FLYWHEEL_MARGIN_ERROR = FLYWHEEL_VELOCITY * 0.10; // 5% of max speed
+    public final double FLYWHEEL_MIN = FLYWHEEL_VELOCITY * .95;
+    public final double FLYWHEEL_MAX = FLYWHEEL_VELOCITY * 1.05;
+
+
+    // Class member variables
+    private VelocityVoltage m_velocityVoltage = new VelocityVoltage(0);
+
+    private double currentFlywheelVel = m_primaryMotor.getVelocity().getValue();
+        m_primaryMotor.setControl(m_velocityVoltage);
+        m_secondaryMotor.setControl(m_velocityVoltage);
+
+        m_primaryMotor.getConfigurator().apply(new TalonFXConfiguration());
+        m_secondaryMotor.getConfigurator().apply(new TalonFXConfiguration());
+
+        /* Various Configs */
+        /*
+        | Ks    |   output to overcome static friction (output)
+        | Kv    |   output per unit of velocity (output/rps)
+        | Ka    |   output per unit of acceleration; unused, as there is no target acceleration (output/(rps/s))
+        | Kp    |   output per unit of error (output/rps)
+        | Ki    |   output per unit of integrated error in velocity (output/rotation)
+        | kd    |   output per unit of error derivative in velocity (output/(rps/s))
+        */
+
+        /* Example provided by CTRE */
+        /*
+        slot0Configs.kS = 0.05; // Add 0.05 V output to overcome static friction
+        slot0Configs.kV = 0.12; // A velocity target of 1 rps results in 0.12 V output
+        slot0Configs.kP = 0.11; // An error of 1 rps results in 0.11 V output
+        slot0Configs.kI = 0; // no output for integrated error
+        slot0Configs.kD = 0; // no output for error derivative
+         */
+
+        var flywheelConfigs0 = new Slot0Configs();        
+            flywheelConfigs0
+                // https://github.com/CrossTheRoadElec/Phoenix6-Examples/blob/main/java/VelocityClosedLoop/src/main/java/frc/robot/Robot.java
+                .withKP(0.10)
+                .withKI(0.00)
+                .withKS(0.00) // Should low for a light flywheel? Maybe the pulley strength would impact it though?
+                .withKV(0.00);
+
+
+        var flywheelVelocityConfig = new VoltageConfigs();
+            flywheelVelocityConfig
+                .withPeakForwardVoltage(12)  // FRC 2910 running 12
+                .withPeakReverseVoltage(12); // Originally -8, with negative the "helper" text goes away
+
+        var flywheelCurrentConfigs = new CurrentLimitsConfigs();
+        flywheelCurrentConfigs
+                .withStatorCurrentLimit(60) 
+                .withStatorCurrentLimitEnable(true);
+
+        var flywheelMotorOutput = new MotorOutputConfigs();
+        flywheelMotorOutput
+                .withNeutralMode(NeutralModeValue.Coast);
+
+        /* Apply Configs */
+        m_primaryMotor.getConfigurator().apply(flywheelConfigs0);
+        m_secondaryMotor.getConfigurator().apply(flywheelConfigs0);
+
+        m_primaryMotor.getConfigurator().apply(flywheelVelocityConfig);
+        m_secondaryMotor.getConfigurator().apply(flywheelVelocityConfig);
+
+        m_primaryMotor.getConfigurator().apply(flywheelCurrentConfigs);
+        m_secondaryMotor.getConfigurator().apply(flywheelCurrentConfigs);
+
+        m_primaryMotor.getConfigurator().apply(flywheelMotorOutput);
+        m_secondaryMotor.getConfigurator().apply(flywheelMotorOutput);
+
+        m_primaryMotor.setInverted(true);
+        m_secondaryMotor.setInverted(true);
+
+        // Alternate way to get the current flywheel velocity?
+}

src/main/java/frc/robot/subsystems/Gyro.java

@@ -1,22 +1,24 @@
 package frc.robot.subsystems;
 
+import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.configs.MountPoseConfigs;
 import com.ctre.phoenix6.configs.Pigeon2Configuration;
 import com.ctre.phoenix6.hardware.Pigeon2;
 
- /* 
-    Pigeon2 treats +X as the forward axis
-    +Y as the left axis
-    +Z towards the sky 
-    This is a common way to define the world frame reference for groundbased vehicles
-   */
-
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.util.Units;
+
 public class Gyro {
 
     private final Pigeon2 pidgey;
 
+    private final double startingPose;
+
     public Gyro() {
         pidgey = new Pigeon2(14); // ID from Change the ID as needed
+
+        this.startingPose = 180;
 
         // StatusSignal<Double> yawSignal;
 
@@ -57,10 +59,16 @@ public double Rotation2d() {
         * Maybe, but not 100% sure
         */
 
-        public void resetGyro() {
-            pidgey.reset();
+
+        public void setAutonStartingPose(double startingPose) {
+            pidgey.setYaw(startingPose);
         }
-
+
+
+        public void setAutonStartingPose180(double startingPose) {
+            pidgey.setYaw(180);
+        }
+
 
 
     } // end of class Gyro