Elevator changes

Author: raghavsial

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

@@ -32,7 +32,7 @@ public void run(final Context context) {
             context.waitFor(() -> RobotProvider.instance.hasNewDriverStationData());
             RobotProvider.instance.refreshDriverStationData();
 
-            Robot.drive.setArcadeDrivePower(joystick0.getAxis(1), joystick0.getAxis(4));
+            Robot.drive.setArcadeDrivePower(joystick0.getAxis(1), joystick0.getAxis(3));
 
             // Add driver controls here - make sure to take/release ownership
             // of mechanisms when appropriate.

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

@@ -5,15 +5,18 @@
 import com.team766.hal.RobotConfigurator;
 import com.team766.robot.example.mechanisms.*;
 import com.team766.robot.rookie_bot.mechanisms.Drive;
+import com.team766.robot.rookie_bot.mechanisms.Elevator;
 
 public class Robot implements RobotConfigurator {
     // Declare mechanisms (as static fields) here
     public static Drive drive;
-    
+    public static Elevator elevator;
+
     @Override
     public void initializeMechanisms() {
         // Initialize mechanisms here
         drive = new Drive();
+        elevator= new Elevator();
     }
 
     @Override

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

@@ -19,8 +19,8 @@ public void drive(double leftPower, double rightPower) {
     }
 
     public void setArcadeDrivePower(double forward, double turn) {
-        double leftMotorPower= turn + forward;
-        double rightMotorPower= -turn + forward;
+        double leftMotorPower= turn - forward;
+        double rightMotorPower= -turn - forward;
         drive(leftMotorPower, rightMotorPower);
 
 

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

@@ -0,0 +1,33 @@
+package.com.team766.robot.rookie_bot.mechanisms;
+
+
+import com.team766.framework.Mechanism;
+import com.team766.hal.RobotProvider;
+import com.team766.hal.MotorController;
+import com.team766.hal.EncoderReader;
+
+public class Elevator extends Mechanism{
+    private MotorController m_elevator;
+    private EncoderReader m_elevatorEncoder;
+
+    public Elevator() {
+        m_elevator = RobotProvider.instance.getMotor("elevator");
+        m_elevatorEncoder = RobotProvider   .instance.getEncoder("elevator_encoder");
+        resetEncoder();
+
+    }
+
+    public void move(double power){
+        m_elevator.set(power);
+    }
+
+    public double getElevatorDistance(){
+        return m_elevatorEncoder.getDistance();
+    }
+
+    public void resetEncoder(){
+        m_elevatorEncoder.reset();
+    }
+
+}
+

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

@@ -1,11 +1,24 @@
-package com.team766.robot.mechanisms;
+package com.team766.robot.rookie_bot.mechanisms;
+
 
 import com.team766.framework.Mechanism;
-import com.team766.hal.SolenoidController;
 import com.team766.hal.RobotProvider;
+import com.team766.hal.SolenoidController;
+
+
+public class Launcher extends Mechanism {
+	private SolenoidController pusher;
+
+
+	public Launcher() {
+		pusher = RobotProvider.instance.getSolenoid("launch");
+	}
+
+
+	public void setPusher(boolean extended) {
+		checkContextOwnership();
 
-public class Launcher extends Mechanism{
-private SolenoidController pusher;
 
-public Launcher()
-}
+		pusher.set(extended);
+	}
+}

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

@@ -0,0 +1,33 @@
+package com.team766.robot.rookie_bot.procedures;
+
+import com.team766.framework.Procedure;
+import com.team766.controllers.PIDController;
+import com.team766.framework.Context;
+import com.team766.robot.rookie_bot.Robot;
+
+public class PIDElevator extends Procedure {
+    double setpoint;
+    PIDController controller;
+
+    public PIDElevator(){
+
+    this.setpoint = 1; 
+    }
+
+    public void run(Context context){
+        context.takeOwnership(Robot.elevator);
+        Robot.elevator.resetEncoder();
+
+        controller = new PIDController(0.001, 0, 0.0001, 0, 1, 0.01);
+        controller.setSetpoint(setpoint);
+        while (!controller.isDone())
+        {
+            controller.calculate(Robot.elevator.getElevatorDistance());
+            double motor_effort = controller.getOutput();
+            Robot.elevator.move(motor_effort);
+        }
+        Robot.elevator.move(0);
+        context.yield();
+    }
+}
+