10/26/21 #1

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

@@ -0,0 +1,93 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.qualcomm.robotcore.eventloop.opmode.*;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.util.ElapsedTime;
+//import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+//import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+//import com.qualcomm.robotcore.hardware.DcMotorSimple;
+//import com.qualcomm.robotcore.hardware.DcMotor;
+
+@Autonomous(name="CDAutonRedWarehouse", group="Linear Opmode")
+//@Disabled
+public class CDAutonRedWarehouse extends LinearOpMode {
+
+    @Override
+    public void runOpMode() {
+        ElapsedTime myTimer = new ElapsedTime();
+        //double moveBackTimer = -1;
+
+        CDHardware myHardware = new CDHardware(hardwareMap);
+        CDDriveChassisAuton myChassis = new CDDriveChassisAuton(myHardware);
+        CDDuckSpinner myDuckSpinner = new CDDuckSpinner(myHardware);
+        CDElevator myElevator = new CDElevator(myHardware);
+        CDIntake myIntake = new CDIntake(myHardware);
+        CDTurret myTurret = new CDTurret(myHardware);
+        // CDGyroscope myGyro = new CDGyroscope();
+        CDDistanceSensor myDistanceSensor = new CDDistanceSensor(myHardware);
+
+        //Send telemetry to signify robot waiting
+        telemetry.addData("Status", "Resetting Encoders");
+        telemetry.update();
+
+        myChassis.robotHardware.rightrearmotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        myChassis.robotHardware.rightfrontmotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        myChassis.robotHardware.leftrearmotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        myChassis.robotHardware.leftfrontmotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        myTurret.robotHardware.turretmotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+
+        myChassis.robotHardware.rightrearmotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        myChassis.robotHardware.rightfrontmotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        myChassis.robotHardware.leftrearmotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        myChassis.robotHardware.leftfrontmotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        myTurret.robotHardware.turretmotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
+        //Send telemetry to indicate successful Encoder reset
+        telemetry.addData("MotorStartPos (RR, RF, LR, LF)", " %7d %7d %7d %7d", myChassis.robotHardware.rightrearmotor.getCurrentPosition(), myChassis.robotHardware.rightfrontmotor.getCurrentPosition(), myChassis.robotHardware.leftrearmotor.getCurrentPosition(), myChassis.robotHardware.leftfrontmotor.getCurrentPosition());
+        telemetry.addData("Status", "Initialized");
+        telemetry.update();
+
+        //Wait fo the game to start (driver presses PLAY)
+        waitForStart();
+        //myTimer.reset();
+        // TODO: Need to use the timer to program the robot in Auton competition
+
+        //Step through each leg of path
+        //Note: Reverse movement is obtained by selling a negative distance not speed
+        /*Drive code is written using 3 methods
+         *     speed should be either DRIVE_SPEED or TURN_SPEED
+         * These assumes that each movement is relative to the last stopping place
+         * 1. encoderDriveStraight (speed, straightInches, straightTimeout)
+         * 2. encoderDriveStrafe (speed, strafeInches, strafeTimeout)
+         * 3. encodeDriveTurn (speed, turnDeg, turnTimeout)
+         */
+
+        if (opModeIsActive()) {
+
+            //myChassis.encoderDriveStraight(CDDriveChassisAuton.DRIVE_SPEED, 30, 20.0); //Move forward 30 inches with 10 second timeout
+            //sleep(250); //optional pause after each move in milliseconds
+            //myChassis.encoderDriveStraight(CDDriveChassisAuton.DRIVE_SPEED, -20, 10.0); //Move back 20 inches with 10 second timeout
+            //myChassis.encoderDriveStrafe(CDDriveChassisAuton.DRIVE_SPEED, 10, 5); //Move right 10 inches with 5 second timeout
+            //myChassis.encoderDriveStrafe(CDDriveChassisAuton.DRIVE_SPEED, -15, 15); //Move left 15 inches with 15 second timeout
+            //yChassis.encoderDriveTurn(CDDriveChassisAuton.TURN_SPEED, 90, 10); //Turn right 90 degrees with 10 second timeout
+            //myChassis.encoderDriveTurn(CDDriveChassisAuton.TURN_SPEED, -180, 20); //Turn left 180 degrees with a 20 second timeout
+            //sleep(250); //optional pause after each move in milliseconds
+            //myElevator.setElevatorPosition(28); //Move elevator to middle position. Do not set outside of range 2.5-39.
+            myTurret.setTurretPosition(0, "right"); //Starting on right side - Set turret position to center
+            myTurret.setTurretPosition(-90, "right");  //Starting on right side - Set turret position -90 (right)
+            myTurret.setTurretPosition(90, "right");   //Starting on right side - Set turret position 90 (left)
+
+            //  myIntake.setIntakePower(1.0);
+            //  myDuckSpinner.setDuckSpinnerPower(.7);
+
+
+            //Run until the end of the match (Driver presses STOP)
+            //telemetry.addData("MotorCurrentPos (RR, RF, LR, LF)", " %7d %7d %7d %7d", myChassis.robotHardware.rightrearmotor.getCurrentPosition(), myChassis.robotHardware.rightfrontmotor.getCurrentPosition(), myChassis.robotHardware.leftrearmotor.getCurrentPosition(), myChassis.robotHardware.leftfrontmotor.getCurrentPosition());
+            //telemetry.update();
+            //sleep (5000);
+            //TODO figure out how to show target position. 3 lines above will give final position.
+            //TODO: Add telemetry for IMU Gyro
+        }
+    }
+}

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

@@ -7,7 +7,7 @@ public class CDElevator {
 
     CDHardware robotHardware;
     CDDistanceSensor myDistanceSensor;
-
+    public boolean elevatorstop;
     public CDElevator(CDHardware theHardware){
 
         robotHardware = theHardware;
@@ -32,7 +32,7 @@ public void setElevatorPosition(double elevatorpostarget) {
         final double THRESHOLD_POS = 3; // CM or whatever the Distance sensor is configured
         double elevatormult = 0.75; // to slow down the elevator if needed
 
-        boolean elevatorstop = false; // initially we want the elevator to move for the while loop
+        elevatorstop = false; // initially we want the elevator to move for the while loop
         while (!elevatorstop) {
             /* This gets the current distance off the floor from the Elevator Distance Sensor
           and sets it to a variable

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

@@ -132,6 +132,35 @@ public void runOpMode() {
           // TODO: Set up encoder sensor for motorTurret
           myTurret.setTurretPower(turretA);
 
+          if (gamepad2.dpad_up){
+              myTurret.setTurretPosition(0,"center");
+              if (myTurret.turretstop){
+              myElevator.setElevatorPosition(elevatorposground);
+              }
+
+          }
+
+          if (gamepad2.dpad_left){
+              myElevator.setElevatorPosition(elevatorposmiddle);
+              if (myElevator.elevatorstop){
+              myTurret.setTurretPosition(-90,"left");
+
+
+              }
+
+          }
+
+          if (gamepad2.dpad_right){
+              myElevator.setElevatorPosition(elevatorposmiddle);
+              if (myElevator.elevatorstop){
+                  myTurret.setTurretPosition(90,"right");
+
+              }
+
+          }
+
+
+
          //double heading = myGyro.getHeading(AngleUnit.DEGREES);
 
           // magnetic switch

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

@@ -7,6 +7,7 @@
 public class CDTurret {
     double Turretslow = .33;
     CDHardware robotHardware;
+    public boolean turretstop;
 
     public  CDTurret(CDHardware theHardware){
 
@@ -44,9 +45,9 @@ public void setTurretPosition(double turretpostargetdeg, String turretstartside)
         final double TURRET_THRESHOLD_POS = 5; // counts
         double turretmult = 0.75; // to slow down the turret if needed
 
-        boolean turretstop = false; // initially we want the turret to move for the while loop
+         turretstop = false; // initially we want the turret to move for the while loop
         while (!turretstop) {
-            /* This gets the current turret position and sets it to a variable
+            /* This gets the urrent turret position and sets it to a variable
            */
             double turretposcurrent = robotHardware.turretmotor.getCurrentPosition(); //updates every loop
             if (Math.abs(turretposcurrent - turretpostarget) < TURRET_THRESHOLD_POS) {