Merge pull request #97 from RobotGirls/issue-90-intakeTransportMechanism

Issue 90 intake transport mechanism
RobotGirls · Nov 18, 2023 · 2f1e866 · 2f1e866
2 parents ffcfdf2 + 2657dfc
commit 2f1e866
Showing 1 changed file with 31 additions and 1 deletion.
diff --git a/2024/opmodes/TwoStickTeleop.java b/2024/opmodes/TwoStickTeleop.java
@@ -48,6 +48,7 @@
 import team25core.TwoStickMechanumControlScheme;
 import team25core.TeleopDriveTask;
 
+
 @TeleOp(name = "TwoStickTeleop")
 //@Disabled
 public class TwoStickTeleop extends StandardFourMotorRobot {
@@ -77,6 +78,8 @@ private enum Direction {
     private DcMotor hangingMotor;
     private DcMotor liftMotor;
 
+    private DcMotor intakeMotor;
+    private DcMotor transportMotor;
     private boolean currentlySlow = false;
 
     MecanumFieldCentricDriveScheme scheme;
@@ -95,6 +98,10 @@ public void init() {
 
         //mechanisms
         hangingMotor = hardwareMap.get(DcMotor.class,"hangingMotor");
+
+        intakeMotor =  hardwareMap.get(DcMotor.class,"intakeMotor");
+        transportMotor  =  hardwareMap.get(DcMotor.class,"transportMotor");
+
         clawServo = hardwareMap.servo.get("clawServo");
         liftMotor = hardwareMap.get(DcMotor.class,"liftMotor");
 
@@ -120,6 +127,12 @@ public void init() {
         hangingMotor.setPower(0.75);
         liftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
 
+        intakeMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        intakeMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+
+        transportMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        transportMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+
         frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
         frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
         backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
@@ -136,7 +149,6 @@ public void init() {
         // since the gamesticks were switched for some reason and we need to do
         // more investigation
         drivetask = new TeleopDriveTask(this, scheme, frontLeft, frontRight, backLeft, backRight);
-
     }
 
     public void initIMU()
@@ -181,6 +193,7 @@ public void handleEvent(RobotEvent e) {
         });
 
         //Gamepad 2
+
         this.addTask(new GamepadTask(this, GamepadTask.GamepadNumber.GAMEPAD_2) {
             public void handleEvent(RobotEvent e) {
                 GamepadEvent gamepadEvent = (GamepadEvent) e;
@@ -206,6 +219,23 @@ public void handleEvent(RobotEvent e) {
                         // set arm to extend to its highest capacity to lift robot
                         hangingMotor.setTargetPosition(HANGING_FULLY_RETRACTED);
                         break;
+
+                    case BUTTON_X_DOWN:
+                        intakeMotor.setPower(-1);
+                        transportMotor.setPower(1);
+                        // intake pixels into robot
+                        break;
+                    case LEFT_BUMPER_DOWN:
+                        intakeMotor.setPower(0);
+                        transportMotor.setPower(0);
+                        // stops pixel motor
+                        break;
+
+                    case BUTTON_B_DOWN:
+                        intakeMotor.setPower(1);
+                        transportMotor.setPower(-1);
+                        // outtakes pixels out of robot
+                        break;
                     default:
                         buttonTlm.setValue("Not Moving");
                         break;