turned up speed on arm

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Common/Robot.java

@@ -74,16 +74,16 @@ public class Robot {
     public static final int COUNTS_PER_REVOLUTION = 1000;
 
     public DcMotor driveWarehouseLeft, driveWarehouseRight, driveGoalLeft, driveGoalRight,
-            collectorArmRiser, collectorArmBobbin, depositorArmRiser, depositorArmBobbin;
+            orangeArmRiser, orangeArmBobbin, whiteArmRiser, whiteArmBobbin;
 
     // Collector
-    public Servo collectorDispenser;
-    public CRServo turretServo1;
+    public Servo orangeDispenser;
+    public CRServo turretServoOrange;
 
     // Depositor
-    public Servo depositorDispenser;
+    public Servo whiteDispenser;
     // Depositor, and carousel
-    public CRServo turretServo2, carouselWheel, collectorOrange, collectorWhite;
+    public CRServo turretServoWhite, carouselWheel, collectorOrange, collectorWhite;
 
 
 
@@ -207,24 +207,24 @@ public class Robot {
     }
 
     private void initCollector() {
-        collectorArmBobbin = engine.hardwareMap.dcMotor.get("collectorArmBobbin");
-        collectorDispenser = engine.hardwareMap.servo.get("collectorDispenser");
-        collectorArmBobbin.setDirection(DcMotorSimple.Direction.FORWARD);
-        turretServo1 = engine.hardwareMap.crservo.get("turretServo1");
-        collectorArmRiser = engine.hardwareMap.dcMotor.get("collectorArmRiser");
-        collectorArmRiser.setDirection(DcMotorSimple.Direction.FORWARD);
-        collectorArmRiser.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        orangeArmBobbin = engine.hardwareMap.dcMotor.get("orangeArmBobbin");
+        orangeDispenser = engine.hardwareMap.servo.get("orangeDispenser");
+        orangeArmBobbin.setDirection(DcMotorSimple.Direction.FORWARD);
+        turretServoOrange = engine.hardwareMap.crservo.get("turretServoOrange");
+        orangeArmRiser = engine.hardwareMap.dcMotor.get("orangeArmRiser");
+        orangeArmRiser.setDirection(DcMotorSimple.Direction.FORWARD);
+        orangeArmRiser.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
         collectorOrange = engine.hardwareMap.crservo.get("collectorOrange");
     }
 
     private void initDepositor(){
-        depositorDispenser = engine.hardwareMap.servo.get("depositorDispenser");
-        depositorArmBobbin = engine.hardwareMap.dcMotor.get("depositorArmBobbin");
-        depositorArmBobbin.setDirection(DcMotorSimple.Direction.REVERSE);
-        turretServo2 = engine.hardwareMap.crservo.get("turretServo2");
-        depositorArmRiser = engine.hardwareMap.dcMotor.get("depositorArmRiser");
-        depositorArmRiser.setDirection(DcMotorSimple.Direction.FORWARD);
-        depositorArmRiser.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        whiteDispenser = engine.hardwareMap.servo.get("whiteDispenser");
+        whiteArmBobbin = engine.hardwareMap.dcMotor.get("whiteArmBobbin");
+        whiteArmBobbin.setDirection(DcMotorSimple.Direction.REVERSE);
+        turretServoWhite = engine.hardwareMap.crservo.get("turretServoWhite");
+        whiteArmRiser = engine.hardwareMap.dcMotor.get("whiteArmRiser");
+        whiteArmRiser.setDirection(DcMotorSimple.Direction.FORWARD);
+        whiteArmRiser.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
         collectorWhite = engine.hardwareMap.crservo.get("collectorWhite");
     }
 

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/TeleOp/States/TeleOpState.java

@@ -8,18 +8,24 @@ import org.timecrafters.FreightFrenzy.Competition.Common.Robot;
 public class TeleOpState extends CyberarmState {
     Robot robot;
     double maxDriveSpeed, maxCollectorArmSpeed, maxDepositorArmSpeed;
+    int maxArmTravelDistance;
 
 
     public TeleOpState(Robot robot) {
         this.robot = robot;
      maxDriveSpeed = 1;
-     maxCollectorArmSpeed = 0.7;
-     maxDepositorArmSpeed = 0.7;
+     maxCollectorArmSpeed = 0.1;
+     maxDepositorArmSpeed = 0.1;
+     maxArmTravelDistance = 4000;
     }
 
     @Override
     public void start() {
         super.start();
+        // FIXME: Don't reset when doing autonomous stuff
+        // FIXME: Fix unable to retract if reset mid-match
+        robot.whiteArmBobbin.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.whiteArmBobbin.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
     }
 
     @Override
@@ -35,18 +41,18 @@ public class TeleOpState extends CyberarmState {
 
         // dispenser powered
         if (engine.gamepad1.b){
-            robot.collectorDispenser.setPosition(.5);
+            robot.orangeDispenser.setPosition(.5);
         }
 
         // if not pressed dispenser off
         else {
-            robot.collectorDispenser.setPosition(0);
+            robot.orangeDispenser.setPosition(0);
         }
         //  if one of triggers pressed arm extends or unextends
-        robot.collectorArmBobbin.setPower(engine.gamepad1.left_trigger * maxCollectorArmSpeed);
+        robot.orangeArmBobbin.setPower(engine.gamepad1.left_trigger * maxCollectorArmSpeed);
 
         if (engine.gamepad1.left_trigger <= 0){
-            robot.collectorArmBobbin.setPower(-engine.gamepad1.right_trigger * maxCollectorArmSpeed);
+            robot.orangeArmBobbin.setPower(-engine.gamepad1.right_trigger * maxCollectorArmSpeed);
         }
 
         //  if either of these buttons... move the servo
@@ -54,33 +60,33 @@ public class TeleOpState extends CyberarmState {
         if (engine.gamepad1.dpad_right || engine.gamepad1.dpad_left) {
 
             if (engine.gamepad1.dpad_right) {
-                robot.turretServo1.setPower(-1);
+                robot.turretServoOrange.setPower(-1);
             }
 
             if (engine.gamepad1.dpad_left) {
-                robot.turretServo1.setPower(1);
+                robot.turretServoOrange.setPower(1);
             }
         }
 
         //  if neither of these buttons... power off
         else {
-            robot.turretServo1.setPower(0);
+            robot.turretServoOrange.setPower(0);
         }
 
         // if dpad verticles pressed arm rises or lowers...
         if (engine.gamepad1.dpad_up || engine.gamepad1.dpad_down) {
 
             if (engine.gamepad1.dpad_up) {
-                robot.collectorArmRiser.setPower(1);
+                robot.orangeArmRiser.setPower(1);
             }
 
             if (engine.gamepad1.dpad_down) {
-                robot.collectorArmRiser.setPower(-1);
+                robot.orangeArmRiser.setPower(-1);
             }
         }
         // nothing pressed nothing move...
         else {
-            robot.collectorArmRiser.setPower(0);
+            robot.orangeArmRiser.setPower(0);
         }
 
         // if bumpers pressed intake wheel move...
@@ -112,19 +118,27 @@ public class TeleOpState extends CyberarmState {
         // GamePad 2
 
         // if triggers are pressed then arm extends or unextends
-        robot.depositorArmBobbin.setPower(engine.gamepad2.left_trigger * maxDepositorArmSpeed);
+        if (robot.whiteArmBobbin.getCurrentPosition() <= maxArmTravelDistance && engine.gamepad2.left_trigger > 0) {
+
+            robot.whiteArmBobbin.setPower(engine.gamepad2.left_trigger * maxDepositorArmSpeed);
+        } else {
+            if (engine.gamepad2.left_trigger <= 0 && robot.whiteArmBobbin.getCurrentPosition() >= 0) {
+                robot.whiteArmBobbin.setPower(-engine.gamepad2.right_trigger * maxDepositorArmSpeed);
+            } else {
+                robot.whiteArmBobbin.setPower(0);
+            }
 
-        if (engine.gamepad2.left_trigger <= 0) {
-            robot.depositorArmBobbin.setPower(-engine.gamepad2.right_trigger * maxDepositorArmSpeed);
         }
 
+
+
         // if b is pressed then depositor door opens, if not pressed not opening.
         if (engine.gamepad2.b){
-            robot.depositorDispenser.setPosition(.5);
+            robot.whiteDispenser.setPosition(.5);
         }
 
         else {
-            robot.depositorDispenser.setPosition(0);
+            robot.whiteDispenser.setPosition(0);
         }
 
         //  if either of these buttons move the servo
@@ -132,32 +146,32 @@ public class TeleOpState extends CyberarmState {
         if (engine.gamepad2.dpad_right || engine.gamepad2.dpad_left) {
 
             if (engine.gamepad2.dpad_right) {
-                robot.turretServo2.setPower(-1);
+                robot.turretServoWhite.setPower(-1);
             }
 
             if (engine.gamepad2.dpad_left) {
-                robot.turretServo2.setPower(1);
+                robot.turretServoWhite.setPower(1);
             }
         }
         //  if neither of these buttons power off
         else {
-            robot.turretServo2.setPower(0);
+            robot.turretServoWhite.setPower(0);
         }
 
         // if dpad verticles pressed arm rises or lowers
         if (engine.gamepad2.dpad_down || engine.gamepad2.dpad_up) {
 
             if (engine.gamepad2.dpad_up) {
-                robot.depositorArmRiser.setPower(1);
+                robot.whiteArmRiser.setPower(-1);
             }
 
             if (engine.gamepad2.dpad_down) {
-                robot.depositorArmRiser.setPower(-1);
+                robot.whiteArmRiser.setPower(1);
             }
         }
         // nothing pressed nothing move...
         else {
-            robot.depositorArmRiser.setPower(0);
+            robot.whiteArmRiser.setPower(0);
         }
 
         if (engine.gamepad2.left_bumper || engine.gamepad2.right_bumper) {
@@ -178,8 +192,8 @@ public class TeleOpState extends CyberarmState {
     }
 
 
-
-
-
-
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("white arm extension", robot.whiteArmBobbin.getCurrentPosition());
+    }
 }