From ed8cf1443cd7093c9844a918d149974ccf7ceee1 Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Thu, 17 Feb 2022 19:54:24 -0700 Subject: [PATCH] :)) --- .../java/frc4388/robot/RobotContainer.java | 6 ++- .../robot/commands/AimWithOdometry.java | 37 +++++++++++++++++++ .../frc4388/robot/subsystems/BoomBoom.java | 33 ++++++----------- .../java/frc4388/robot/subsystems/Turret.java | 30 +++++---------- .../java/frc4388/robot/subsystems/Vision.java | 2 + 5 files changed, 64 insertions(+), 44 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/AimWithOdometry.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 964e0d9..907cdb5 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -4,6 +4,8 @@ package frc4388.robot; +import com.revrobotics.CANSparkMax; + import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -72,7 +74,7 @@ public class RobotContainer { //Turret default command m_robotTurret.setDefaultCommand( - new RunCommand(() -> m_robotTurret.runShooterWithInput(getOperatorController().getLeftXAxis()), m_robotTurret)); + new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftXAxis()), m_robotTurret)); // m_robotTurret.setDefaultCommand( // new RunCommand(() -> m_robotTurret.aimToCenter())); } @@ -96,7 +98,7 @@ public class RobotContainer { .whenPressed(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0.1)) .whenReleased(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0)); - + /* Driver Buttons */ } /** diff --git a/src/main/java/frc4388/robot/commands/AimWithOdometry.java b/src/main/java/frc4388/robot/commands/AimWithOdometry.java new file mode 100644 index 0000000..cb882a4 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/AimWithOdometry.java @@ -0,0 +1,37 @@ +// 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 frc4388.robot.commands; + +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.wpilibj2.command.CommandBase; + +public class AimWithOdometry extends CommandBase { + /** Creates a new AimWithOdometry. */ + public AimWithOdometry() { + // Use addRequirements() here to declare subsystem dependencies. + } + + // 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() { + Pose2d odo = new Pose2d(new Translation2d(0,0), new Rotation2d(0)); + } + + // 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/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index 0a235e3..60caaf3 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -11,6 +11,7 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import edu.wpi.first.wpilibj.simulation.JoystickSim; import edu.wpi.first.wpilibj.smartdashboard.*; import edu.wpi.first.math.controller.BangBangController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; @@ -28,7 +29,7 @@ public ShooterTables m_shooterTable; public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS; public static BoomBoom m_boomBoom; public static IHandController m_driverController; //not sure if driverController in 2022 = m_controller in 2020 -BangBangController m_controller = new BangBangController(); +// BangBangController m_controller = new BangBangController(); double velP; double input; @@ -39,33 +40,19 @@ public double m_fireVel; public Hood m_hoodSubsystem; public Turret m_turretSubsystem; -SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(69, 42, 0); //get real values later +// SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(69, 42, 0); //get real values later /* * Creates new BoomBoom subsystem, has drum shooter and angle adjuster */ -public BoomBoom(){ -//Testing purposes resetting gyros -//resetGryoAngleADj(); -//shooterTrims = new Trims(0,0); -feedforward.calculate(15, 20); // feedforward.calculate(velocity, acceleration); - - -} /** Creates a new BoomBoom. */ - public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) { +public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) { m_shooterFalconLeft = shooterFalconLeft; m_shooterFalconRight = shooterFalconRight; - - - - - m_shooterTable = new ShooterTables(); - - - } + m_shooterTable = new ShooterTables(); +} @Override @@ -92,7 +79,10 @@ try { m_hoodSubsystem = subsystem0; m_turretSubsystem = subsystem1; } - +/** + * Runs the Drum motor at a given speed + * @param speed percent output form -1.0 to 1.0 + */ public void runDrumShooter(double speed) { m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed); @@ -114,7 +104,8 @@ public void setShooterGains() { //Controls a motor with the output of the BangBang conroller and a feedforward //Shrinks the feedforward slightly to avoid over speeding the shooter + // m_shooterFalconLeft.set(controller.calculate(encoder.getRate(), targetVel) + 0.9 * feedforward.calculate(targetVel)); - m_shooterFalconLeft.set(m_controller.calculate(m_shooterFalconLeft.get(), targetVel)); + // m_shooterFalconLeft.set(m_controller.calculate(m_shooterFalconLeft.get(), targetVel)); } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 2f70146..2890df7 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -42,14 +42,14 @@ public class Turret extends SubsystemBase { //Variables - public Turret() { + public Turret(CANSparkMax BoomBoomRotateMotor) { //Take in rotate motor as an argument - m_boomBoomRotateMotor = new CANSparkMax(30, MotorType.kBrushless); + m_boomBoomRotateMotor = BoomBoomRotateMotor; m_boomBoomRotatePIDController = m_boomBoomRotateMotor.getPIDController(); m_boomBoomRotateEncoder = m_boomBoomRotateMotor.getEncoder(); m_boomBoomRotateMotor.setIdleMode(IdleMode.kBrake); - m_turretGyro = getGyroInterface(); + m_boomBoomLeftLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); m_boomBoomRightLimit.enableLimitSwitch(true); @@ -57,14 +57,11 @@ public class Turret extends SubsystemBase { m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, false); m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); - m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_RIGHT_SOFT_LIMIT); + m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_RIGHT_SOFT_LIMIT); //Set second soft limit m_boomBoomRotateMotor.setInverted(false); } - private Gyro getGyroInterface() { - return null; - } @Override public void periodic() { @@ -81,16 +78,18 @@ public class Turret extends SubsystemBase { m_sDriveSubsystem = subsystem1; } - public void runShooterWithInput(double input) { + public void runTurretWithInput(double input) { //Rename to turret m_boomBoomRotateMotor.set(input*ShooterConstants.TURRET_SPEED_MULTIPLIER); } - public void runshooterRotatePID(double targetAngle) { + public void runshooterRotatePID(double targetAngle) { //Split into configure and run m_boomBoomRotatePIDController.setP(m_shooterTGains.m_kP); m_boomBoomRotatePIDController.setI(m_shooterTGains.m_kI); m_boomBoomRotatePIDController.setD(m_shooterTGains.m_kD); m_boomBoomRotatePIDController.setFF(m_shooterTGains.m_kF); m_boomBoomRotatePIDController.setIZone(m_shooterTGains.m_kIzone); + + m_boomBoomRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTGains.m_kPeakOutput); targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT; @@ -111,16 +110,5 @@ public class Turret extends SubsystemBase { public double getAnglePositionDegrees() { return (m_boomBoomRotateEncoder.getPosition() - ShooterConstants.TURRET_MOTOR_POS_AT_ZERO_ROT) * 360/ShooterConstants. TURRET_MOTOR_ROTS_PER_ROT; } - - - //function turnWithJoystick(double input) - // motor.set(input) -} - - - -/** TODO -* setPosition function -* Limit switches -**/ \ No newline at end of file +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index 310d70f..0b6ca09 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -37,6 +37,8 @@ public static double fireAngle; public double m_hoodTrim; public double m_turretTrim; public double m_fireAngle; + + public Vision(Turret aimSubsystem, BoomBoom boomBoom) { m_turret = aimSubsystem; m_boomBoom = boomBoom;