v1.0.0

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

@@ -1,25 +1,20 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-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;
 
 import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.buttons.Button;
 import edu.wpi.first.wpilibj.buttons.JoystickButton;
-import frc.robot.commands.ElevatorCommand.levelHeight;
-import frc.robot.commands.ElevatorCommand;
+import frc.robot.commands.HatchPanelCommand;
+import frc.robot.commands.IntakeBallCommand;
+import frc.robot.commands.IntakeBallReleaseCommand;
+import frc.robot.commands.IntakeSpinCP;
 
-/**
- * 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.
- */
 public class OI {
+    public boolean finish = false;
     public Joystick xboxLeft = new Joystick(0);
     public Joystick xboxRight = new Joystick(1);
+    public int revertDT = 0;
+    public int elevatorPosition = 0;
 
     public Button buttonA1 = new JoystickButton(xboxLeft, 1),
             buttonB1 = new JoystickButton(xboxLeft, 2),
@@ -40,9 +35,13 @@ public class OI {
             buttonStart2 = new JoystickButton(xboxRight, 8);
 
     public OI() {
-        // buttonA1.whenPressed(new ElevatorCommand(levelHeight.LEVEL_1));
-        // buttonB1.whenPressed(new ElevatorCommand(levelHeight.LEVEL_2));
-        // buttonX1.whenPressed(new ElevatorCommand(levelHeight.LEVEL_3));
+        buttonA1.whileHeld(new IntakeBallReleaseCommand(-0.5, -0.5));
+        buttonB1.whileHeld(new IntakeBallCommand(0.5,0.5));
+
+        buttonA2.toggleWhenPressed(new IntakeSpinCP(-5000));
+        buttonB2.toggleWhenPressed(new IntakeSpinCP(0));
+        buttonY2.toggleWhenPressed(new IntakeSpinCP(30000));
+        buttonX2.toggleWhenPressed(new HatchPanelCommand());
     }
 
     public double getLeftAxis(int port) {
@@ -52,4 +51,22 @@ public double getLeftAxis(int port) {
     public boolean getLeftButton(int port) {
         return xboxLeft.getRawButton(port);
     }
+
+    public double getRightAxis(int port) {
+        return xboxRight.getRawAxis(port);
+    }
+
+    public boolean getRightButton(int port) {
+        return xboxRight.getRawButton(port);
+    }
+
+    public boolean getDrivetrainRevert() {
+        if (xboxLeft.getRawButton(1)) {
+            Timer.delay(0.02);
+            if (!xboxLeft.getRawButton(1)) {
+                revertDT++;
+            }
+        }
+        return revertDT % 2 != 0;
+    }
 }

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

@@ -3,9 +3,7 @@
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.command.Scheduler;
 import frc.robot.commands.BackGroundCommand;
-import frc.robot.commands.ElevatorCommand;
-import frc.robot.commands.ElevatorCommand.levelHeight;
-import frc.robot.commands.ElevatorRawCommand;
+import frc.robot.commands.MainCommandGroup;
 import frc.robot.constants.PIDConstants;
 import frc.robot.constants.PhysicsConstants;
 import frc.robot.constants.PortConstants;
@@ -20,20 +18,19 @@ public class Robot extends TimedRobot {
 
     //    Command instances
     public BackGroundCommand backGroundCommand = new BackGroundCommand();
-    public ElevatorCommand elevatorCommand = new ElevatorCommand(levelHeight.LEVEL_1);
+    public MainCommandGroup mainCommandGroup = new MainCommandGroup();
 
     //    Constants objects
     public static TimeConstants timeConstants = new TimeConstants();
     public static PhysicsConstants physicsConstants = new PhysicsConstants();
     public static PortConstants portConstants = new PortConstants();
     public static PIDConstants elevatorPID = new PIDConstants("Elevator", 0.5, 0, 0);
-    public static PIDConstants intakePID = new PIDConstants("Intake", 0.5, 0, 0);
+    public static PIDConstants intakePID = new PIDConstants("Intake", 0, 0, 0);
     public static PIDConstants dtLeftPID = new PIDConstants("dtLeft", 0.5, 0, 0);
     public static PIDConstants dtRightPID = new PIDConstants("dtRight", 0.5, 0, 0);
 
     //    Subsystem instances
     public static ElevatorSubsystem elevatorSubsytem = new ElevatorSubsystem();
-    public static ElevatorRawCommand elevatorRawCommand = new ElevatorRawCommand();
     public static IntakeSubsystem intakeSubsystem = new IntakeSubsystem();
     public static DrivetrainSubsystem drivetrainSubsystem = new DrivetrainSubsystem();
 
@@ -42,6 +39,9 @@ public void robotInit() {
         m_oi = new OI();
         backGroundCommand.setRunWhenDisabled(true);
         backGroundCommand.start();
+
+        elevatorSubsytem.resetEncoder();
+        intakeSubsystem.resetEncoder();
     }
 
     @Override
@@ -60,6 +60,7 @@ public void disabledPeriodic() {
 
     @Override
     public void autonomousInit() {
+        mainCommandGroup.start();
     }
 
     @Override
@@ -69,7 +70,10 @@ public void autonomousPeriodic() {
 
     @Override
     public void teleopInit() {
-        elevatorRawCommand.start();
+        mainCommandGroup.start();
+
+        elevatorSubsytem.resetEncoder();
+        intakeSubsystem.resetEncoder();
     }
 
     @Override

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

@@ -11,51 +11,49 @@
 import frc.robot.Robot;
 
 public class BackGroundCommand extends Command {
-    int refreshCount = 0;
 
     public BackGroundCommand() {
 
     }
 
-    // Called just before this Command runs the first time
     @Override
     protected void initialize() {
         Robot.timeConstants.set();
         Robot.physicsConstants.set();
         Robot.portConstants.set();
         Robot.elevatorPID.set();
+        Robot.intakePID.set();
     }
 
-    // Called repeatedly when this Command is scheduled to run
     @Override
     protected void execute() {
         Robot.timeConstants.refresh();
         Robot.physicsConstants.refresh();
         Robot.portConstants.refresh();
         Robot.elevatorPID.refresh();
+        Robot.intakePID.refresh();
 
-//        if (refreshCount >= 100) {
-//            timeConstants.print_value();
-//            physicsConstants.print_value();
-//            refreshCount = 0;
-//        }
-//
-//        refreshCount++;
+        Robot.elevatorSubsytem.getSpeed();
+        Robot.intakeSubsystem.getSpeed();
+        Robot.elevatorSubsytem.getPosition();
+        Robot.intakeSubsystem.getPosition();
+
+        if (Robot.m_oi.getLeftButton(5))
+            Robot.elevatorSubsytem.resetEncoder();
+
+        if (Robot.m_oi.getLeftButton(6))
+            Robot.intakeSubsystem.resetEncoder();
     }
 
-    // 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/ElevatorCP.java

@@ -0,0 +1,64 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.command.Command;
+import frc.robot.Robot;
+
+public class ElevatorCP extends Command {
+    double targetPosition = 0;
+    double currentPosition;
+
+    public ElevatorCP() {
+    }
+
+    @Override
+    protected void initialize() {
+        Robot.elevatorSubsytem.config();
+    }
+
+    @Override
+    protected void execute() {
+        if (Robot.m_oi.getRightButton(5) && Robot.m_oi.elevatorPosition > 0) {
+            Robot.m_oi.elevatorPosition--;
+        }
+
+        if (Robot.m_oi.getRightButton(6) && Robot.m_oi.elevatorPosition < 3) {
+            Robot.m_oi.elevatorPosition++;
+        }
+
+        if (Robot.m_oi.elevatorPosition == 0){
+            targetPosition = -33073;
+        } else if(Robot.m_oi.elevatorPosition == 1){
+            targetPosition = -40000;
+        } else if (Robot.m_oi.elevatorPosition == 2){
+            targetPosition = -50000;
+        } else if(Robot.m_oi.elevatorPosition == 3){
+            targetPosition = -60000;
+        } else {
+            targetPosition = 0;
+        }
+
+        currentPosition = Robot.elevatorSubsytem.getPosition();
+        if (currentPosition < targetPosition - 200) {
+            Robot.elevatorSubsytem.setSpeed(-0.3);
+        } else if (currentPosition > targetPosition + 200) {
+            Robot.elevatorSubsytem.setSpeed(0.3);
+        } else {
+            Robot.elevatorSubsytem.setSpeed(0);
+        }
+    }
+
+    @Override
+    protected boolean isFinished() {
+        return false;
+    }
+
+    @Override
+    protected void end() {
+        Robot.elevatorSubsytem.setSpeed(0);
+    }
+
+    @Override
+    protected void interrupted() {
+        end();
+    }
+}

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

@@ -22,7 +22,11 @@ protected void initialize() {
 
     @Override
     protected void execute() {
-        Robot.elevatorSubsytem.setSpeed(Robot.m_oi.getLeftAxis(1));
+        double input = Robot.m_oi.getRightAxis(3) - Robot.m_oi.getRightAxis(2);
+        if (input < 0.1 && input > -0.1) {
+            Robot.elevatorSubsytem.setSpeed(0.05);
+        }
+        Robot.elevatorSubsytem.setSpeed(input);
     }
 
     @Override
@@ -32,9 +36,11 @@ protected boolean isFinished() {
 
     @Override
     protected void end() {
+        Robot.elevatorSubsytem.setSpeed(0);
     }
 
     @Override
     protected void interrupted() {
+        end();
     }
 }

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

@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* 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 HatchPanelCommand extends Command {
+    public HatchPanelCommand() {
+    }
+
+    @Override
+    protected void initialize() {
+    }
+
+    @Override
+    protected void execute() {
+        Robot.intakeSubsystem.setHatchSolenoid(true);
+    }
+
+    @Override
+    protected boolean isFinished() {
+        return false;
+    }
+
+    @Override
+    protected void end() {
+        Robot.intakeSubsystem.setHatchSolenoid(false);
+    }
+
+    @Override
+    protected void interrupted() {
+        end();
+    }
+}

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

@@ -10,36 +10,24 @@
 import edu.wpi.first.wpilibj.command.Command;
 import frc.robot.Robot;
 
-public class ElevatorCommand extends Command {
-    public double rotation = 0;
-    public levelHeight currentLevel;
-
-    public static enum levelHeight {
-        LEVEL_1,
-        LEVEL_2,
-        LEVEL_3
-    }
+public class IntakeBallCommand extends Command {
+    double upSpeed = 0;
+    double downSpeed = 0;
+
 
-    public ElevatorCommand(levelHeight level) {
-        this.currentLevel = level;
+    public IntakeBallCommand(double upSpeed, double downSpeed) {
+        this.downSpeed = downSpeed;
+        this.upSpeed = upSpeed;
     }
 
     @Override
     protected void initialize() {
-        Robot.elevatorSubsytem.config();
+        Robot.intakeSubsystem.intake_config();
     }
 
     @Override
     protected void execute() {
-         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);
+        Robot.intakeSubsystem.setIntakeUDSpeed(upSpeed, downSpeed);
     }
 
     @Override
@@ -49,9 +37,11 @@ protected boolean isFinished() {
 
     @Override
     protected void end() {
+        Robot.intakeSubsystem.setIntakeUDSpeed(0, 0);
     }
 
     @Override
     protected void interrupted() {
+        end();
     }
 }