From be979b943227fa88c753292c441191527504b8f4 Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Tue, 6 Feb 2024 19:24:32 -0700 Subject: [PATCH] created arm PID, changed IDs, fixed swerve (im tweakin) (pushkar has a mental disability) --- src/main/java/frc4388/robot/Constants.java | 4 +- .../java/frc4388/robot/RobotContainer.java | 12 ++++-- .../Intake/RotateIntakeToPosition.java | 38 +++++++++++++++++++ .../java/frc4388/robot/subsystems/Intake.java | 12 ++++-- .../frc4388/robot/subsystems/SwerveDrive.java | 4 +- 5 files changed, 59 insertions(+), 11 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/Intake/RotateIntakeToPosition.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 17f51fd..3dc5352 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -159,8 +159,8 @@ public final class Constants { } public static final class IntakeConstants { - public static final int INTAKE_MOTOR_ID = 17; //TODO: - public static final int PIVOT_MOTOR_ID = 18; //TODO: + public static final int INTAKE_MOTOR_ID = 18; //TODO: + public static final int PIVOT_MOTOR_ID = 17; //TODO: public static final double INTAKE_SPEED = 0.2; //TODO: public static final double HANDOFF_SPEED = 0.2; //TODO: public static final double PIVOT_SPEED = 0.2; //TODO: diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 3e1c75f..b485a53 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -16,6 +16,7 @@ import frc4388.robot.Constants.OIConstants; import frc4388.robot.commands.Autos.PlaybackChooser; import frc4388.robot.commands.Swerve.JoystickPlayback; import frc4388.robot.commands.Swerve.JoystickRecorder; +import frc4388.robot.commands.Intake.RotateIntakeToPosition; import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.Shooter; @@ -46,7 +47,7 @@ public class RobotContainer { private final Shooter m_robotShooter = new Shooter(m_robotMap.leftShooter, m_robotMap.rightShooter); private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.pivotMotor); - + //private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.pivotMotor); /* Controllers */ @@ -115,11 +116,14 @@ public class RobotContainer { /* Operator Buttons */ - new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) + new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) .onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter)) .onFalse(new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter)); - - new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) + + new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) + .onTrue(new InstantCommand(() -> new RotateIntakeToPosition(m_robotIntake, 360).execute(), m_robotIntake)) + .onFalse(new InstantCommand(() -> new RotateIntakeToPosition(m_robotIntake, 0).execute(), m_robotShooter)); + new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) .onTrue(new InstantCommand(() -> m_robotIntake.spinIntakeMotor())) .onFalse(new InstantCommand(() -> m_robotIntake.stopIntakeMotors())); diff --git a/src/main/java/frc4388/robot/commands/Intake/RotateIntakeToPosition.java b/src/main/java/frc4388/robot/commands/Intake/RotateIntakeToPosition.java new file mode 100644 index 0000000..2dd7a55 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Intake/RotateIntakeToPosition.java @@ -0,0 +1,38 @@ +// 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 frc4388.robot.commands.Intake; + + +import com.revrobotics.CANSparkMax; + +import edu.wpi.first.math.geometry.Translation2d; +import frc4388.robot.commands.PID; +import frc4388.robot.subsystems.SwerveDrive; +import edu.wpi.first.wpilibj.motorcontrol.Spark; + +import frc4388.robot.subsystems.Intake; +public class RotateIntakeToPosition extends PID { + + Intake intake; + double targetAngle; + + /** Creates a new PIDSparkMax. */ + public RotateIntakeToPosition(Intake intake, double targetAngle) { + super(0.3, 0.0, 0.0, 0.0, 1); + + this.intake = intake; + this.targetAngle = targetAngle; + + addRequirements(intake); + } + + @Override + public double getError() { + return targetAngle - (((intake.getEncoder().getPosition()) * (360))%360); + } + + @Override + public void runWithOutput(double output) { + intake.setVoltage(output / Math.abs(getError())); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index 3e1d82e..11ece07 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -6,7 +6,7 @@ package frc4388.robot.subsystems; import com.revrobotics.CANSparkMax; import com.revrobotics.SparkLimitSwitch; - +import com.revrobotics.RelativeEncoder; import edu.wpi.first.wpilibj.CAN; import edu.wpi.first.wpilibj.motorcontrol.Spark; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -16,7 +16,7 @@ public class Intake extends SubsystemBase { private CANSparkMax intakeMotor; private CANSparkMax pivot; - + /** Creates a new Intake. */ public Intake(CANSparkMax intakeMotor, CANSparkMax pivot) { this.intakeMotor = intakeMotor; @@ -46,8 +46,14 @@ public class Intake extends SubsystemBase { public void stopIntakeMotors() { intakeMotor.set(0); } - + public RelativeEncoder getEncoder() { + return pivot.getEncoder(); + } + public void setVoltage(double voltage) { + pivot.setVoltage(voltage); + } public void rotateArm() { + } @Override diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 01fae8c..86dd04b 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -77,9 +77,9 @@ public class SwerveDrive extends SubsystemBase { // 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(speed.getX(), speed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d());//.times(-1)); + chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d());//.times(-1)); } else { // Create robot-relative speeds. - chassisSpeeds = new ChassisSpeeds(leftStick.getX(), leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); + chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); } setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds)); }