Added control to intake and elevator.

src/main/java/com/team766/robot/rookie_bot/OI.java

@@ -30,11 +30,29 @@ public void run(final Context context) {
             "  J0 A1: " + joystick0.getAxis(1) +
             "  J1 A0: " + joystick1.getAxis(0) +
             "  J1 A1: " + joystick1.getAxis(1) +
-            "  J0 B1: " + joystick0.getButton(1) +
-            "  J0 B2: " + joystick0.getButton(2) +
-            "  J0 B3: " + joystick0.getButton(3));
+            "  J0 B1: " + joystick0.getButton(3) +
+            "  J0 B2: " + joystick0.getButton(1) +
+            "  J0 B3: " + joystick0.getButton(2));
 
         Robot.drive.setArcadeDrivePower(joystick0.getAxis(1), joystick0.getAxis(0));
+
+        if (joystick0.getButtonPressed(1)) {
+            Robot.elevator.move(0.3);
+        } else if (joystick0.getButtonPressed(2)) {
+            Robot.elevator.move(-0.3);
+        } else if (joystick0.getButtonReleased(1) || joystick0.getButtonReleased(1)) {
+            Robot.elevator.move(0);
+        }
+
+        if (joystick0.getButtonPressed(3)) {
+            Robot.intake.setPowerBoth(0.3);
+        } else if (joystick0.getButtonPressed(4)) {
+            Robot.intake.setPowerBoth(-0.3);
+        } else if (joystick0.getButtonReleased(3) || joystick0.getButtonReleased(4)) {
+            Robot.intake.setPowerBoth(0);
+        }
+
+
         context.waitFor(() -> RobotProvider.instance.hasNewDriverStationData()); 
         }
 

src/main/java/com/team766/robot/rookie_bot/Robot.java

@@ -3,19 +3,22 @@
 import com.team766.framework.AutonomousMode;
 import com.team766.framework.Procedure;
 import com.team766.hal.RobotConfigurator;
-import com.team766.robot.gatorade.mechanisms.Elevator;
+import com.team766.robot.rookie_bot.mechanisms.Elevator;
+import com.team766.robot.rookie_bot.mechanisms.Intake;
 import com.team766.robot.rookie_bot.mechanisms.Drive;
 
 public class Robot implements RobotConfigurator {
     // Declare mechanisms (as static fields) here
     public static Drive drive;  
     public static Elevator elevator;
+    public static Intake intake;
 
     @Override
     public void initializeMechanisms() {
         // Initialize mechanisms here
         drive = new Drive();
         elevator = new Elevator();
+        intake = new Intake ();
 
     }
 

src/main/java/com/team766/robot/rookie_bot/mechanisms/Drive.java

@@ -4,6 +4,11 @@
 import com.team766.hal.MotorController;
 import com.team766.hal.RobotProvider;
 
+//Drive_R - ID 3
+//Drive_L - ID 4
+//Right grabber - ID 6
+//Left grabber - ID 5
+
 public class Drive extends Mechanism {
     private MotorController leftMotor;
     private MotorController rightMotor;

src/main/java/com/team766/robot/rookie_bot/mechanisms/Elevator → src/main/java/com/team766/robot/rookie_bot/mechanisms/Elevator.java

@@ -1,21 +1,33 @@
 package com.team766.robot.rookie_bot.mechanisms;
 
+import com.revrobotics.CANSparkMax;
 import com.team766.framework.Mechanism;
+import com.team766.hal.EncoderReader;
 import com.team766.hal.MotorController;
 import com.team766.hal.RobotProvider;
+import com.team766.hal.wpilib.CANSparkMaxMotorController;
 
 public class Elevator extends Mechanism{
     private MotorController m_elevator;
-    private EncoderReader m_elevatorEncoder
+    private EncoderReader m_elevatorEncoder;
+    private final double TOP_LIMIT = 400;
+    private final double BOTTOM_LIMIT = 0;
+
 
     public Elevator() {
         m_elevator = RobotProvider.instance.getMotor("elevator");
         m_elevatorEncoder = RobotProvider.instance.getEncoder("elevator_encoder");
+        ((CANSparkMaxMotorController) m_elevator).setSmartCurrentLimit(10, 80, 200);
         resetEncoder();
     }
 
     public void move(double power) {
-        m_elevator.set(power);
+        if (!((power > 0 && getElevatorDistance() > TOP_LIMIT) || (power < 0 && getElevatorDistance() < BOTTOM_LIMIT))) {
+            m_elevator.set(power);
+        } else {
+            m_elevator.set(0);
+        }
+
     }
 
     public double getElevatorDistance(){

src/main/java/com/team766/robot/rookie_bot/mechanisms/Intake.java

@@ -0,0 +1,50 @@
+package com.team766.robot.rookie_bot.mechanisms;
+
+import com.revrobotics.CANSparkMax;
+import com.team766.framework.Context;
+import com.team766.framework.LaunchedContext;
+import com.team766.framework.Mechanism;
+import com.team766.framework.Procedure;
+import com.team766.hal.JoystickReader;
+import com.team766.hal.MotorController;
+import com.team766.hal.RobotProvider;
+import com.team766.logging.Category;
+import com.team766.robot.rookie_bot.procedures.*;
+
+public class Intake extends Mechanism {
+    private MotorController intakeChainLeft;
+    private MotorController intakeChainRight;
+    private final double INNER_LIMIT = 20;
+    private final double OUTER_LIMIT = -400;
+
+    public Intake() {
+        intakeChainLeft = RobotProvider.instance.getMotor("intake.ChainLeft");
+        intakeChainRight = RobotProvider.instance.getMotor("intake.ChainRight");
+        ((CANSparkMax) intakeChainLeft).setSmartCurrentLimit(10, 80, 200);
+        ((CANSparkMax) intakeChainRight).setSmartCurrentLimit(10, 80, 200);
+
+        intakeChainLeft.setSensorPosition(0);
+        intakeChainRight.setSensorPosition(0);
+
+    }
+
+    public void setIntake(double leftPower, double rightPower) {
+        if (!((leftPower > 0 && intakeChainLeft.getSensorPosition() > INNER_LIMIT) || (leftPower < 0 && intakeChainLeft.getSensorPosition() < OUTER_LIMIT))) {
+            intakeChainLeft.set(leftPower);
+        } else {
+            intakeChainLeft.set(0);
+        }
+
+        if (!((rightPower > 0 && intakeChainLeft.getSensorPosition() > INNER_LIMIT) || (rightPower < 0 && intakeChainLeft.getSensorPosition() < OUTER_LIMIT))) {
+            intakeChainRight.set(rightPower);
+        } else {
+            intakeChainRight.set(0);
+        }
+    }
+
+    public void setPowerBoth(double powerBoth) {
+        setIntake(powerBoth, powerBoth);
+    }
+
+}
+

src/main/java/com/team766/robot/swerveandshoot/procedures/PIDElevator → src/main/java/com/team766/robot/rookie_bot/procedures/PIDElevator.java

@@ -1,9 +1,12 @@
-package com.team766.robot.rookie_bot;
+package com.team766.robot.rookie_bot.procedures;
 
+import com.team766.controllers.PIDController;
 import com.team766.framework.AutonomousMode;
+import com.team766.framework.Context;
 import com.team766.framework.Procedure;
 import com.team766.hal.RobotConfigurator;
 import com.team766.robot.gatorade.mechanisms.Elevator;
+import com.team766.robot.rookie_bot.Robot;
 import com.team766.robot.rookie_bot.mechanisms.Drive;
 
 public class PIDElevator extends Procedure{
@@ -22,7 +25,7 @@ public void run(Context context){
         controller.setSetpoint(setpoint); //tell the PID controller our setpoint
         while(!controller.isDone()) //stop the loop when we hit the setpoint
         {
-            controller.calculate(Robot.elevator.getElevatorDistance(), true); //pass ht efeedback into the PID
+            controller.calculate(Robot.elevator.getElevatorDistance()); //pass ht efeedback into the PID
             double motor_effort = controller.getOutput(); //get the PID controller output for this cycle
             Robot.elevator.move(motor_effort); //move the elevator!
         }