hood servo

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"
 }

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

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() {

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,34 +132,24 @@ 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);
 
         // 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));
     }
+
 }

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
+  }
+}

Tyranus/src/main/java/frc/robot/subsystems/Shooter.java

@@ -15,18 +15,21 @@
 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;
 
   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)
   }
 }
+
+
+
+
+
+
+
+