From 0d3457c19dd1b0677e3282e42787baadf22dc877 Mon Sep 17 00:00:00 2001 From: IsaacThoman Date: Mon, 25 Sep 2023 13:53:09 -0400 Subject: [PATCH 1/4] start of stage one --- src/main/java/frc/robot/Constants.java | 42 +++++++ src/main/java/frc/robot/RobotContainer.java | 3 + .../java/frc/robot/subsystems/arm/Arm.java | 20 +++ .../frc/robot/subsystems/arm/StageOneSub.java | 114 ++++++++++++++++++ 4 files changed, 179 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/arm/Arm.java create mode 100644 src/main/java/frc/robot/subsystems/arm/StageOneSub.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b7cf0df..8f73248 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -98,9 +98,51 @@ public static final class DriveConstants{ public static final double turningMotorkF = 0.0; + } + + + public static class ArmConstants { + + public static final double stageOnekP = 0.1; + public static final double stageOnekI = 0.0; + public static final double stageOnekD = 0.0; + public static final double stageOnekF = 0.0; + + public static final double stageTwokP = 0.1; + public static final double stageTwokI = 0.0; + public static final double stageTwokD = 0.0; + public static final double stageTwokF = 0.0; + + + + public static final int stageOneLeftId = 11; + public static final int stageOneRightId = 13; + public static final double continuousCurrentLimit = 20; + public static final double peakCurrentLimit = 40; + public static final double peakCurrentTime = 250; + public static final int stageTwoLeftId = 49; + public static final int stageTwoRightId = 48; + public static final int stageTwoSmartCurrentLimit = 40; + public static final double stageTwoSecondaryCurrentLimit = 60; + + public static final double magEncoderCountsPerRotation = 4096;//4096 + public static final double radiansPerRotation = 2 * Math.PI; + + public static final double stageOneEncoderTicksToRadians = (radiansPerRotation/magEncoderCountsPerRotation); + + public static final double stageOneEncoderOffset = Units.degreesToRadians(291.9 + 90 - .145); + + public static final double stageOneLength = 28.75; + public static final double[] stageOnePivotCoordinate = {-4.864, 18.66}; + + public static final double stageTwoLength = 28.75; + public static final double stageTwoEncoderOffset = Units.degreesToRadians(43.6);//180 - 43.6 //43.6 + 180 + public static final double stageTwoEncoderRatio = 1;//32.0/22 } + + } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index edf26a8..769f6a6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,6 +13,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.subsystems.arm.Arm; import frc.robot.subsystems.drive.SwerveSubsystem; import frc.robot.subsystems.drive.VisionSubsystem; @@ -31,6 +32,8 @@ public class RobotContainer { SwerveSubsystem swerveSubsystem = new SwerveSubsystem(visionSub); RunCommand drive = new RunCommand(()->swerveSubsystem.joystickDrive(driver.getLeftX(),driver.getLeftY(),driver.getRightX()),swerveSubsystem); + Arm armSubsystemn = new Arm(); + /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java new file mode 100644 index 0000000..c845bfe --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -0,0 +1,20 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.arm; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Arm extends SubsystemBase { + /** Creates a new Arm. */ + StageOneSub stageOneSub = new StageOneSub(); + public Arm() { + + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} diff --git a/src/main/java/frc/robot/subsystems/arm/StageOneSub.java b/src/main/java/frc/robot/subsystems/arm/StageOneSub.java new file mode 100644 index 0000000..4bc988d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/StageOneSub.java @@ -0,0 +1,114 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.arm; + +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.InvertType; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import static frc.robot.Constants.ArmConstants.*; + +public class StageOneSub extends SubsystemBase { + /** Creates a new StageOneSub. */ + + TalonSRX primary; + TalonSRX secondary; + + + public StageOneSub() { + primary = new TalonSRX(stageOneRightId); + secondary = new TalonSRX(stageOneLeftId); + + primary.configFactoryDefault(1000); + secondary.configFactoryDefault(1000); + + primary.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Absolute,0,1000); + primary.configFeedbackNotContinuous(true,1000); + + primary.setNeutralMode(NeutralMode.Coast); + secondary.setNeutralMode(NeutralMode.Coast); + + + primary.configVoltageCompSaturation(12.0,1000); + primary.enableVoltageCompensation(true); + secondary.configVoltageCompSaturation(12.0,1000); + secondary.enableVoltageCompensation(true); + + primary.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true,continuousCurrentLimit,peakCurrentLimit,peakCurrentTime),1000); + secondary.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true,continuousCurrentLimit,peakCurrentLimit,peakCurrentTime),1000); + + + primary.setSensorPhase(false); + primary.setInverted(true); + + secondary.follow(primary); + secondary.setInverted(InvertType.OpposeMaster); + + primary.configForwardSoftLimitThreshold((int)radToNativeSensorPosition(Units.degreesToRadians(80)),1000); + primary.configReverseSoftLimitThreshold((int)radToNativeSensorPosition(Units.degreesToRadians(45)),1000); + primary.configForwardSoftLimitEnable(true,1000); + + + + // primary.config_kP(0,stageOnekP,1000); + // primary.config_kI(0,stageOnekI,1000); + // primary.config_kD(0,stageOnekD,1000); + // primary.config_kF(0,stageOnekF,1000); + + + + + + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + SmartDashboard.putNumber("stageOneAngle",getAngle().getDegrees()); + SmartDashboard.putNumber("rawSensor",primary.getSelectedSensorPosition()); + } + + public Rotation2d getAngle(){ //gets angle from horizontal + + double angleRad = nativeSensorPositionToRad(primary.getSelectedSensorPosition()); + + return Rotation2d.fromRadians(angleRad); + } + + public double nativeSensorPositionToRad(double sensorReading){ + sensorReading *= stageOneEncoderTicksToRadians; + + sensorReading = (2*Math.PI) + sensorReading; + + if(sensorReading>Math.PI) + sensorReading = sensorReading - 2*Math.PI; + + sensorReading+= Units.degreesToRadians(22.5); + return sensorReading; + } + + public double radToNativeSensorPosition(double angleRad) { + + angleRad -= Units.degreesToRadians(22.5); + + + if (angleRad < -Math.PI) + angleRad += 2 * Math.PI; + + angleRad -= 2 * Math.PI; + + angleRad /= stageOneEncoderTicksToRadians; + + return angleRad; + } + + +} From cd57d0aa24f06fb696b5c4b10d4095ee4412f556 Mon Sep 17 00:00:00 2001 From: IsaacThoman Date: Mon, 25 Sep 2023 14:22:42 -0400 Subject: [PATCH 2/4] soft limits --- src/main/java/frc/robot/RobotContainer.java | 5 ++++- src/main/java/frc/robot/subsystems/arm/Arm.java | 6 ++++-- .../java/frc/robot/subsystems/arm/StageOneSub.java | 12 ++++++++---- .../frc/robot/subsystems/drive/VisionSubsystem.java | 2 +- 4 files changed, 17 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 769f6a6..56d72aa 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.subsystems.arm.Arm; +import frc.robot.subsystems.arm.StageOneSub; import frc.robot.subsystems.drive.SwerveSubsystem; import frc.robot.subsystems.drive.VisionSubsystem; @@ -32,13 +33,15 @@ public class RobotContainer { SwerveSubsystem swerveSubsystem = new SwerveSubsystem(visionSub); RunCommand drive = new RunCommand(()->swerveSubsystem.joystickDrive(driver.getLeftX(),driver.getLeftY(),driver.getRightX()),swerveSubsystem); - Arm armSubsystemn = new Arm(); + StageOneSub stageOneSub = new StageOneSub(); /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { swerveSubsystem.setDefaultCommand(drive); + // stageOneSub.setDefaultCommand(new RunCommand(()->stageOneSub.setPercentOutput(driver.getLeftTriggerAxis() - driver.getRightTriggerAxis()),stageOneSub)); + // Configure the trigger bindings configureBindings(); diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index c845bfe..1087e88 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -4,15 +4,17 @@ package frc.robot.subsystems.arm; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Arm extends SubsystemBase { /** Creates a new Arm. */ - StageOneSub stageOneSub = new StageOneSub(); - public Arm() { + // StageOneSub stageOneSub = new StageOneSub(); + public Arm() { } + @Override public void periodic() { // This method will be called once per scheduler run diff --git a/src/main/java/frc/robot/subsystems/arm/StageOneSub.java b/src/main/java/frc/robot/subsystems/arm/StageOneSub.java index 4bc988d..df2aa32 100644 --- a/src/main/java/frc/robot/subsystems/arm/StageOneSub.java +++ b/src/main/java/frc/robot/subsystems/arm/StageOneSub.java @@ -4,10 +4,7 @@ package frc.robot.subsystems.arm; -import com.ctre.phoenix.motorcontrol.FeedbackDevice; -import com.ctre.phoenix.motorcontrol.InvertType; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; +import com.ctre.phoenix.motorcontrol.*; import com.ctre.phoenix.motorcontrol.can.TalonSRX; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; @@ -55,6 +52,7 @@ public StageOneSub() { primary.configForwardSoftLimitThreshold((int)radToNativeSensorPosition(Units.degreesToRadians(80)),1000); primary.configReverseSoftLimitThreshold((int)radToNativeSensorPosition(Units.degreesToRadians(45)),1000); primary.configForwardSoftLimitEnable(true,1000); + primary.configReverseSoftLimitEnable(true); @@ -76,6 +74,12 @@ public void periodic() { SmartDashboard.putNumber("rawSensor",primary.getSelectedSensorPosition()); } + public void setPercentOutput(double output){ + if(output>0.5)output = 0.5; + if(output<-0.5)output = -0.5; + primary.set(ControlMode.PercentOutput,output); + } + public Rotation2d getAngle(){ //gets angle from horizontal double angleRad = nativeSensorPositionToRad(primary.getSelectedSensorPosition()); diff --git a/src/main/java/frc/robot/subsystems/drive/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/drive/VisionSubsystem.java index ca7e939..4c7ce8a 100644 --- a/src/main/java/frc/robot/subsystems/drive/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/VisionSubsystem.java @@ -36,7 +36,7 @@ public VisionSubsystem() { e.printStackTrace(); } - cam = new PhotonCamera("arducam1"); + cam = new PhotonCamera("Arducam_OV2311_USB_Camera"); Transform3d robotToCam = new Transform3d(new Translation3d(Units.inchesToMeters(-9.5), Units.inchesToMeters(6.5), Units.inchesToMeters(26)), new Rotation3d(0,0,0)); //Cam mounted facing forward, half a meter forward of center, half a meter up from center. photonPoseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP, cam, robotToCam); From 2ad897dfa27e79a7ed89dc3e369e9d3191219871 Mon Sep 17 00:00:00 2001 From: IsaacThoman Date: Wed, 27 Sep 2023 12:42:57 -0400 Subject: [PATCH 3/4] make it continuous --- src/main/java/frc/robot/RobotContainer.java | 6 ++++-- .../frc/robot/subsystems/arm/StageOneSub.java | 16 ++++++++++------ 2 files changed, 14 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 56d72aa..f51595a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -39,9 +39,11 @@ public class RobotContainer { /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - swerveSubsystem.setDefaultCommand(drive); - // stageOneSub.setDefaultCommand(new RunCommand(()->stageOneSub.setPercentOutput(driver.getLeftTriggerAxis() - driver.getRightTriggerAxis()),stageOneSub)); + // swerveSubsystem.setDefaultCommand(drive); + RunCommand sendArmVoltage = new RunCommand(()-> stageOneSub.setPercentOutput(driver.getLeftTriggerAxis()-driver.getRightTriggerAxis()),stageOneSub); + + stageOneSub.setDefaultCommand(sendArmVoltage); // Configure the trigger bindings configureBindings(); diff --git a/src/main/java/frc/robot/subsystems/arm/StageOneSub.java b/src/main/java/frc/robot/subsystems/arm/StageOneSub.java index df2aa32..0b16176 100644 --- a/src/main/java/frc/robot/subsystems/arm/StageOneSub.java +++ b/src/main/java/frc/robot/subsystems/arm/StageOneSub.java @@ -28,7 +28,7 @@ public StageOneSub() { secondary.configFactoryDefault(1000); primary.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Absolute,0,1000); - primary.configFeedbackNotContinuous(true,1000); + primary.configFeedbackNotContinuous(false,1000); primary.setNeutralMode(NeutralMode.Coast); secondary.setNeutralMode(NeutralMode.Coast); @@ -49,8 +49,8 @@ public StageOneSub() { secondary.follow(primary); secondary.setInverted(InvertType.OpposeMaster); - primary.configForwardSoftLimitThreshold((int)radToNativeSensorPosition(Units.degreesToRadians(80)),1000); - primary.configReverseSoftLimitThreshold((int)radToNativeSensorPosition(Units.degreesToRadians(45)),1000); + primary.configForwardSoftLimitThreshold((int)radToNativeSensorPosition(Units.degreesToRadians(90)),1000); + primary.configReverseSoftLimitThreshold((int)radToNativeSensorPosition(Units.degreesToRadians(35)),1000); primary.configForwardSoftLimitEnable(true,1000); primary.configReverseSoftLimitEnable(true); @@ -71,12 +71,15 @@ public StageOneSub() { public void periodic() { // This method will be called once per scheduler run SmartDashboard.putNumber("stageOneAngle",getAngle().getDegrees()); + SmartDashboard.putNumber("rawSensor",primary.getSelectedSensorPosition()); + SmartDashboard.putNumber("convertedSensor", radToNativeSensorPosition(nativeSensorPositionToRad(primary.getSelectedSensorPosition()))); + } public void setPercentOutput(double output){ - if(output>0.5)output = 0.5; - if(output<-0.5)output = -0.5; + // if(output>0.5)output = 0.5; + // if(output<-0.5)output = -0.5; primary.set(ControlMode.PercentOutput,output); } @@ -95,7 +98,7 @@ public double nativeSensorPositionToRad(double sensorReading){ if(sensorReading>Math.PI) sensorReading = sensorReading - 2*Math.PI; - sensorReading+= Units.degreesToRadians(22.5); + sensorReading+= Units.degreesToRadians(22.5); return sensorReading; } @@ -111,6 +114,7 @@ public double radToNativeSensorPosition(double angleRad) { angleRad /= stageOneEncoderTicksToRadians; + return angleRad; } From c0cd8ca8a16f011f1f88e81b737a3163a3b43656 Mon Sep 17 00:00:00 2001 From: Isaac Thoman Date: Wed, 4 Oct 2023 07:48:49 -0400 Subject: [PATCH 4/4] arm tuned mostly, goToPosition --- src/main/java/frc/robot/Constants.java | 31 ++-- src/main/java/frc/robot/RobotContainer.java | 26 +++- .../java/frc/robot/subsystems/arm/Arm.java | 61 ++++++++ .../frc/robot/subsystems/arm/StageOneSub.java | 30 +++- .../frc/robot/subsystems/arm/StageTwoSub.java | 136 ++++++++++++++++++ .../robot/subsystems/drive/SwerveModule.java | 4 + .../subsystems/drive/SwerveSubsystem.java | 43 +++++- 7 files changed, 308 insertions(+), 23 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/arm/StageTwoSub.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8f73248..decdc5c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -17,13 +17,24 @@ * constants are needed, to reduce verbosity. */ public final class Constants { + + public static class AutoConstants { + public static final double translationkP = 4; + public static final double rotationkP = 4; + + public static final double maxTranslation = 1; + public static final double maxRotation = 1; + + } public static class OperatorConstants { public static final int kDriverControllerPort = 0; public static final double driveExponent = 1.8; - public static final double driveMaxSpeed = 0.5; //5 + public static final double driveMaxSpeed = 5; //5 public static final double turnExponent = 1.8; - public static final double turnMaxSpeed = 1; //11 + public static final double turnMaxSpeed = 11; //11 + + public static final double maxDrivePower = 1; public static final double radFeedClamp = 0.5; //max heading adjustment speed @@ -103,14 +114,14 @@ public static final class DriveConstants{ public static class ArmConstants { - public static final double stageOnekP = 0.1; + public static final double stageOnekP = 8; public static final double stageOnekI = 0.0; public static final double stageOnekD = 0.0; public static final double stageOnekF = 0.0; - public static final double stageTwokP = 0.1; - public static final double stageTwokI = 0.0; - public static final double stageTwokD = 0.0; + public static final double stageTwokP = 0.3; + public static final double stageTwokI = 0.001; + public static final double stageTwokD = 2; public static final double stageTwokF = 0.0; @@ -122,8 +133,8 @@ public static class ArmConstants { public static final double peakCurrentLimit = 40; public static final double peakCurrentTime = 250; - public static final int stageTwoLeftId = 49; - public static final int stageTwoRightId = 48; + public static final int stageTwoPrimaryId = 49; + public static final int stageTwoSecondaryId = 48; public static final int stageTwoSmartCurrentLimit = 40; public static final double stageTwoSecondaryCurrentLimit = 60; @@ -135,10 +146,10 @@ public static class ArmConstants { public static final double stageOneEncoderOffset = Units.degreesToRadians(291.9 + 90 - .145); - public static final double stageOneLength = 28.75; + public static final double stageOneLength = Units.inchesToMeters(28.75); public static final double[] stageOnePivotCoordinate = {-4.864, 18.66}; - public static final double stageTwoLength = 28.75; + public static final double stageTwoLength = Units.inchesToMeters(28.75); public static final double stageTwoEncoderOffset = Units.degreesToRadians(43.6);//180 - 43.6 //43.6 + 180 public static final double stageTwoEncoderRatio = 1;//32.0/22 } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f51595a..7a94d64 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,7 +4,10 @@ package frc.robot; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import frc.robot.Constants.OperatorConstants; import frc.robot.commands.Autos; @@ -15,6 +18,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.subsystems.arm.Arm; import frc.robot.subsystems.arm.StageOneSub; +import frc.robot.subsystems.arm.StageTwoSub; import frc.robot.subsystems.drive.SwerveSubsystem; import frc.robot.subsystems.drive.VisionSubsystem; @@ -27,23 +31,35 @@ public class RobotContainer { // The robot's subsystems and commands are defined here... - XboxController driver = new XboxController(0); + CommandXboxController driver = new CommandXboxController(0); VisionSubsystem visionSub = new VisionSubsystem(); SwerveSubsystem swerveSubsystem = new SwerveSubsystem(visionSub); RunCommand drive = new RunCommand(()->swerveSubsystem.joystickDrive(driver.getLeftX(),driver.getLeftY(),driver.getRightX()),swerveSubsystem); - StageOneSub stageOneSub = new StageOneSub(); + + + + Arm arm = new Arm(); /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - // swerveSubsystem.setDefaultCommand(drive); + swerveSubsystem.setDefaultCommand(drive); + + driver.a().whileTrue(new RunCommand(()->swerveSubsystem.driveToPose(new Pose2d()),swerveSubsystem)); + + driver.b().onTrue(new InstantCommand(()->swerveSubsystem.resetOdometry())); + +// RunCommand sendArmVoltage = new RunCommand(()-> stageOneSub.setPercentOutput(driver.getLeftTriggerAxis()-driver.getRightTriggerAxis()),stageOneSub); + + // RunCommand setArmPosition = new RunCommand(()-> stageOneSub.setAngle(Rotation2d.fromDegrees(45)),stageOneSub); + - RunCommand sendArmVoltage = new RunCommand(()-> stageOneSub.setPercentOutput(driver.getLeftTriggerAxis()-driver.getRightTriggerAxis()),stageOneSub); + //driver.a().whileTrue(setArmPosition); - stageOneSub.setDefaultCommand(sendArmVoltage); + // stageOneSub.setDefaultCommand(sendArmVoltage); // Configure the trigger bindings configureBindings(); diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index 1087e88..066bc61 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -4,19 +4,80 @@ package frc.robot.subsystems.arm; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import static frc.robot.Constants.ArmConstants.*; +import frc.robot.Constants; + +import java.awt.geom.Point2D; public class Arm extends SubsystemBase { /** Creates a new Arm. */ // StageOneSub stageOneSub = new StageOneSub(); + StageOneSub stageOneSub = new StageOneSub(); + StageTwoSub stageTwoSub = new StageTwoSub(); + + public Arm() { + +// SmartDashboard.putNumber("stageOneRef",90); +// SmartDashboard.putNumber("stageTwoRef",15); + } + + + + public static double[] calculateEFPosition(double ang1, double ang2) { + double stageOneEndX = stageOneLength * Math.cos(ang1); + double stageOneEndY = stageOneLength * Math.sin(ang1); + + double stageTwoEndX = stageOneEndX + stageTwoLength * -Math.cos(ang1 + ang2); + double stageTwoEndY = stageOneEndY + stageTwoLength * -Math.sin(ang1 + ang2); + + return new double[]{stageTwoEndX, stageTwoEndY}; } + public static Rotation2d[] calculateArmAngles(double x, double y){ + double hypot = Math.hypot(x,y); + double stageTwoAngle = Math.acos((Math.pow(stageTwoLength,2) + Math.pow(stageOneLength,2) - Math.pow(hypot,2)) / (2*stageOneLength * stageTwoLength) ); + double stageOneAngle = Math.acos((Math.pow(stageOneLength,2) + Math.pow(hypot,2) - Math.pow(stageTwoLength,2)) / (2*stageOneLength * hypot)); + + stageOneAngle+=Math.atan2(y,x); + + return new Rotation2d[] {Rotation2d.fromRadians(stageOneAngle),Rotation2d.fromRadians(stageTwoAngle)}; + } + @Override public void periodic() { // This method will be called once per scheduler run + + double[] efPosition = calculateEFPosition(stageOneSub.getAngle().getRadians(), stageTwoSub.getAngle().getRadians()); + + SmartDashboard.putNumber("stageOneAngle",stageOneSub.getAngle().getDegrees()); + SmartDashboard.putNumber("stageTwoAngle",stageTwoSub.getAngle().getDegrees()); + + + SmartDashboard.putNumber("ef_x",efPosition[0]); + SmartDashboard.putNumber("ef_y",efPosition[1]); + + + Rotation2d[] calculatedArmAngles = calculateArmAngles(efPosition[0],efPosition[1]); + + SmartDashboard.putNumber("stageOneRad_calculated",calculatedArmAngles[0].getDegrees()); + SmartDashboard.putNumber("stageTwoRad_calculated",calculatedArmAngles[1].getDegrees()); + + + + + // stageOneSub.setAngle(Rotation2d.fromDegrees(SmartDashboard.getNumber("stageOneRef",70))); + // stageTwoSub.setAngle(Rotation2d.fromDegrees(SmartDashboard.getNumber("stageTwoRef",15))); + + stageOneSub.setAngle(Rotation2d.fromDegrees(90)); + stageTwoSub.setAngle(Rotation2d.fromDegrees(15)); + + } } diff --git a/src/main/java/frc/robot/subsystems/arm/StageOneSub.java b/src/main/java/frc/robot/subsystems/arm/StageOneSub.java index 0b16176..2642b88 100644 --- a/src/main/java/frc/robot/subsystems/arm/StageOneSub.java +++ b/src/main/java/frc/robot/subsystems/arm/StageOneSub.java @@ -49,16 +49,23 @@ public StageOneSub() { secondary.follow(primary); secondary.setInverted(InvertType.OpposeMaster); - primary.configForwardSoftLimitThreshold((int)radToNativeSensorPosition(Units.degreesToRadians(90)),1000); + primary.configForwardSoftLimitThreshold((int)radToNativeSensorPosition(Units.degreesToRadians(95)),1000); primary.configReverseSoftLimitThreshold((int)radToNativeSensorPosition(Units.degreesToRadians(35)),1000); primary.configForwardSoftLimitEnable(true,1000); primary.configReverseSoftLimitEnable(true); + // primary.configNominalOutputForward(0.05,1000); + // primary.configNominalOutputReverse(-0.05,1000); - // primary.config_kP(0,stageOnekP,1000); - // primary.config_kI(0,stageOnekI,1000); - // primary.config_kD(0,stageOnekD,1000); + primary.configPeakOutputForward(0.7,1000); + primary.configPeakOutputReverse(-0.7,1000); + + + + primary.config_kP(0,stageOnekP,1000); + primary.config_kI(0,stageOnekI,1000); + primary.config_kD(0,stageOnekD,1000); // primary.config_kF(0,stageOnekF,1000); @@ -70,10 +77,14 @@ public StageOneSub() { @Override public void periodic() { // This method will be called once per scheduler run - SmartDashboard.putNumber("stageOneAngle",getAngle().getDegrees()); + // SmartDashboard.putNumber("stageOneAngle",getAngle().getDegrees()); + + // SmartDashboard.putNumber("rawSensor",primary.getSelectedSensorPosition()); + // SmartDashboard.putNumber("convertedSensor", radToNativeSensorPosition(nativeSensorPositionToRad(primary.getSelectedSensorPosition()))); + + // SmartDashboard.putNumber("stageOneOutput",primary.getMotorOutputPercent()); + - SmartDashboard.putNumber("rawSensor",primary.getSelectedSensorPosition()); - SmartDashboard.putNumber("convertedSensor", radToNativeSensorPosition(nativeSensorPositionToRad(primary.getSelectedSensorPosition()))); } @@ -83,6 +94,10 @@ public void setPercentOutput(double output){ primary.set(ControlMode.PercentOutput,output); } + public void setAngle(Rotation2d angle) { + primary.set(ControlMode.Position,radToNativeSensorPosition(angle.getRadians())); + } + public Rotation2d getAngle(){ //gets angle from horizontal double angleRad = nativeSensorPositionToRad(primary.getSelectedSensorPosition()); @@ -119,4 +134,5 @@ public double radToNativeSensorPosition(double angleRad) { } + } diff --git a/src/main/java/frc/robot/subsystems/arm/StageTwoSub.java b/src/main/java/frc/robot/subsystems/arm/StageTwoSub.java new file mode 100644 index 0000000..3bb82c9 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/StageTwoSub.java @@ -0,0 +1,136 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.arm; + +import com.revrobotics.*; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import java.time.Clock; +import java.util.ArrayList; + +import static frc.robot.Constants.ArmConstants.*; + +public class StageTwoSub extends SubsystemBase { + /** Creates a new StageTwoSub. */ + + + CANSparkMax primary; + CANSparkMax secondary; + SparkMaxPIDController pidController; + + SparkMaxAbsoluteEncoder encoder; + + public StageTwoSub() { + primary = new CANSparkMax(stageTwoPrimaryId, CANSparkMaxLowLevel.MotorType.kBrushless); + secondary = new CANSparkMax(stageTwoSecondaryId, CANSparkMaxLowLevel.MotorType.kBrushless); + + // ArrayList primaryErrors = new ArrayList<>(); + + primary.restoreFactoryDefaults(); + secondary.restoreFactoryDefaults(); + + primary.setCANTimeout(1000); + secondary.setCANTimeout(1000); + + primary.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus0, 10); + secondary.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus0, 10); + + primary.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus1, 100); + secondary.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus1, 100); + + primary.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus2, 100); + secondary.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus2, 100); + + primary.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus3, 100); + secondary.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus3, 100); + + primary.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus4, 100); + secondary.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus4, 100); + + primary.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus5, 100); + secondary.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus5, 100); + + primary.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus6, 100); + secondary.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus6, 100); + + encoder = primary.getAbsoluteEncoder(SparkMaxAbsoluteEncoder.Type.kDutyCycle); + pidController = primary.getPIDController(); + + primary.enableVoltageCompensation(12.0); + secondary.enableVoltageCompensation(12.0); + + primary.setSecondaryCurrentLimit(stageTwoSecondaryCurrentLimit); + secondary.setSecondaryCurrentLimit(stageTwoSecondaryCurrentLimit); + primary.setSmartCurrentLimit(stageTwoSmartCurrentLimit); + secondary.setSmartCurrentLimit(stageTwoSmartCurrentLimit); + + + + secondary.follow(primary, true); + + encoder.setPositionConversionFactor(2*Math.PI); + encoder.setVelocityConversionFactor(2*Math.PI); + // encoder.setZeroOffset(Units.radiansToDegrees(267.3+90))); + encoder.setInverted(true); + + primary.setSoftLimit(CANSparkMax.SoftLimitDirection.kReverse,(float)Units.degreesToRadians(10)); + primary.setSoftLimit(CANSparkMax.SoftLimitDirection.kForward,(float)Units.degreesToRadians(180)); + primary.enableSoftLimit(CANSparkMax.SoftLimitDirection.kForward, true); + primary.enableSoftLimit(CANSparkMax.SoftLimitDirection.kReverse, true); + + + + pidController.setFeedbackDevice(encoder); + pidController.setP(stageTwokP,0); + pidController.setI(stageTwokI,0); + pidController.setD(stageTwokD,0); + pidController.setOutputRange(-0.6,0.6); + pidController.setPositionPIDWrappingEnabled(false); + + pidController.setIZone(0.3,0); + pidController.setIMaxAccum(0.05,0); + + + + primary.burnFlash(); + secondary.burnFlash(); + + + + + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + // pidController.setReference(Units.degreesToRadians(35), CANSparkMax.ControlType.kPosition); + + // SmartDashboard.putNumber("stageTwoEncoder",getAngle().getDegrees()); + double error = referenceRad - getAngle().getRadians(); + SmartDashboard.putNumber("stageTwoError", error); + SmartDashboard.putNumber("stageTwoIAccum",pidController.getIAccum()); + + // if(error>0.25){ + // pidController.setIAccum(0); + // } + + } + + double referenceRad = 0; + public void setAngle(Rotation2d angle){ + referenceRad = angle.getRadians(); + + pidController.setReference(angle.getRadians(), CANSparkMax.ControlType.kPosition,0,0); + + } + + + public Rotation2d getAngle(){ + return Rotation2d.fromRadians(encoder.getPosition()); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveModule.java b/src/main/java/frc/robot/subsystems/drive/SwerveModule.java index b96890d..c1caccc 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveModule.java @@ -12,6 +12,7 @@ import frc.robot.Constants; import static frc.robot.Constants.DriveConstants.*; +import static frc.robot.Constants.OperatorConstants.maxDrivePower; public class SwerveModule extends SubsystemBase { @@ -61,6 +62,9 @@ public SwerveModule(int driveMotorID, int turningMotorID, boolean driveMotorReve driveMotor.setNeutralMode(NeutralMode.Coast); driveMotor.setInverted(driveMotorReversed); + driveMotor.configPeakOutputForward(maxDrivePower,1000); + driveMotor.configPeakOutputReverse(-maxDrivePower,1000); + /*Turning Motor Configs */ turningMotor.config_kP(0,turningMotorkP); turningMotor.config_kI(0,turningMotorkI); diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/SwerveSubsystem.java index 20d06a2..482b443 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveSubsystem.java @@ -26,6 +26,7 @@ import java.util.Optional; +import static frc.robot.Constants.AutoConstants.*; import static frc.robot.Constants.DriveConstants.*; import static frc.robot.Constants.OperatorConstants.*; @@ -142,8 +143,48 @@ public void periodic() { field2d.setRobotPose(odometry.getEstimatedPosition()); - SmartDashboard.putData("field",field2d); + //SmartDashboard.putData("field",field2d); + SmartDashboard.putNumber("odom_x",odometry.getEstimatedPosition().getX()); + SmartDashboard.putNumber("odom_y",odometry.getEstimatedPosition().getY()); + SmartDashboard.putNumber("odom_deg",odometry.getEstimatedPosition().getRotation().getDegrees()); + + + } + + public void resetOdometry(){ + odometry.resetPosition(pigeon.getRotation2d(),getPositions(),new Pose2d()); + } + + public void driveToPose(Pose2d desiredPose){ + Pose2d myPose = odometry.getEstimatedPosition(); + + double xDiff = desiredPose.getX() - myPose.getX(); + double yDiff = desiredPose.getY() - myPose.getY(); + + double angDiff = desiredPose.getRotation().getRadians() - myPose.getRotation().getRadians(); //difference in angles + + + double hypot = Math.hypot(xDiff,yDiff); + double angleOfDiff = Math.atan2(yDiff,xDiff); //angle between two poses + + double angleOfTravel = angleOfDiff + angDiff; + + + + hypot*= translationkP; + angDiff*=rotationkP; + + if(hypot > maxTranslation) + hypot = maxTranslation; + + if(angDiff>maxRotation) + angDiff = maxRotation; + + + + + drive( hypot*Math.cos(angleOfTravel), hypot * Math.sin(angleOfTravel), angDiff); }