API tools faq
paste
Advertisement
archen2019

auto1

archen2019
Jan 18th, 2019
114
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Java 14.39 KB | None | 0 0
raw download clone embed print report
  1. package org.firstinspires.ftc.teamcode;
  2.  
  3. import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
  4. import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
  5. import com.qualcomm.robotcore.hardware.DcMotor;
  6. import com.qualcomm.robotcore.util.ElapsedTime;
  7.  
  8. import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
  9. import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
  10. import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
  11. import org.opencv.core.Mat;
  12.  
  13. @Autonomous
  14. public class AutoOp_Andrew extends LinearOpMode {
  15.  
  16.     private HardwareMurderbot robot;
  17.     static {System.loadLibrary("opencv_java3");}
  18.     private ElapsedTime runtime = new ElapsedTime();
  19.  
  20.     private final double TICKS_PER_MOTOR_REV = 537.6; // AndyMark Motor Encoders
  21.     private final double DRIVE_GEAR_REDUCTION = 1.0; // This is < 1.0 if geared UP -- might have to check this cus official site says gearbox is 20:1
  22.     private final double WHEEL_DIAMETER_INCHES = 4.0; // For figuring circumference
  23.     private final double COUNTS_PER_INCH = (TICKS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / (WHEEL_DIAMETER_INCHES * Math.PI);
  24.  
  25.     private final double PINION_POWER = 1.0;
  26.     private final double DRIVE_POWER = 0.5;
  27.     private final double TURN_POWER = 0.1;
  28.  
  29.     private final double TURN_ERROR = 0.25;
  30.     private double origin = 0;
  31.  
  32.     private final double ROBOT_WIDTH = 15.125; //inches between two driving wheel centers approx
  33.  
  34.     @Override
  35.     public void runOpMode() {
  36.         // Initialize
  37.         initMode();
  38.  
  39.         waitForStart();
  40.  
  41.         //Autonomous
  42. //        encoderDrive(DRIVE_POWER, 12, 12, 5);
  43.         gyroTurn(TURN_POWER, 180, 5, 5);
  44. //        displaceGoldMineralv2();
  45. //        wallAlign();
  46. //        dropMarker();
  47. //        drive2Crater();
  48.  
  49.     }
  50.  
  51.     public void initMode(){
  52.         robot = new HardwareMurderbot(hardwareMap);
  53.         //robot.initCV();
  54.  
  55.         robot.enableEncoders();
  56.         origin = robot.imu.getAngularOrientation().thirdAngle;
  57.  
  58.     }
  59.  
  60.     // Drop from hanging position
  61. //    public void unhangBot(){
  62. //
  63. //        double inchesDown = 12;
  64. //        int newPos = robot.pinion.getCurrentPosition() + (int)(COUNTS_PER_INCH * inchesDown);
  65. //
  66. //        robot.pinion.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  67. //
  68. //        runtime.reset();
  69. //        robot.setPinionPower(PINION_POWER);
  70. //
  71. //        while (opModeIsActive() && robot.pinion.isBusy()) {
  72. //
  73. //            // Display it for the driver.
  74. //            telemetry.addData("Target Position", "Running to %7d", newPos);
  75. //            telemetry.addData("Current Position", "Running at %7d", robot.pinion.getCurrentPosition());
  76. //            telemetry.update();
  77. //        }
  78. //
  79. //        // Stop all motion;
  80. //        robot.setPinionPower(0);
  81. //
  82. //        // Turn off RUN_TO_POSITION
  83. //        robot.pinion.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  84. //    }
  85.  
  86.     public void wallAlign(){
  87.         gyroTurn(TURN_POWER, 45, 5, 5);
  88.         encoderDrive(DRIVE_POWER,50, 50, 3.0);
  89.         encoderDrive(DRIVE_POWER, -3, -3, 3.0);
  90.         encoderTurn(TURN_POWER, -90, 5.0);
  91.     }
  92.  
  93.     public void dropMarker(){
  94.         encoderDrive(DRIVE_POWER, -30, -30, 5.0);
  95.         // robot.teamMarkerDropper.setPosition(1);
  96.         sleep(750);
  97.     }
  98.  
  99.     public void drive2Crater(){
  100.         encoderDrive(DRIVE_POWER, 55, 55, 5.0);
  101.         encoderTurn(TURN_POWER, -10, 5);
  102.         encoderDrive(DRIVE_POWER, 25, 25, 5.0);
  103.     }
  104.  
  105.     public void displaceGoldMineralv2(){
  106.         double strafeSpeed = 0.2;
  107.         double STRAFE_BOUND = 12 * COUNTS_PER_INCH;
  108.         double initialPos = robot.leftFrontDrive.getCurrentPosition();
  109.         int refreshTick = 0;
  110.         int scanCount = 0;
  111.  
  112.         // turn and move away from lander
  113.         encoderStrafe(0.2, 6, 5);
  114.         gyroTurn(TURN_POWER, 90, 5, 5);
  115.         encoderDrive(0.2, 6, 6, 5);
  116.  
  117.         robot.setDrivePower(-strafeSpeed, strafeSpeed, strafeSpeed, -strafeSpeed);
  118.  
  119.         while(opModeIsActive() && !robot.cv.getAligned()){
  120.             if(refreshTick != 0) refreshTick--;
  121.             if(robot.cv.isFound()) {
  122.                 strafeSpeed /= 2;
  123.                 robot.setDrivePower(-strafeSpeed, strafeSpeed, strafeSpeed, -strafeSpeed);
  124.             }
  125.             if(Math.abs(robot.leftFrontDrive.getCurrentPosition() - initialPos) > STRAFE_BOUND && refreshTick == 0){
  126.                 strafeSpeed = -strafeSpeed;
  127.                 robot.setDrivePower(-strafeSpeed, strafeSpeed, strafeSpeed, -strafeSpeed);
  128.                 scanCount++;
  129.                 refreshTick = 5000;
  130.                 if(scanCount > 2) break;
  131.             }
  132.         }
  133.         robot.setDrivePower(0,0);
  134.         robot.cv.disable();
  135.         encoderDrive(0.3, -26, -26, 5);
  136.         encoderDrive(0.3, 25, 25, 5);
  137.         sleep(100);
  138. //        encoderTurn(TURN_POWER, LANDER_OFFSET, 5);
  139.     }
  140.  
  141.     public double encoderTurn(double power, double degrees, double timeout){
  142.         double robotSector = Math.PI * (degrees / 180.0);
  143.         double turnInches = 2 * 0.5 * ROBOT_WIDTH * robotSector;
  144.         encoderDrive(power, turnInches, -turnInches, 3);
  145.         return turnInches;
  146.     }
  147.  
  148.     // Drives the specifed distance (inches) with a timeout at the specified number of seconds at the specified speed using motor encoders (negative distance = reverse movement)
  149.     public void encoderDrive(double power, double leftInches, double rightInches, double timeout) {
  150.         int newLeftFrontTarget;
  151.         int newLeftRearTarget;
  152.         int newRightFrontTarget;
  153.         int newRightRearTarget;
  154.  
  155.         // Ensure that the opmode is still active
  156.         if (opModeIsActive()) {
  157.  
  158.             // Determine new target position, and pass to motor controller
  159.             newLeftFrontTarget = robot.leftFrontDrive.getCurrentPosition() + (int) (-leftInches * COUNTS_PER_INCH);
  160.             newLeftRearTarget = robot.leftFrontDrive.getCurrentPosition() + (int) (-leftInches * COUNTS_PER_INCH);
  161.             newRightFrontTarget = robot.rightFrontDrive.getCurrentPosition() + (int) (-rightInches * COUNTS_PER_INCH);
  162.             newRightRearTarget = robot.rightFrontDrive.getCurrentPosition() + (int) (-rightInches * COUNTS_PER_INCH);
  163.             robot.leftFrontDrive.setTargetPosition(newLeftFrontTarget);
  164.             robot.leftRearDrive.setTargetPosition(newLeftRearTarget);
  165.             robot.rightFrontDrive.setTargetPosition(newRightFrontTarget);
  166.             robot.rightRearDrive.setTargetPosition(newRightRearTarget);
  167.  
  168.             // Turn On RUN_TO_POSITION
  169.             robot.leftFrontDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  170.             robot.leftRearDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  171.             robot.rightFrontDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  172.             robot.rightRearDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  173.  
  174.             // reset the timeout time and start motion.
  175.             runtime.reset();
  176.             robot.setDrivePower(Math.abs(power), Math.abs(power));
  177.  
  178.             // keep looping while we are still active, and there is time left, and both motors are running.
  179.             // Note: We use (isBusy() && isBusy()) in the loop test, which means that when EITHER motor hits
  180.             // its target position, the motion will stop.  This is "safer" in the event that the robot will
  181.             // always end the motion as soon as possible.
  182.             // However, if you require that BOTH motors have finished their moves before the robot continues
  183.             // onto the next step, use (isBusy() || isBusy()) in the loop test.
  184.             while (opModeIsActive() &&
  185.                     (runtime.seconds() < timeout) &&
  186.                     (robot.leftFrontDrive.isBusy() && robot.rightFrontDrive.isBusy() &&
  187.                             robot.leftRearDrive.isBusy() && robot.rightRearDrive.isBusy())) {
  188.  
  189.                 // Display it for the driver.
  190.                 telemetry.addData("Path1", "Running to %7d :%7d",
  191.                         newLeftFrontTarget, newRightFrontTarget,
  192.                         newLeftRearTarget, newRightRearTarget);
  193.                 telemetry.addData("Path2", "Running at %7d :%7d",
  194.                         robot.leftFrontDrive.getCurrentPosition(),
  195.                         robot.leftRearDrive.getCurrentPosition(),
  196.                         robot.rightFrontDrive.getCurrentPosition(),
  197.                         robot.rightRearDrive.getCurrentPosition());
  198.                 telemetry.update();
  199.             }
  200.  
  201.             // Stop all motion;
  202.             robot.setDrivePower(0, 0);
  203.  
  204.             // Turn off RUN_TO_POSITION
  205.             robot.leftFrontDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  206.             robot.leftRearDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  207.             robot.rightFrontDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  208.             robot.rightRearDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  209.         }
  210.     }
  211.  
  212.     // Drives the specifed distance (inches) with a timeout at the specified number of seconds at the specified speed using motor encoders (negative distance = reverse movement)
  213.     public void encoderStrafe(double power, double strafeInches, double timeout) {
  214.         int newLeftFrontTarget;
  215.         int newLeftRearTarget;
  216.         int newRightFrontTarget;
  217.         int newRightRearTarget;
  218.  
  219.         // Ensure that the opmode is still active
  220.         if (opModeIsActive()) {
  221.  
  222.             // Determine new target position, and pass to motor controller
  223.             newLeftFrontTarget = robot.leftFrontDrive.getCurrentPosition() + (int) (-strafeInches * COUNTS_PER_INCH);
  224.             newLeftRearTarget = robot.leftFrontDrive.getCurrentPosition() + (int) (strafeInches * COUNTS_PER_INCH);
  225.             newRightFrontTarget = robot.rightFrontDrive.getCurrentPosition() + (int) (strafeInches * COUNTS_PER_INCH);
  226.             newRightRearTarget = robot.rightFrontDrive.getCurrentPosition() + (int) (-strafeInches * COUNTS_PER_INCH);
  227.             robot.leftFrontDrive.setTargetPosition(newLeftFrontTarget);
  228.             robot.leftRearDrive.setTargetPosition(newLeftRearTarget);
  229.             robot.rightFrontDrive.setTargetPosition(newRightFrontTarget);
  230.             robot.rightRearDrive.setTargetPosition(newRightRearTarget);
  231.  
  232.             // Turn On RUN_TO_POSITION
  233.             robot.leftFrontDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  234.             robot.leftRearDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  235.             robot.rightFrontDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  236.             robot.rightRearDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  237.  
  238.             // reset the timeout time and start motion.
  239.             runtime.reset();
  240.             robot.setDrivePower(Math.abs(power), Math.abs(power));
  241.  
  242.             // keep looping while we are still active, and there is time left, and both motors are running.
  243.             // Note: We use (isBusy() && isBusy()) in the loop test, which means that when EITHER motor hits
  244.             // its target position, the motion will stop.  This is "safer" in the event that the robot will
  245.             // always end the motion as soon as possible.
  246.             // However, if you require that BOTH motors have finished their moves before the robot continues
  247.             // onto the next step, use (isBusy() || isBusy()) in the loop test.
  248.             while (opModeIsActive() &&
  249.                     (runtime.seconds() < timeout) &&
  250.                     (robot.leftFrontDrive.isBusy() && robot.rightFrontDrive.isBusy() &&
  251.                             robot.leftRearDrive.isBusy() && robot.rightRearDrive.isBusy())) {
  252.  
  253.                 // Display it for the driver.
  254.                 telemetry.addData("Path1", "Running to %7d :%7d",
  255.                         newLeftFrontTarget, newRightFrontTarget,
  256.                         newLeftRearTarget, newRightRearTarget);
  257.                 telemetry.addData("Path2", "Running at %7d :%7d",
  258.                         robot.leftFrontDrive.getCurrentPosition(),
  259.                         robot.leftRearDrive.getCurrentPosition(),
  260.                         robot.rightFrontDrive.getCurrentPosition(),
  261.                         robot.rightRearDrive.getCurrentPosition());
  262.                 telemetry.update();
  263.             }
  264.  
  265.             // Stop all motion;
  266.             robot.setDrivePower(0, 0);
  267.  
  268.             // Turn off RUN_TO_POSITION
  269.             robot.leftFrontDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  270.             robot.leftRearDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  271.             robot.rightFrontDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  272.             robot.rightRearDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  273.         }
  274.     }
  275.  
  276.     // Turn using gyro
  277.     public void gyroTurn(double power, double degrees, double timeout, int sleep) {
  278.         origin = robot.imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES).thirdAngle;
  279.         double target = origin + degrees;
  280.  
  281.         // make target between -180 exclusive and 180 inclusive
  282.         target = (target - 180) % 360 + 180;
  283.  
  284.         double heading = origin;
  285.         double leftPower = power;
  286.         double rightPower = power;
  287.         double slowFactor = 1; // power = power / slowFactor
  288.  
  289.         runtime.reset();
  290.  
  291.         // set power -- need to check direction
  292.         if((degrees - 180) % 360 + 180 >= 0) leftPower = -leftPower;
  293.         else rightPower = -rightPower;
  294.  
  295.         while(opModeIsActive() && runtime.seconds() < timeout && Math.abs(heading - target) < TURN_ERROR) {
  296.  
  297.             if (slowFactor < 2 && Math.abs(heading - target) < 20) {
  298.                 leftPower /= 2;
  299.                 rightPower /= 2;
  300.                 slowFactor = 2;
  301.             }
  302.             else if (slowFactor < 4 && Math.abs(heading - target) < 10) {
  303.                 leftPower /= 2;
  304.                 rightPower /= 2;
  305.                 slowFactor = 4;
  306.             }
  307.  
  308.             robot.setDrivePower(leftPower, rightPower);
  309.  
  310.             heading = robot.imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES).thirdAngle;
  311.  
  312.             telemetry.addData("Heading", heading);
  313.             telemetry.addData("Target", target);
  314.             telemetry.addData("Origin", origin);
  315.             telemetry.addData("Left", leftPower);
  316.             telemetry.addData("Right", rightPower);
  317.             telemetry.update();
  318.         }
  319.  
  320.         robot.setDrivePower(0, 0);
  321.         origin = robot.imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES).thirdAngle;
  322.         if (sleep > 0) sleep((long) sleep);
  323.     }
  324. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!