From 493c4f69de00c27da7b91d685481a851433cac55 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Sat, 29 Feb 2020 18:42:37 -0700 Subject: [PATCH] hood turret and tracking working --- src/main/java/frc4388/robot/Constants.java | 6 +- .../java/frc4388/robot/RobotContainer.java | 27 +++++--- .../robot/commands/CalibrateShooter.java | 63 +++++++++++++++++++ .../frc4388/robot/commands/HoldTarget.java | 20 ++++-- .../robot/commands/HoodPositionPID.java | 48 ++++++++++++++ .../frc4388/robot/commands/TrackTarget.java | 8 +-- .../frc4388/robot/subsystems/Shooter.java | 23 ++++--- .../frc4388/robot/subsystems/ShooterAim.java | 24 ++++--- 8 files changed, 184 insertions(+), 35 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/CalibrateShooter.java create mode 100644 src/main/java/frc4388/robot/commands/HoodPositionPID.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 02b6f7b..52fa895 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -122,7 +122,7 @@ public final class Constants { //public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0005, 13, 0.05, 0, 1.0); public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.2, 0.0, 0, 0.0453, 0, 1.0); public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0); - public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.5, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 1.0); public static final double SHOOTER_TURRET_MIN = -1.0; public static final double ENCODER_TICKS_PER_REV = 2048; public static final double NEO_UNITS_PER_REV = 42; @@ -169,8 +169,8 @@ public final class Constants { public static final class VisionConstants { public static final double FOV = 29.8; //Field of view of limelight - public static final double TARGET_HEIGHT = 82.75; - public static final double LIME_ANGLE = 18.7366; + public static final double TARGET_HEIGHT = 64; + public static final double LIME_ANGLE = 25; public static final double TURN_P_VALUE = 0.65; public static final double X_ANGLE_ERROR = 1.3; public static final double MOTOR_DEAD_ZONE = 0.3; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 08277bc..46f2b90 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -16,22 +16,26 @@ import edu.wpi.first.wpilibj.controller.RamseteController; import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Rotation2d; import edu.wpi.first.wpilibj.geometry.Translation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.trajectory.Trajectory; import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig; import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RamseteCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; +import frc4388.robot.commands.CalibrateShooter; import frc4388.robot.commands.DrivePositionMPAux; import frc4388.robot.commands.DriveWithJoystick; import frc4388.robot.commands.DriveStraightToPositionMM; import frc4388.robot.commands.DriveStraightToPositionPID; import frc4388.robot.commands.DriveWithJoystickUsingDeadAssistPID; import frc4388.robot.commands.HoldTarget; +import frc4388.robot.commands.HoodPositionPID; import frc4388.robot.commands.DriveWithJoystickDriveStraight; import frc4388.robot.commands.RunClimberWithTriggers; import frc4388.robot.commands.RunExtenderOutIn; @@ -169,14 +173,18 @@ public class RobotContainer { // safety for climber and leveler new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON) - .whenPressed(new InstantCommand(() -> m_robotClimber.setSafetyPressed(), m_robotClimber)) - .whenReleased(new InstantCommand(() -> m_robotClimber.setSafetyNotPressed(), m_robotClimber)); + //.whenPressed(new InstantCommand(() -> m_robotClimber.setSafetyPressed(), m_robotClimber)) + //.whenReleased(new InstantCommand(() -> m_robotClimber.setSafetyNotPressed(), m_robotClimber)); + .whileHeld(new RunCommand(() -> SmartDashboard.putNumber("Angle Current", m_robotShooter.m_angleAdjustMotor.getOutputCurrent()))) + .whileHeld(new RunCommand(() -> SmartDashboard.putNumber("Rotate Current", m_robotShooterAim.m_shooterRotateMotor.getOutputCurrent()))); // starts tracking target new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) - .whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage)) - //.whileHeld(new TrackTarget(m_robotShooter,m_robotShooterAim)) - .whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage)); + //.whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage)) + .whileHeld(new ParallelCommandGroup( + new HoldTarget(m_robotShooter,m_robotShooterAim), + new HoodPositionPID(m_robotShooter))); + //.whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage)); //.whenReleased(new RunCommand(() -> m_robotLime.limeOff())); //Prepares storage for intaking @@ -188,16 +196,17 @@ public class RobotContainer { .whileHeld(new StorageOutake(m_robotStorage)); new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) - .whileHeld(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0.2))) + .whileHeld(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0.3))) .whenReleased(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0))); new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON) - .whileHeld(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(-0.2))) + .whileHeld(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(-0.3))) .whenReleased(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0))); new JoystickButton(getOperatorJoystick(), XboxController.START_BUTTON) - .whileHeld(new RunCommand(() -> m_robotStorage.runStoragePositionPID(7))) - .whenReleased(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0))); + .whileHeld(new CalibrateShooter(m_robotShooter, m_robotShooterAim)); + + } /** diff --git a/src/main/java/frc4388/robot/commands/CalibrateShooter.java b/src/main/java/frc4388/robot/commands/CalibrateShooter.java new file mode 100644 index 0000000..1349f1c --- /dev/null +++ b/src/main/java/frc4388/robot/commands/CalibrateShooter.java @@ -0,0 +1,63 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.commands; + +import com.revrobotics.CANSparkMax.SoftLimitDirection; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.Shooter; +import frc4388.robot.subsystems.ShooterAim; + +public class CalibrateShooter extends CommandBase { + Shooter m_shooter; + ShooterAim m_shooterAim; + /** + * Creates a new CalibrateShooter. + */ + public CalibrateShooter(Shooter shootSub, ShooterAim aimSub) { + m_shooter = shootSub; + m_shooterAim = aimSub; + addRequirements(m_shooter, m_shooterAim); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_shooter.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, false); + m_shooter.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, false); + m_shooter.m_angleEncoder.setPosition(0); + m_shooter.m_angleAdjustMotor.set(-0.1); + + m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, false); + m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, false); + m_shooterAim.m_shooterRotateEncoder.setPosition(0); + m_shooterAim.m_shooterRotateMotor.set(0.075); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + m_shooter.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, true); + m_shooter.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); + + m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, true); + m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc4388/robot/commands/HoldTarget.java b/src/main/java/frc4388/robot/commands/HoldTarget.java index bbbe656..791266d 100644 --- a/src/main/java/frc4388/robot/commands/HoldTarget.java +++ b/src/main/java/frc4388/robot/commands/HoldTarget.java @@ -41,7 +41,7 @@ public class HoldTarget extends CommandBase { public HoldTarget(Shooter shooterSubsystem, ShooterAim aimSubsystem) { m_shooterAim = aimSubsystem; m_shooter = shooterSubsystem; - addRequirements(m_shooter); + addRequirements(m_shooterAim); NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1); } @@ -69,7 +69,7 @@ public class HoldTarget extends CommandBase { //Deadzones else if(turnAmount > 0 && turnAmount < VisionConstants.MOTOR_DEAD_ZONE){turnAmount = VisionConstants.MOTOR_DEAD_ZONE;} else if(turnAmount < 0 && turnAmount > -VisionConstants.MOTOR_DEAD_ZONE){turnAmount = -VisionConstants.MOTOR_DEAD_ZONE;} - m_shooterAim.runShooterWithInput(turnAmount/3); + m_shooterAim.runShooterWithInput(-turnAmount); //Finding Distance distance = VisionConstants.TARGET_HEIGHT/Math.tan((VisionConstants.LIME_ANGLE + yAngle)*(Math.PI/180)); @@ -79,17 +79,29 @@ public class HoldTarget extends CommandBase { double xVel = (distance*VisionConstants.GRAV)/(yVel); fireVel = Math.sqrt((Math.pow(xVel, 2))+(Math.pow(yVel,2))); - fireAngle = Math.atan(yVel/xVel); + fireAngle = Math.atan(yVel/xVel) * (180/Math.PI); m_shooter.m_fireVel = fireVel; m_shooter.m_fireAngle = fireAngle; + }/* + else{ + System.err.println("Shooter Pos: " + m_shooterAim.getShooterRotatePosition()); + double curveInput = -Math.abs(-Math.cos(Math.PI * ((2*m_shooterAim.getShooterRotatePosition())/55))+1) * 0.1; + if (m_shooterAim.getShooterRotatePosition() >= -3 || m_shooterAim.getShooterRotatePosition() <= -54){ + curveInput = -curveInput; + } + System.err.println("Curve Input: " + curveInput); + + m_shooterAim.runShooterWithInput(curveInput); } + */ } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - + NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1); + NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0); } // Returns true when the command should end. diff --git a/src/main/java/frc4388/robot/commands/HoodPositionPID.java b/src/main/java/frc4388/robot/commands/HoodPositionPID.java new file mode 100644 index 0000000..dd276f0 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/HoodPositionPID.java @@ -0,0 +1,48 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.commands; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.Shooter; + +public class HoodPositionPID extends CommandBase { + Shooter m_shooter; + /** + * Creates a new HoodPositionPID. + */ + public HoodPositionPID(Shooter subSystem) { + m_shooter = subSystem; + addRequirements(m_shooter); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double firingAngle = (-0.47*m_shooter.addFireAngle())+40.5; + SmartDashboard.putNumber("Shoot Angle From Equation", m_shooter.addFireAngle()); + SmartDashboard.putNumber("Fire Angle", firingAngle); + m_shooter.runAngleAdjustPID(firingAngle); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java index 5db2669..2e0e512 100644 --- a/src/main/java/frc4388/robot/commands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/TrackTarget.java @@ -74,7 +74,7 @@ public class TrackTarget extends CommandBase { } else if (turnAmount < 0 && turnAmount > -VisionConstants.MOTOR_DEAD_ZONE) { turnAmount = -VisionConstants.MOTOR_DEAD_ZONE; } - m_shooterAim.runShooterWithInput(turnAmount); + m_shooterAim.runShooterWithInput(-turnAmount); // Finding Distance distance = VisionConstants.TARGET_HEIGHT / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180)); @@ -84,17 +84,17 @@ public class TrackTarget extends CommandBase { double xVel = (distance * VisionConstants.GRAV) / (yVel); fireVel = Math.sqrt((Math.pow(xVel, 2)) + (Math.pow(yVel, 2))); - fireAngle = Math.atan(yVel / xVel); + fireAngle = Math.atan(yVel / xVel) * (180/Math.PI); m_shooter.m_fireVel = fireVel; m_shooter.m_fireAngle = fireAngle; - } } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - + NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1); + NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0); } // Returns true when the command should end. diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index c8bb07d..1f0a699 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -13,6 +13,7 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANSparkMax.SoftLimitDirection; import com.revrobotics.CANDigitalInput; import com.revrobotics.CANEncoder; import com.revrobotics.CANPIDController; @@ -35,7 +36,7 @@ public class Shooter extends SubsystemBase { public CANSparkMax m_angleAdjustMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); CANPIDController m_angleAdjustPIDController = m_angleAdjustMotor.getPIDController(); - CANEncoder m_angleEncoder = m_angleAdjustMotor.getEncoder(); + public CANEncoder m_angleEncoder = m_angleAdjustMotor.getEncoder(); public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS; public static Gains m_angleAdjustGains = ShooterConstants.SHOOTER_ANGLE_GAINS; @@ -57,10 +58,10 @@ public class Shooter extends SubsystemBase { */ public Shooter() { //Testing purposes reseting gyros - resetGyroAngleAdj(); + //resetGyroAngleAdj(); m_shooterFalcon.configFactoryDefault(); m_shooterFalcon.setNeutralMode(NeutralMode.Coast); - m_shooterFalcon.setInverted(false); + m_shooterFalcon.setInverted(true); m_angleAdjustMotor.setIdleMode(IdleMode.kBrake); m_shooterFalcon.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); setShooterGains(); @@ -91,6 +92,11 @@ public class Shooter extends SubsystemBase { m_hoodDownLimit = m_angleAdjustMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen); m_hoodUpLimit.enableLimitSwitch(true); m_hoodDownLimit.enableLimitSwitch(true); + + m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, true); + m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); + m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kForward, 33); + m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kReverse, 3); } @Override @@ -106,6 +112,7 @@ public class Shooter extends SubsystemBase { return m_fireAngle; } + /** * Runs drum shooter motor. * @param speed Speed to set the motor at @@ -126,9 +133,8 @@ public class Shooter extends SubsystemBase { } /* Angle Adjustment PID Control */ - public void runAngleAdjustPID(double mmtargetAngle) + public void runAngleAdjustPID(double targetAngle) { - double targetAngle = addFireAngle(); // Set PID Coefficients m_angleAdjustPIDController.setP(m_angleAdjustGains.m_kP); m_angleAdjustPIDController.setI(m_angleAdjustGains.m_kI); @@ -137,9 +143,6 @@ public class Shooter extends SubsystemBase { m_angleAdjustPIDController.setFF(m_angleAdjustGains.m_kF); m_angleAdjustPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_angleAdjustGains.m_kPeakOutput); - // Convert input angle in degrees to rotations of the motor - targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT; - m_angleAdjustPIDController.setReference(targetAngle, ControlType.kPosition); } @@ -162,4 +165,8 @@ public class Shooter extends SubsystemBase { public void resetGyroAngleAdj(){ m_angleEncoder.setPosition(0); } + + public double getAnglePosition(){ + return m_angleAdjustMotor.getEncoder().getPosition(); + } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/ShooterAim.java b/src/main/java/frc4388/robot/subsystems/ShooterAim.java index beeccda..be69e18 100644 --- a/src/main/java/frc4388/robot/subsystems/ShooterAim.java +++ b/src/main/java/frc4388/robot/subsystems/ShooterAim.java @@ -14,9 +14,11 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.ControlType; import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANSparkMax.SoftLimitDirection; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Gains; import frc4388.robot.Constants.ShooterConstants; @@ -28,18 +30,24 @@ public class ShooterAim extends SubsystemBase { // Configure PID Controllers CANPIDController m_shooterRotatePIDController = m_shooterRotateMotor.getPIDController(); - CANEncoder m_shooterRotateEncoder = m_shooterRotateMotor.getEncoder(); + public CANEncoder m_shooterRotateEncoder = m_shooterRotateMotor.getEncoder(); + /** * Creates a new ShooterAim. */ public ShooterAim() { - resetGyroShooterRotate(); + //resetGyroShooterRotate(); m_shooterRotateMotor.setIdleMode(IdleMode.kBrake); - m_shooterRightLimit = m_shooterRotateMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen); - m_shooterLeftLimit = m_shooterRotateMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen); + m_shooterLeftLimit = m_shooterRotateMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen); + m_shooterRightLimit = m_shooterRotateMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen); m_shooterRightLimit.enableLimitSwitch(true); m_shooterLeftLimit.enableLimitSwitch(true); + + m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, false); + m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); + m_shooterRotateMotor.setSoftLimit(SoftLimitDirection.kForward, -2); + m_shooterRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, -56); } public void runShooterWithInput(double input) { @@ -64,11 +72,13 @@ public class ShooterAim extends SubsystemBase { m_shooterRotatePIDController.setReference(targetAngle, ControlType.kPosition); } - - /* For Testing Purposes, reseting gyro for shooter rotation */ public void resetGyroShooterRotate() { - m_shooterRotateEncoder.setPosition(0); + m_shooterRotateEncoder.setPosition(0); + } + + public double getShooterRotatePosition(){ + return m_shooterRotateMotor.getEncoder().getPosition(); } @Override