Elevator Command

src/main/java/frc/robot/OI.java

@@ -12,6 +12,7 @@
 import edu.wpi.first.wpilibj.buttons.JoystickButton;
 import frc.robot.commands.ElevatorCommand.levelHeight;
 import frc.robot.commands.ElevatorCommand;
+
 /**
  * This class is the glue that binds the controls on the physical operator
  * interface to the commands and command groups that allow control of the robot.
@@ -29,7 +30,7 @@ public class OI {
             buttonBack1 = new JoystickButton(xboxLeft, 7),
             buttonStart1 = new JoystickButton(xboxLeft, 8);
 
-    public  Button buttonA2 = new JoystickButton(xboxRight,1),
+    public Button buttonA2 = new JoystickButton(xboxRight, 1),
             buttonB2 = new JoystickButton(xboxRight, 2),
             buttonX2 = new JoystickButton(xboxRight, 3),
             buttonY2 = new JoystickButton(xboxRight, 4),
@@ -38,17 +39,17 @@ public class OI {
             buttonBack2 = new JoystickButton(xboxRight, 7),
             buttonStart2 = new JoystickButton(xboxRight, 8);
 
-    public OI(){
+    public OI() {
         // buttonA1.whenPressed(new ElevatorCommand(levelHeight.LEVEL_1));
         // buttonB1.whenPressed(new ElevatorCommand(levelHeight.LEVEL_2));
         // buttonX1.whenPressed(new ElevatorCommand(levelHeight.LEVEL_3));
     }
 
-    public double getLeftAxis(int port){
+    public double getLeftAxis(int port) {
         return xboxLeft.getRawAxis(port);
     }
 
-    public boolean getLeftButton(int port){
+    public boolean getLeftButton(int port) {
         return xboxLeft.getRawButton(port);
     }
 }

src/main/java/frc/robot/Robot.java

@@ -11,6 +11,7 @@
 import frc.robot.constants.TimeConstants;
 import frc.robot.subsystems.ElevatorSubsystem;
 import frc.robot.subsystems.ExampleSubsystem;
+import frc.robot.subsystems.IntakeSubsystem;
 
 
 public class Robot extends TimedRobot {
@@ -29,6 +30,7 @@ public class Robot extends TimedRobot {
 
     //    Subsystem instances
     public static ElevatorSubsystem elevatorSubsytem = new ElevatorSubsystem();
+    public static IntakeSubsystem intakeSubsystem = new IntakeSubsystem();
     public static ExampleSubsystem m_subsystem = new ExampleSubsystem();
 
     @Override

src/main/java/frc/robot/commands/ElevatorCommand.java

@@ -21,43 +21,36 @@ public static enum levelHeight {
     }
 
     public ElevatorCommand(levelHeight level) {
-        requires(Robot.elevatorSubsytem);
+        this.currentLevel = level;
     }
 
-    // Called just before this Command runs the first time
     @Override
     protected void initialize() {
         Robot.elevatorSubsytem.config();
     }
 
-    // Called repeatedly when this Command is scheduled to run
     @Override
     protected void execute() {
-        Robot.elevatorSubsytem.setSpeed(Robot.m_oi.getLeftAxis(1));
-        // if (currentLevel == levelHeight.LEVEL_1){
-        //     this.rotation = Robot.physicsConstants.elevatorLevelOneR;
-        // } else if (currentLevel == levelHeight.LEVEL_2) {
-        //     this.rotation = Robot.physicsConstants.elevatorLevelTwoR;
-        // } else if (currentLevel == levelHeight.LEVEL_3) {
-        //     this.rotation = Robot.physicsConstants.elevatorLevelThreeR;
-        // }
-
-        // Robot.elevatorSubsytem.setRotation(rotation);
+         if (currentLevel == levelHeight.LEVEL_1){
+             this.rotation = Robot.physicsConstants.elevatorLevelOneR;
+         } else if (currentLevel == levelHeight.LEVEL_2) {
+             this.rotation = Robot.physicsConstants.elevatorLevelTwoR;
+         } else if (currentLevel == levelHeight.LEVEL_3) {
+             this.rotation = Robot.physicsConstants.elevatorLevelThreeR;
+         }
+
+         Robot.elevatorSubsytem.setRotation(rotation);
     }
 
-    // Make this return true when this Command no longer needs to run execute()
     @Override
     protected boolean isFinished() {
         return false;
     }
 
-    // Called once after isFinished returns true
     @Override
     protected void end() {
     }
 
-    // Called when another command which requires one or more of the same
-    // subsystems is scheduled to run
     @Override
     protected void interrupted() {
     }

src/main/java/frc/robot/commands/ElevatorRawCommand.java

@@ -0,0 +1,39 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 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.commands;
+
+import edu.wpi.first.wpilibj.command.Command;
+import frc.robot.Robot;
+
+public class ElevatorRawCommand extends Command {
+    public ElevatorRawCommand() {
+
+    }
+
+    @Override
+    protected void initialize() {
+    }
+
+    @Override
+    protected void execute() {
+        Robot.elevatorSubsytem.setSpeed(Robot.m_oi.getLeftAxis(1));
+    }
+
+    @Override
+    protected boolean isFinished() {
+        return false;
+    }
+
+    @Override
+    protected void end() {
+    }
+
+    @Override
+    protected void interrupted() {
+    }
+}

src/main/java/frc/robot/constants/PortConstants.java

@@ -18,6 +18,9 @@ public class PortConstants implements Constants {
     public int pIFRLeft = 0;
     public int pIFRRight = 1;
 
+//    Solenoid Node ID
+    public int pHatchPanel = 1;
+
     @Override
     public void refresh() {
 //        CAN port
@@ -34,6 +37,9 @@ public void refresh() {
 //        DigitalInput port
         this.pIFRLeft = (int) SmartDashboard.getNumber("/Port/pIFRLeft", this.pIFRLeft);
         this.pIFRRight = (int) SmartDashboard.getNumber("/Port/pIFRRight", this.pIFRRight);
+
+//        Solenoid Node ID
+        this.pHatchPanel = (int) SmartDashboard.getNumber("/Port/pHatchPanel", this.pHatchPanel);
     }
 
     @Override
@@ -52,6 +58,9 @@ public void set() {
 //        DigitalInput Port
         SmartDashboard.putNumber("/Port/pIFRLeft", pIFRLeft);
         SmartDashboard.putNumber("/Port/pIFRRight", pIFRRight);
+
+//      Solenoid Node ID
+        SmartDashboard.putNumber("/Port/pHatchPanel", pHatchPanel);
     }
 
     @Override

src/main/java/frc/robot/subsystems/IntakeSubsystem.java

@@ -7,33 +7,32 @@
 
 package frc.robot.subsystems;
 
-import edu.wpi.first.wpilibj.command.Subsystem;
-import frc.robot.Robot;
-
 import com.ctre.phoenix.motorcontrol.ControlMode;
 import com.ctre.phoenix.motorcontrol.FeedbackDevice;
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;
+import edu.wpi.first.wpilibj.Solenoid;
+import edu.wpi.first.wpilibj.command.Subsystem;
+import frc.robot.Robot;
+
 
-/**
- * An example subsystem.  You can replace me with your own Subsystem.
- */
 public class IntakeSubsystem extends Subsystem {
-    // Put methods for controlling this subsystem
-    // here. Call these from Commands.
-    private WPI_VictorSPX intakeMacin; //victor1 openloop Mac
-    private WPI_VictorSPX intakeAttrition;// victor2 openloop attrition
-    private WPI_TalonSRX intakeMotor;// talon1 closeloop
+
+    private WPI_VictorSPX intakeMacin; // Victor1 Open-loop Macin
+    private WPI_VictorSPX intakeAttrition;// Victor2 Open-loop Attrition
+    private WPI_TalonSRX intakeMotor;// Talon1 Closed-loop
+    private Solenoid hatchSolenoid;
 
     public IntakeSubsystem() {
         intake_config();
     }
 
     public void intake_config() {
-        intakeAttrition = new WPI_VictorSPX(Robot.portConstants.pIntakeSpin); //attrition pip control
-        intakeMacin = new WPI_VictorSPX(Robot.portConstants.pIntakeUp);//intake_macin control up
-        intakeMotor = new WPI_TalonSRX(Robot.portConstants.pIntakeDown);//intakeMotor contorl down
+        intakeAttrition = new WPI_VictorSPX(Robot.portConstants.pIntakeSpin);
+        intakeMacin = new WPI_VictorSPX(Robot.portConstants.pIntakeUp);
+        intakeMotor = new WPI_TalonSRX(Robot.portConstants.pIntakeDown);
+        hatchSolenoid = new Solenoid(Robot.portConstants.pHatchPanel);
 
 //      Main talon controller programmed with mag encoder
         intakeMotor.configFactoryDefault();
@@ -66,17 +65,25 @@ public void intake_config() {
         intakeMacin.setInverted(Robot.physicsConstants.macinInvert);
         intakeAttrition.setInverted(!Robot.physicsConstants.macinInvert);
 
-        // Test
     }
 
-    public void setRotation(double targetRotation) {
+    public void setIntakeRotation(double targetRotation) {
         intakeMotor.set(ControlMode.MotionMagic, targetRotation * 4096);
     }
 
-    public void setSpeed(double targetSpeed) {
+    public void setIntakeSpeed(double targetSpeed) {
         intakeMotor.set(ControlMode.PercentOutput, targetSpeed);
     }
 
+    public void setIntakeUDSpeed(double targetSpeedUp, double targerSpeedDown) {
+        intakeMacin.set(ControlMode.PercentOutput, targetSpeedUp);
+        intakeAttrition.set(ControlMode.PercentOutput, targerSpeedDown);
+    }
+
+    public void setHatchSolenoid(boolean forward){
+        hatchSolenoid.set(forward);
+    }
+
     public void resetEncoder() {
         intakeMotor.setSelectedSensorPosition(0, 0, Robot.timeConstants.kTimeOutMs);
     }