diff --git a/.idea/workspace.xml b/.idea/workspace.xml
index fe3b9bf..d627741 100644
--- a/.idea/workspace.xml
+++ b/.idea/workspace.xml
@@ -4,6 +4,10 @@
+
+
+
+
@@ -50,6 +54,30 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -62,17 +90,17 @@
-
-
+
+
-
+
-
-
+
+
@@ -83,8 +111,8 @@
-
-
+
+
@@ -95,16 +123,16 @@
-
-
+
+
-
-
-
+
+
+
@@ -137,29 +165,8 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
@@ -174,6 +181,7 @@
+
@@ -182,16 +190,17 @@
-
-
+
+
-
+
+
@@ -248,6 +257,15 @@
+
+
+
+
+
+
+
+
+
@@ -276,7 +294,7 @@
-
+
@@ -306,12 +324,20 @@
-
+
+
+ 1563354538019
+
+
+
+ 1563354538019
+
+
-
+
@@ -353,6 +379,10 @@
+
+
+
+
@@ -389,16 +419,6 @@
-
-
-
-
-
-
-
-
-
-
@@ -406,13 +426,6 @@
-
-
-
-
-
-
-
@@ -437,6 +450,23 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -446,23 +476,23 @@
-
-
+
+
-
-
+
+
-
-
-
+
+
+
@@ -473,27 +503,37 @@
+
+
+
+
+
+
+
-
-
+
+
-
+
-
-
+
+
+
+
+
-
+
-
-
+
+
diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java
index 12b5f33..3748896 100644
--- a/src/main/java/frc/robot/OI.java
+++ b/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);
}
}
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 063dcf1..37f3ee3 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/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
diff --git a/src/main/java/frc/robot/commands/ElevatorCommand.java b/src/main/java/frc/robot/commands/ElevatorCommand.java
index 9573479..3b9a8be 100644
--- a/src/main/java/frc/robot/commands/ElevatorCommand.java
+++ b/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() {
}
diff --git a/src/main/java/frc/robot/commands/ElevatorRawCommand.java b/src/main/java/frc/robot/commands/ElevatorRawCommand.java
new file mode 100644
index 0000000..a60f302
--- /dev/null
+++ b/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() {
+ }
+}
diff --git a/src/main/java/frc/robot/constants/PortConstants.java b/src/main/java/frc/robot/constants/PortConstants.java
index e27be74..5fff019 100644
--- a/src/main/java/frc/robot/constants/PortConstants.java
+++ b/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
diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
index 9173115..355a939 100644
--- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
+++ b/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);
}