Merge pull request #34 from Team4388/arm

Armfjklffjklfjefjfjiofjio

yes

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2023.1.1"
+    id "edu.wpi.first.GradleRIO" version "2023.3.2"
 }
 
 sourceCompatibility = JavaVersion.VERSION_11

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

@@ -105,13 +105,28 @@ public final class Constants {
   public static final class GyroConstants {
     public static final int ID = 14; // TODO: find the actual ID
   }
-  
+
   public static final class ArmConstants {
-      public static final int MIN_ARM_LEN = 0;
+    public static final double MIN_ARM_LEN = 1;
-      public static final int MAX_ARM_LEN = 1;
+    public static final double MAX_ARM_LEN = 2;
-      public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
+    public static final double ARM_HEIGHT  = 1;
+    public static final double CURVE_POWER = 2;
+
+    public static final double TELE_TICKS_PER_SECOND = (-5);
+    
+    public static final double PIVOT_FORWARD_SOFT_LIMIT = -1.0; // TODO: find actual value
+    public static final double PIVOT_REVERSE_SOFT_LIMIT = -1.0; // TODO: find actual value
+    
+    public static final double TELE_FORWARD_SOFT_LIMIT = 0;
+    public static final double TELE_REVERSE_SOFT_LIMIT = 0;
+
+    public static final double kP = 0;
+    public static final double kI = 0;
+    public static final double kD = 0;
+
+    public static final double OFFSET = 0;
   }
-  
+
   public static final class LEDConstants {
     // public static final int LED_SPARK_ID = 0;
 

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

@@ -7,40 +7,17 @@
 
 package frc4388.robot;
 
-import java.util.ArrayList;
-import java.util.List;
-
-import org.opencv.objdetect.HOGDescriptor;
-
-import edu.wpi.first.math.controller.HolonomicDriveController;
-import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.controller.ProfiledPIDController;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.trajectory.Trajectory;
-import edu.wpi.first.math.trajectory.TrajectoryConfig;
-import edu.wpi.first.math.trajectory.TrajectoryGenerator;
-import edu.wpi.first.math.trajectory.Trajectory.State;
-import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import edu.wpi.first.wpilibj2.command.SwerveControllerCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc4388.robot.Constants.*;
-import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
+import frc4388.robot.subsystems.Arm;
-import frc4388.robot.Constants.SwerveDriveConstants.PIDConstants;
 import frc4388.robot.commands.AutoBalance;
 import frc4388.robot.subsystems.SwerveDrive;
-import frc4388.utility.controller.IHandController;
-import frc4388.robot.commands.JoystickPlayback;
 import frc4388.robot.commands.JoystickRecorder;
 import frc4388.robot.commands.PlaybackChooser;
-import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.utility.controller.DeadbandedXboxController;
 import frc4388.utility.controller.XboxController;
 
@@ -56,6 +33,10 @@ public class RobotContainer {
     public final RobotMap m_robotMap = new RobotMap();
 
     /* Subsystems */
+    private final Arm m_robotArm = new Arm(m_robotMap.pivot, m_robotMap.tele, m_robotMap.pivotEncoder);
+    // private final LED m_robotLED = new LED(m_robotMap.LEDController);
+
+    
     public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront,
                                                                   m_robotMap.rightFront,
                                                                   m_robotMap.leftBack,
@@ -69,7 +50,7 @@ public class RobotContainer {
     /* Autos */
     public SendableChooser<Command> chooser = new SendableChooser<>();
     
-    private Command noAuto = new InstantCommand();
+    // private Command noAuto = new InstantCommand();
     
     // private Command balance = new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive);
     
@@ -101,7 +82,6 @@ public class RobotContainer {
 
         // chooser.addOption("Blue1Path", blue1Path);
         // chooser.addOption("Blue1PathWithBalance", blue1PathWithBalance);
-
         // chooser.addOption("Red1Path", red1Path);
         // chooser.addOption("Red1PathWithBalance", red1PathWithBalance);
         

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

@@ -7,11 +7,12 @@
 
 package frc4388.robot;
 
-import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 import com.ctre.phoenix.motorcontrol.NeutralMode;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 import com.ctre.phoenix.sensors.CANCoder;
 import com.ctre.phoenix.sensors.WPI_Pigeon2;
 
+import frc4388.robot.Constants.ArmConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.robot.subsystems.SwerveModule;
 import frc4388.utility.RobotGyro;
@@ -30,10 +31,10 @@ public class RobotMap {
   public SwerveModule leftBack;
   public SwerveModule rightBack;
 
-
   public RobotMap() {
     configureLEDMotorControllers();
     configureDriveMotors();
+    configArmMotors();
   }
 
   /* LED Subsystem */
@@ -60,7 +61,6 @@ public class RobotMap {
   public final WPI_TalonFX rightBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER_ID);
   public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID);
 
-
   void configureDriveMotors() {
     // config factory default
     leftFrontWheel.configFactoryDefault();
@@ -126,4 +126,38 @@ public class RobotMap {
     this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, -240.029297);
     this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, -40.869142);
   }
+
+  // arm stuff
+  public WPI_TalonFX pivot        = new WPI_TalonFX(-1); // TODO: Add real id
+  public WPI_TalonFX tele         =  new WPI_TalonFX(-1); // TODO: Add real id
+  public CANCoder    pivotEncoder = new CANCoder(-1);
+
+  public void configArmMotors() {
+    // config factory default
+    pivot.configFactoryDefault();
+    tele.configFactoryDefault();
+
+    // config open loop ramp
+    pivot.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+    tele.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+
+    // config closed loop ramp
+    pivot.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+    tele.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+
+    // config neutral mode to brake
+    pivot.setNeutralMode(NeutralMode.Brake);
+    tele.setNeutralMode(NeutralMode.Brake);
+
+    // soft limits
+    pivot.configForwardSoftLimitThreshold(ArmConstants.PIVOT_FORWARD_SOFT_LIMIT);
+    pivot.configReverseSoftLimitThreshold(ArmConstants.PIVOT_REVERSE_SOFT_LIMIT);
+    pivot.configForwardSoftLimitEnable(false);
+    pivot.configReverseSoftLimitEnable(false);
+
+    tele.configForwardSoftLimitThreshold(ArmConstants.TELE_FORWARD_SOFT_LIMIT);
+    tele.configReverseSoftLimitThreshold(ArmConstants.TELE_REVERSE_SOFT_LIMIT);
+    tele.configForwardSoftLimitEnable(false);
+    tele.configReverseSoftLimitEnable(false);
+  }
 }

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

@@ -1,39 +1,118 @@
 package frc4388.robot.subsystems;
 
 import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.FeedbackDevice;
+import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
+import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
+import com.ctre.phoenix.sensors.CANCoder;
+import com.ctre.phoenix.sensors.CANCoderConfiguration;
 import frc4388.robot.Constants.ArmConstants;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class Arm extends SubsystemBase {
-    private WPI_TalonFX m_armmotor;
+    private WPI_TalonFX m_tele;
-    private boolean m_debug;
+    private WPI_TalonFX m_pivot;
-    // Moves arm to distence [dist] then returns new ang
+    private CANCoder    m_pivotEncoder;
-    public Arm(WPI_TalonFX armmotor, boolean debug) {
+    private boolean     m_debug;
-        armmotor.configFactoryDefault();
+
-        m_armmotor = armmotor;
+    // Moves arm to distance [dist] then returns new ang
+    public Arm(WPI_TalonFX pivot, WPI_TalonFX tele, CANCoder encoder, boolean debug) {
+        m_tele         = tele;
+        m_pivot        = pivot;
+        m_pivotEncoder = encoder;
+
+        TalonFXConfiguration pivotConfig = new TalonFXConfiguration();
+        pivotConfig.slot0.kP = ArmConstants.kP;
+        pivotConfig.slot0.kI = ArmConstants.kI;
+        pivotConfig.slot0.kD = ArmConstants.kD;
+
+        pivotConfig.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID();
+        pivotConfig.remoteFilter0.remoteSensorSource   = RemoteSensorSource.CANCoder;
+        pivotConfig.primaryPID.selectedFeedbackSensor  = FeedbackDevice.RemoteSensor0;
+        pivot.configAllSettings(pivotConfig);
+
+        CANCoderConfiguration config = new CANCoderConfiguration();
+        config.magnetOffsetDegrees = ArmConstants.OFFSET;
+        m_pivotEncoder.configAllSettings(config);
+        
+        tele.configFactoryDefault();
+        pivot.configFactoryDefault();
     }
 
-    public Arm(WPI_TalonFX armmoter) {
+    public Arm(WPI_TalonFX pivot, WPI_TalonFX tele, CANCoder encoder) {
-        this(armmoter, false);
+        this(pivot, tele, encoder, false);
+    }
+
+    public void setRotVel(double vel) {
+        m_pivot.set(ControlMode.Velocity, vel);
+    }
+
+    public void setTeleVel(double vel) {
+        m_tele.set(ControlMode.Velocity, vel);
+    }
+
+    public void armSetRotation(double rot) {
+        if (rot > 1 || rot < 0) return;
+        // Move arm code
+        m_pivot.set(ControlMode.Position, rot * Math.abs(ArmConstants.PIVOT_REVERSE_SOFT_LIMIT - ArmConstants.PIVOT_FORWARD_SOFT_LIMIT) +
+            ArmConstants.PIVOT_FORWARD_SOFT_LIMIT);
     }
 
     public void armSetLength(double len) {
         if (len > 1 || len < 0) return;
         // Move arm code
-        m_armmotor.set(ControlMode.Position, len * (ArmConstants.MAX_ARM_LEN - ArmConstants.MIN_ARM_LEN) +
+        m_tele.set(ControlMode.Position, len * Math.abs(ArmConstants.TELE_REVERSE_SOFT_LIMIT - ArmConstants.TELE_FORWARD_SOFT_LIMIT) +
-            ArmConstants.MIN_ARM_LEN);
+            ArmConstants.TELE_FORWARD_SOFT_LIMIT);
+
+        if (m_tele.isRevLimitSwitchClosed() == 1) {
+            m_tele.setSelectedSensorPosition(ArmConstants.TELE_REVERSE_SOFT_LIMIT);
+        } else if (m_tele.isFwdLimitSwitchClosed() == 1) {
+            m_tele.setSelectedSensorPosition(ArmConstants.TELE_FORWARD_SOFT_LIMIT);
+        }
     }
 
     public double getArmLength() {
-        return (m_armmotor.getSelectedSensorPosition() - ArmConstants.MIN_ARM_LEN) /
+        return (m_tele.getSelectedSensorPosition() - ArmConstants.TELE_FORWARD_SOFT_LIMIT) /
-            (ArmConstants.MAX_ARM_LEN - ArmConstants.MIN_ARM_LEN);
+            (ArmConstants.TELE_REVERSE_SOFT_LIMIT - ArmConstants.TELE_FORWARD_SOFT_LIMIT);
+    }
+
+    public double getArmRotation() {
+        return (m_pivotEncoder.getAbsolutePosition() - ArmConstants.PIVOT_FORWARD_SOFT_LIMIT) /
+            (ArmConstants.PIVOT_REVERSE_SOFT_LIMIT - ArmConstants.PIVOT_FORWARD_SOFT_LIMIT);
+    }
+
+    public void runPivotTele(double pivot, double tele) {
+        var rot = 0;
+
+        if (checkLimits(tele, rot)) {
+            armSetRotation(pivot);
+            armSetLength(tele);
+        }
+    }
+
+    /**
+     * Checks that an input is within bounds
+     * @param _len length from 0 to 1
+     * @param _theta radians from the zero (straight up)
+     * @return
+     */
+    public static boolean checkLimits(double _len, double _theta) {
+        var len = _len * (ArmConstants.MAX_ARM_LEN - ArmConstants.MIN_ARM_LEN) + ArmConstants.MIN_ARM_LEN;
+        var x   = len * Math.cos(_theta);
+        var y   = ArmConstants.ARM_HEIGHT + len * Math.sin(_theta);
+
+        var minHeight = Math.pow(ArmConstants.CURVE_POWER, Math.abs(x));
+        if (y < minHeight)
+            return false;
+        
+        return true;
     }
 
     @Override
     public void periodic() {
         if (m_debug)
-            SmartDashboard.putNumber("Arm Motor", m_armmotor.getSelectedSensorPosition());
+            SmartDashboard.putNumber("Arm Motor", m_tele.getSelectedSensorPosition());
     }
 }