From 40c666d3e327b6b97be37ef5a4cf470154265b80 Mon Sep 17 00:00:00 2001 From: Lori2018 Date: Thu, 16 Jul 2020 20:01:59 -0400 Subject: [PATCH] hood servo --- Tyranus/.vscode/settings.json | 9 +- Tyranus/build.gradle | 2 +- Tyranus/src/main/java/frc/robot/Robot.java | 3 +- .../main/java/frc/robot/RobotContainer.java | 56 +++---- .../java/frc/robot/subsystems/IntakeNEW.java | 46 ++++++ .../java/frc/robot/subsystems/Shooter.java | 35 ++++- .../java/frc/robot/subsystems/Vision.java | 141 ++++++++++++++++++ 7 files changed, 250 insertions(+), 42 deletions(-) create mode 100644 Tyranus/src/main/java/frc/robot/subsystems/IntakeNEW.java create mode 100644 Tyranus/src/main/java/frc/robot/subsystems/Vision.java diff --git a/Tyranus/.vscode/settings.json b/Tyranus/.vscode/settings.json index f7b78e0..a9cb731 100644 --- a/Tyranus/.vscode/settings.json +++ b/Tyranus/.vscode/settings.json @@ -12,5 +12,12 @@ "**/.settings": true, "**/.factorypath": true, "**/*~": true - } + }, + "extensions.ignoreRecommendations": true + "extensions.ignoreRecommendations": true, + "files.autoSave": "off" + "extensions.ignoreRecommendations": true, + "files.autoSave": "off" + "extensions.ignoreRecommendations": true, + "files.autoSave": "off" } diff --git a/Tyranus/build.gradle b/Tyranus/build.gradle index 3efbd14..6ecfc70 100644 --- a/Tyranus/build.gradle +++ b/Tyranus/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2020.2.2" + id "edu.wpi.first.GradleRIO" version "2020.3.2" } sourceCompatibility = JavaVersion.VERSION_11 diff --git a/Tyranus/src/main/java/frc/robot/Robot.java b/Tyranus/src/main/java/frc/robot/Robot.java index e46f994..8ff6a04 100644 --- a/Tyranus/src/main/java/frc/robot/Robot.java +++ b/Tyranus/src/main/java/frc/robot/Robot.java @@ -36,8 +36,7 @@ public void robotInit() { CameraServer.getInstance().startAutomaticCapture().setResolution(320, 160); // hoodEncoder.configAllSettings(_canCoderConfiguration); - - } + } @Override public void robotPeriodic() { diff --git a/Tyranus/src/main/java/frc/robot/RobotContainer.java b/Tyranus/src/main/java/frc/robot/RobotContainer.java index 0c59119..0fc3f25 100644 --- a/Tyranus/src/main/java/frc/robot/RobotContainer.java +++ b/Tyranus/src/main/java/frc/robot/RobotContainer.java @@ -48,6 +48,7 @@ public class RobotContainer { public final static Shooter shooter = new Shooter(); private final Climb climb = new Climb(); private final Turret spinny = new Turret(); +// private final Vision vision = new Vision(); // The driver's controller public static Joystick m_driverController = new Joystick(OIConstants.kDriverControllerPort); @@ -97,6 +98,8 @@ public RobotContainer() { spinny.setDefaultCommand(new RunCommand( () -> spinny.spin(m_driverController.getRawAxis(2), m_driverController.getRawAxis(3)), spinny)); + + // vision.setDefaultCommand(new RunCommand(vision::runVision, vision)); } /** @@ -111,11 +114,15 @@ private void configureButtonBindings() { .whenReleased(intake::retract, intake); new JoystickButton(m_operatorController, 6).whileHeld(new InstantCommand(shooter::runShooter, shooter)) .whenReleased(shooter::stopShooter, shooter); + new JoystickButton(m_operatorController, 9).whileHeld(new RunCommand(() -> shooter.runHood(0), shooter)); + new JoystickButton(m_operatorController, 10).whileHeld(new RunCommand(() -> shooter.runHood(1), shooter)); new JoystickButton(m_operatorController, 6) .whenPressed(new RunCommand(() -> zoom.feedShooter(0.8, shooter.atSpeed()), zoom)) .whenReleased(new RunCommand(zoom::autoIndex, zoom)); new JoystickButton(m_operatorController, 5).whenPressed(new RunCommand(() -> zoom.manualControl(-1), zoom)) .whenReleased(new RunCommand(zoom::autoIndex, zoom)); + // new JoystickButton(m_operatorController, 8).whileHeld(new InstantCommand(vision::runVision, vision)) + // .whenReleased(vision::filler, vision); // not sure what to put as method here } /** @@ -125,7 +132,7 @@ private void configureButtonBindings() { */ public Command getAutonomousCommand() { // Create config for trajectory - TrajectoryConfig config = new TrajectoryConfig(AutoConstants.kMaxSpeedMetersPerSecond, + TrajectoryConfig config = new TrajectoryConfig(.75, AutoConstants.kMaxAccelerationMetersPerSecondSquared) // Add kinematics to ensure max speed is actually obeyed .setKinematics(SwerveDriveConstants.kDriveKinematics); @@ -133,26 +140,16 @@ public Command getAutonomousCommand() { // An example trajectory to follow. All units in meters. Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory( // Start at the origin facing the +X direction - new Pose2d(0, 0, new Rotation2d(-(Math.PI) / 2.)), - // Pass through these two interior waypoints, making an 's' curve path - List.of(new Translation2d(0, 1) - - ), + new Pose2d(0, 0, new Rotation2d((Math.PI) / 2)), + List.of(), // End 3 meters straight ahead of where we started, facing forward - new Pose2d(0, 2, new Rotation2d(-(Math.PI) / 2.)), config); + new Pose2d(-4, 2.3, new Rotation2d((Math.PI) / 2)), config); + - Trajectory exampleTrajectory2 = TrajectoryGenerator.generateTrajectory( - // Start at the origin facing the +X direction - new Pose2d(0, 2, new Rotation2d(-(Math.PI) / 2.)), - // Pass through these two interior waypoints, making an 's' curve path - List.of(new Translation2d(0, -1) - ), - // End 3 meters straight ahead of where we started, facing forward - new Pose2d(0, -2, new Rotation2d(-(Math.PI) / 2.)), config); SwerveControllerCommand swerveControllerCommand1 = new SwerveControllerCommand(exampleTrajectory, - (-(Math.PI) / 2.), swerveDrive::getPose, // Functional interface to feed supplier + (1.875), swerveDrive::getPose, // Functional interface to feed supplier SwerveDriveConstants.kDriveKinematics, // Position controllers @@ -165,28 +162,13 @@ public Command getAutonomousCommand() { ); - // SwerveControllerCommand swerveControllerCommand2 = new - // SwerveControllerCommand(exampleTrajectory2, - // (-(Math.PI) / 2.), swerveDrive::getPose, // Functional interface to feed - // supplier - // SwerveDriveConstants.kDriveKinematics, - - // // Position controllers - // new PIDController(AutoConstants.kPXController, 0, 0), - // new PIDController(AutoConstants.kPYController, 0, 0), theta, + Command shootCommand = new InstantCommand(() -> shooter.setHood(0.5), shooter) + .andThen(shooter::runShooter, shooter) + .andThen(new RunCommand(() -> zoom.feedShooter(0.75, shooter.atSpeed()), zoom)) + .withTimeout(15).andThen(new InstantCommand(shooter::stopShooter, shooter)); - // swerveDrive::setModuleStates, + return swerveControllerCommand1.andThen(() -> swerveDrive.drive(0, 0, 0, false)).andThen(shootCommand); - // swerveDrive - - // ); - - // Run path following command, then stop at the end. - return new RunCommand(() -> spinny.setPosition(-42), spinny) - .andThen(new InstantCommand(shooter::runShooter, shooter)) - .andThen(new RunCommand(() -> zoom.feedShooter(0.75, shooter.atSpeed()), zoom)).withTimeout(4) - .andThen(new InstantCommand(shooter::stopShooter, shooter)) - .andThen(new RunCommand(zoom::autoIndex, zoom)).withTimeout(5).andThen(swerveControllerCommand1) - /* .andThen(swerveControllerCommand2) */.andThen(() -> swerveDrive.drive(0, 0, 0, false)); } + } diff --git a/Tyranus/src/main/java/frc/robot/subsystems/IntakeNEW.java b/Tyranus/src/main/java/frc/robot/subsystems/IntakeNEW.java new file mode 100644 index 0000000..6d2af88 --- /dev/null +++ b/Tyranus/src/main/java/frc/robot/subsystems/IntakeNEW.java @@ -0,0 +1,46 @@ +/*----------------------------------------------------------------------------*/ +/* 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 frc.robot.subsystems; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + +import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.DoubleSolenoid.Value; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + +public class IntakeNEW extends SubsystemBase { + /** + * Creates a new IntakeNEW. + */ + + private CANSparkMax intakeMotor = new CANSparkMax(Constants.Intake.motor, MotorType.kBrushless); + private DoubleSolenoid solenoid = new DoubleSolenoid(0, 1); + + public IntakeNEW() { + + } + + public void extend() { + solenoid.set(Value.kForward); + } + + public void retract() { + solenoid.set(Value.kReverse); + } + + public void runWheels() { + intakeMotor.set(1); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} diff --git a/Tyranus/src/main/java/frc/robot/subsystems/Shooter.java b/Tyranus/src/main/java/frc/robot/subsystems/Shooter.java index cbc2e60..0a227f3 100644 --- a/Tyranus/src/main/java/frc/robot/subsystems/Shooter.java +++ b/Tyranus/src/main/java/frc/robot/subsystems/Shooter.java @@ -15,11 +15,13 @@ import com.revrobotics.CANSparkMaxLowLevel.MotorType; import com.revrobotics.CANSparkMaxLowLevel.PeriodicFrame; +import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Shooter extends SubsystemBase { private final CANSparkMax shooter1, shooter2; + private final Servo hoodServo; private CANPIDController m_pidController; private CANEncoder m_encoder; public double kP, kI, kD, kIz, kFF, kMaxOutput, kMinOutput, maxRPM, setPoint, speed; @@ -27,6 +29,7 @@ public class Shooter extends SubsystemBase { public Shooter() { shooter1 = new CANSparkMax(12, MotorType.kBrushless); shooter2 = new CANSparkMax(13, MotorType.kBrushless); + hoodServo = new Servo(0); shooter1.restoreFactoryDefaults(); shooter1.setIdleMode(IdleMode.kCoast); shooter1.setSmartCurrentLimit(60); @@ -58,6 +61,7 @@ public Shooter() { m_pidController.setOutputRange(kMinOutput, kMaxOutput); SmartDashboard.putNumber("Shooter P", kP); SmartDashboard.putNumber("Shooter Velocity", speed); + } public void runShooter() { @@ -69,13 +73,42 @@ public void runShooter() { SmartDashboard.putNumber("OutputCurrent", shooter1.get()); } + public void stopShooter() { shooter1.set(0); } + + + + + public void runHood(double pos) { + double currentPos = hoodServo.get(); + if (pos == 0) { + hoodServo.set(currentPos - 0.01); + } else if (pos == 1) { + hoodServo.set(currentPos + 0.01); + } + } + + public void setHood(double pos) { + hoodServo.set(pos); + } + + public void periodic() { SmartDashboard.putNumber("ProcessVariable", m_encoder.getVelocity()); + SmartDashboard.putNumber("SHOOTER HOODS POS", hoodServo.get()); + SmartDashboard.putNumber("SHOOTER HOODS ANGLE", hoodServo.getAngle()); } public boolean atSpeed() { - return Math.abs(setPoint - m_encoder.getVelocity())<250; + return Math.abs(setPoint - m_encoder.getVelocity())<300; // play with the number (go up to 1,000) } } + + + + + + + + diff --git a/Tyranus/src/main/java/frc/robot/subsystems/Vision.java b/Tyranus/src/main/java/frc/robot/subsystems/Vision.java new file mode 100644 index 0000000..7f2a677 --- /dev/null +++ b/Tyranus/src/main/java/frc/robot/subsystems/Vision.java @@ -0,0 +1,141 @@ +// package frc.robot.subsystems; + +// import frc.robot.Robot; +// import frc.robot.RobotContainer; +// import frc.robot.subsystems.SwerveDrive; + +// import edu.wpi.first.cameraserver.CameraServer; +// import edu.wpi.first.networktables.NetworkTable; +// import edu.wpi.first.networktables.NetworkTableEntry; +// import edu.wpi.first.networktables.NetworkTableInstance; +// import edu.wpi.first.wpilibj.Joystick; +// import edu.wpi.first.wpilibj.command.Command; +// import edu.wpi.first.wpilibj.command.InstantCommand; +// import edu.wpi.first.wpilibj.geometry.Rotation2d; +// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +// import edu.wpi.first.wpilibj2.command.RunCommand; +// import edu.wpi.first.wpilibj2.command.SubsystemBase; +// import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; +// import frc.robot.subsystems.*; + + +// public class Vision extends SubsystemBase { +// public static SwerveDrive swerveDrive = new SwerveDrive(); +// private final Conveyor zoom = new Conveyor(); +// public final static Shooter shooter = new Shooter(); + +// NetworkTable table; + +// NetworkTableEntry targetX; // horizontal angle +// NetworkTableEntry targetY; // vertical angle + +// double rotationError; // Rotation error value +// double distanceError; // Distance error value + +// // Deadzone is necessary becasue the robot can only get so accurate and cannot be perfectly head on target +// double angleTolerance = 5; // Deadzone for the angle control loop +// double distanceTolerance = 5; // Deadzone for the distance control loop + +// double rotationAjust; // rotational signal for the drivetrain (?) +// double distanceAjust; // distance signal for the drivetrain (?) + +// // these may be specific to the drivetrain used in the docs...? +// double KpRot=-0.1; +// double KpDist=-0.1; + +// double constantForce = 0.05; + +// double random; // really confused at this point + +// double xDistance = 0; // to be used in later function triggy() +// double yDistance = 0; // ditto ^ +// Rotation2d currentRotationSwerve = new Rotation2d(); +// SwerveControllerCommand visionCommand; + +// // to be called in RobotContainer +// public Vision() { + +// // initializes "table" as the Network Tables database named "chameleon-vision" +// table = NetworkTableInstance.getDefault().getTable("chameleon-vision").getSubTable("CameraName"); + +// // points targets to database values +// targetX = table.getEntry("yaw"); +// targetY = table.getEntry("pitch"); + +// SmartDashboard.putNumber("idk", 4); +// } + +// public Command runVision() { +// rotationAjust = 0; // not sure what this does +// distanceAjust = 0; // maybe: will be used to tell the robot how much to move + +// // control loop for rotation +// if (rotationError > angleTolerance) { +// rotationAjust = KpRot * rotationError + constantForce; +// } else if (rotationError < angleTolerance) { +// rotationAjust = KpRot * rotationError - constantForce; +// } + +// // control loop for distance +// if (distanceError > distanceTolerance) { +// distanceAjust = KpDist * distanceError + constantForce; +// } else if (distanceError < distanceTolerance) { +// distanceAjust = KpDist * distanceError - constantForce; +// } + +// // *** ATTEMPT #1 *** - simply using drive() +// // line up swerve/drivetrain with target +// // somehow convert rotation & distance into x & y values - trigonometry + +// // probably have to reset gyro --- wait no i think that'd be bad + +// // this doesn't take the robot's current position into account i think +// // triggy(rotationAjust, distanceAjust); +// respectfulTriggy(rotationAjust, distanceAjust); + + +// // run it all and shoot! +// // this didn't work +// // visionCommand = new RunCommand(() -> RobotContainer.swerveDrive.drive(xDistance, +// // yDistance, +// // rotationAjust, // not really sure what to put for rot +// // true), +// // swerveDrive); +// // call the command -- don't know how to do that (where do commands get ultimately called?) + +// // return visionCommand; + +// // *** ATTEMPT #2 *** - using trajectories +// // nevermind it seems a lot more complicated +// } + +// public void filler() { +// double random = 0; +// } + +// // trigonometry to figure our x and y coordinates +// public void triggy(double rotation, double distance) { +// // x-part of triangle = cos(ANGLE) * HYPOTENUSE +// xDistance = Math.cos(rotation) * distance; + +// // y-part of triangle = sin(ANGLE) * HYPOTENUSE +// yDistance = Math.sin(rotation) * distance; +// } + +// // as in with respect to current values +// public void respectfulTriggy(double rotation, double distance) { +// // double firstXDistance = Math.cos(rotation) * distance; +// if (currentRotationSwerve.getCos() > rotation) { +// xDistance = (distance * (currentRotationSwerve.getCos() - rotation)); +// } else { +// xDistance = (distance * (rotation - currentRotationSwerve.getCos())); +// } + +// if (currentRotationSwerve.getSin() > rotation) { +// yDistance = (distance * (currentRotationSwerve.getSin() - rotation)); +// } else { +// yDistance = (distance * (rotation - currentRotationSwerve.getSin())); +// } +// } + +// } \ No newline at end of file