finished elevator

Author: XavierCraw

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

@@ -6,6 +6,7 @@
 import com.team766.hal.RobotProvider;
 import com.team766.logging.Category;
 import com.team766.robot.example.procedures.*;
+import com.team766.robot.rookie_bot.procedures.PIDElevator;
 
 /**
  * This class is the glue that binds the controls on the physical operator
@@ -36,6 +37,13 @@ public void run(final Context context) {
 
             Robot.drive.setArcadeDrivePower(-1 * joystick0.getAxis(1), joystick0.getAxis(3));
             context.waitFor(() -> RobotProvider.instance.hasNewDriverStationData());
+            if (joystick0.getButtonPressed(1)) {
+                context.startAsync(new PIDElevator(true));
+            
+            }
+            if (joystick0.getButtonPressed(2)) {
+                context.startAsync(new PIDElevator(false));
+            }
         }
     }
 }

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

@@ -4,14 +4,34 @@
 import com.team766.framework.Context;
 import com.team766.robot.rookie_bot.Robot;
 
-    public class PIDElevator extends Procedure{
+    public class PIDElevator extends Procedure {
         double setpoint;
         PIDController controller;
 
-        public PIDElevator(){
-            this.setpoint = 1;
-     }  
+        public PIDElevator(boolean top) {
+            if (top) {
+                setpoint = 400;
+                
+            } else {
+                setpoint = 0;
+            }
 
-     Public void run (Context context){
-        context.
+        }  
 
+        public void run(Context context) {
+            context.takeOwnership(Robot.elevator);
+            Robot.elevator.resetEncoder(); //clear encoder
+            
+            controller = new PIDController(0, 0, 0, 0, 1, 0.01); // values a P I D, min. max. and threshold
+            controller.setSetpoint(setpoint); //tell our PID contoller our setpoint
+            
+            while (!controller.isDone()) { //stop loop when we get to setpoint
+                controller.calculate(Robot.elevator.getElevatorDistance()); //pass the feedback into the PID
+                double motor_effort = controller.getOutput(); //get the PID controller output for this cycle
+                Robot.elevator.move(motor_effort); //MOVE THE ELEVATOR!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!1 (that 1 was a typo)
+            }
+           
+            Robot.elevator.move(0); //Make sure that the dang elevator stopped moving!!!!!!
+            context.yield();
+        }
+}