Merge branch 'branch-rc-chassis-14493' into branch-black-14493

TeamCode/src/main/java/ServoPractice.java

@@ -0,0 +1,27 @@
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.Servo;
+
+public class ServoPractice extends OpMode {
+    //servo
+    Servo test_servo;
+
+
+    public void init(){
+        //hw map
+        test_servo = hardwareMap.get(Servo.class, "test_servo");
+    }
+    public void loop(){
+        //make it move
+        test_servo.setPosition(0);
+        try {
+            Thread.sleep(500);
+        } catch (InterruptedException e) {
+            throw new RuntimeException(e);
+        }
+        test_servo.setPosition(1);
+    }
+    public void stop(){
+
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOmniOpMode_Linear.java

@@ -29,8 +29,11 @@
 
 package org.firstinspires.ftc.teamcode;
 
+<<<<<<< HEAD
+=======
 import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER;
 import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER_DIRECTION;
+>>>>>>> branch-rc-chassis-14493
 import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR;
 import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_DIRECTION;
 import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR;
@@ -39,20 +42,28 @@ import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR;
 import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_DIRECTION;
 import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR;
 import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_DIRECTION;
+<<<<<<< HEAD
+=======
 import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER;
 import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER_DIRECTION;
 import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER;
 import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER_DIRECTION;
+>>>>>>> branch-rc-chassis-14493
 
 import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import com.qualcomm.robotcore.hardware.DcMotor;
+<<<<<<< HEAD
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+=======
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.util.ElapsedTime;
 
 import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
 
+>>>>>>> branch-rc-chassis-14493
 /*
  * This file contains an example of a Linear "OpMode".
  * An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match.
@@ -85,13 +96,27 @@ import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
 public class BasicOmniOpMode_Linear extends LinearOpMode {
 
     // Declare OpMode members for each of the 4 motors.
+<<<<<<< HEAD
+    private ElapsedTime runtime = new ElapsedTime();
+    private DcMotor leftFrontDrive = null;
+    private DcMotor leftBackDrive = null;
+    private DcMotor rightFrontDrive = null;
+    private DcMotor rightBackDrive = null;
+=======
     private final ElapsedTime runtime = new ElapsedTime();
+>>>>>>> branch-rc-chassis-14493
 
     @Override
     public void runOpMode() {
 
         // Initialize the hardware variables. Note that the strings used here must correspond
         // to the names assigned during the robot configuration step on the DS or RC devices.
+<<<<<<< HEAD
+        leftFrontDrive  = hardwareMap.get(DcMotor.class, FRONT_LEFT_MOTOR);
+        leftBackDrive  = hardwareMap.get(DcMotor.class, BACK_LEFT_MOTOR);
+        rightFrontDrive = hardwareMap.get(DcMotor.class, FRONT_RIGHT_MOTOR);
+        rightBackDrive = hardwareMap.get(DcMotor.class, BACK_RIGHT_MOTOR);
+=======
         DcMotor leftFrontDrive = hardwareMap.get(DcMotor.class, FRONT_LEFT_MOTOR);
         DcMotor leftBackDrive = hardwareMap.get(DcMotor.class, BACK_LEFT_MOTOR);
         DcMotor rightFrontDrive = hardwareMap.get(DcMotor.class, FRONT_RIGHT_MOTOR);
@@ -108,6 +133,7 @@ public class BasicOmniOpMode_Linear extends LinearOpMode {
         leftEncoder.setDirection(LEFT_ENCODER_DIRECTION);
         rightEncoder.setDirection(RIGHT_ENCODER_DIRECTION);
         strafeEncoder.setDirection(BACK_ENCODER_DIRECTION);
+>>>>>>> branch-rc-chassis-14493
 
         // ########################################################################################
         // !!!            IMPORTANT Drive Information. Test your motor directions.            !!!!!
@@ -126,9 +152,12 @@ public class BasicOmniOpMode_Linear extends LinearOpMode {
 
         // Wait for the game to start (driver presses START)
         telemetry.addData("Status", "Initialized");
+<<<<<<< HEAD
+=======
         telemetry.addData("Left Encoder Value", leftEncoder.getDeltaPosition());
         telemetry.addData("Right Encoder Value", rightEncoder.getDeltaPosition());
         telemetry.addData("Strafe Encoder Value", strafeEncoder.getDeltaPosition());
+>>>>>>> branch-rc-chassis-14493
         telemetry.update();
 
         waitForStart();
@@ -190,9 +219,12 @@ public class BasicOmniOpMode_Linear extends LinearOpMode {
             telemetry.addData("Status", "Run Time: " + runtime.toString());
             telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower);
             telemetry.addData("Back  left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower);
+<<<<<<< HEAD
+=======
             telemetry.addData("Left Encoder Value", leftEncoder.getDeltaPosition());
             telemetry.addData("Right Encoder Value", rightEncoder.getDeltaPosition());
             telemetry.addData("Strafe Encoder Value", strafeEncoder.getDeltaPosition());
+>>>>>>> branch-rc-chassis-14493
             telemetry.update();
         }
     }}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java

@@ -40,13 +40,14 @@ public class PedroConstants {
     public static final String BACK_ENCODER = "encoder back";
 
     // Robot encoder direction
-    public static final double LEFT_ENCODER_DIRECTION = Encoder.FORWARD;
+    public static final double LEFT_ENCODER_DIRECTION = Encoder.REVERSE;
     public static final double RIGHT_ENCODER_DIRECTION = Encoder.FORWARD;
     public static final double BACK_ENCODER_DIRECTION = Encoder.FORWARD;
 
     /*
         Pedro's parameters
     */
+    // -0.0708
 
     // The weight of the robot in Kilograms
     public static final double ROBOT_WEIGHT_IN_KG = 10.5;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SlideArm.java

@@ -0,0 +1,165 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.Servo;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import org.firstinspires.ftc.robotcontroller.external.samples.UtilityOctoQuadConfigMenu;
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+
+
+@TeleOp(name = "ArmControl")
+public class SlideArm extends OpMode {
+
+
+
+    DcMotor Slide;
+    Servo ClawServo;
+    Servo ArmServo;
+    Servo WristServo;
+    boolean xPressed;
+    boolean yPressed;
+    boolean bPressed;
+    double ticks = 753.2;
+
+
+    public void init(){
+        Slide = hardwareMap.get(DcMotor.class, "SlideMotor");
+        ArmServo = hardwareMap.get(Servo.class,"WristServo");
+        WristServo = hardwareMap.get(Servo.class, "ArmServo");
+        ClawServo = hardwareMap.get(Servo.class, "ClawServo");
+        xPressed = false;
+        yPressed = false;
+        bPressed = false;
+    }
+
+    double power = 0;
+    double position = 1;
+
+    public void loop() {
+        Slide.setPower(-gamepad2.left_stick_y);
+        Slide.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        if (gamepad2.left_bumper) {
+            ClawServo.setPosition(0);
+        } else if (gamepad2.right_bumper) {
+            ClawServo.setPosition(1);
+        }
+
+        /*if(gamepad2.dpad_down){
+            WristServo.setPosition(0.78);
+            ArmServo.setPosition(0.55);
+        }
+        else if (gamepad2.dpad_up){
+            WristServo.setPosition(0.4);
+            ArmServo.setPosition(0.2);
+
+        }*/
+        if (gamepad2.a) {
+            Slide.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        }
+
+        if (gamepad2.dpad_down) {
+            SlideArm.ServoSteps(WristServo, 0.78, 4, 8);
+            try {
+                Thread.sleep(50);
+            } catch (InterruptedException e) {
+                throw new RuntimeException(e);
+            }
+            SlideArm.ServoSteps(ArmServo, 0.55, 10, 5);
+        } else if (gamepad2.dpad_up) {
+
+            WristServo.setPosition(0.4);
+            ArmServo.setPosition(0.2);
+
+        }
+
+        telemetry.addData("Slide Ticks", Slide.getCurrentPosition());
+        telemetry.update();
+        //limit = 6600;
+
+
+        if (gamepad2.x) {
+            xPressed = true;
+        }
+
+        while (xPressed && Slide.getCurrentPosition() < 6300) {
+            Slide.setPower(0.6);
+            Slide.setTargetPosition(6300);
+            Slide.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+            telemetry.addData("In While Loop:", Slide.getCurrentPosition());
+            telemetry.update();
+        }
+        if (xPressed && Slide.getCurrentPosition() >= 6300)
+        {
+            xPressed = false;
+        }
+
+        Slide.setPower(0);
+
+        if (gamepad2.y) {
+            yPressed = false;
+        }
+
+        while (yPressed && Slide.getCurrentPosition() < 3150) {
+            Slide.setPower(0.6);
+            Slide.setTargetPosition(3150);
+            Slide.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+            telemetry.addData("In While Loop:", Slide.getCurrentPosition());
+            telemetry.update();
+        }
+        Slide.setPower(0);
+        if (yPressed && Slide.getCurrentPosition() >= 3150)
+        {
+            yPressed = false;
+        }
+        if (gamepad2.b) {
+            bPressed = !bPressed;
+        }
+
+        while (bPressed && Slide.getCurrentPosition() > 0) {
+            Slide.setPower(0.6);
+            Slide.setTargetPosition(0);
+            Slide.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+            telemetry.addData("In While Loop:", Slide.getCurrentPosition());
+            telemetry.update();
+        }
+        Slide.setPower(0);
+        if (xPressed && Slide.getCurrentPosition() >= 0)
+        {
+            bPressed = false;
+        } }
+
+    /**
+     * This function takes 4 parameters and makes your servo move in multiple steps for precision
+     * @param servo the servo you want to move
+     * @param targetPosition the position you want to go to
+     * @param Steps how many steps you want to move before reaching targetPosition
+     * @param millis how much to sleep between steps
+     */
+    public static void ServoSteps (Servo servo, double targetPosition, int Steps, long millis){
+        double startingPosition = servo.getPosition();
+        double howFarToMove = targetPosition-startingPosition;
+        double Increment = howFarToMove/Steps;
+        double currentPosition = startingPosition;
+        for(int i = 0; i < Steps; i ++) {
+            servo.setPosition(currentPosition + Increment);
+            currentPosition += Increment;
+            try {
+                Thread.sleep(millis);
+            } catch (InterruptedException e) {
+                throw new RuntimeException(e);
+            }
+        }
+
+    }
+
+
+
+    public void stop(){
+
+    }
+
+
+}