API tools faq
paste
Advertisement
Guest User

Code

a guest
Jan 16th, 2017
195
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Java 13.07 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.ColorSensor;
  6. import com.qualcomm.robotcore.hardware.DcMotor;
  7. import com.qualcomm.robotcore.hardware.Servo;
  8. import com.vuforia.Image;
  9. import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
  10. import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables;
  11.  
  12.  
  13.  
  14. /**
  15.  * Created by mgandhi on 12/19/16.
  16.  */
  17. @Autonomous(name = "RedAuto")
  18. public class NewAuto extends LinearOpMode {
  19.  
  20.     //Motors
  21.     private DcMotor motorFrontLeft;
  22.     private DcMotor motorFrontRight;
  23.     private DcMotor motorBackLeft;
  24.     private DcMotor motorBackRight;
  25.     private DcMotor motorConveyor;
  26.     private DcMotor motorShooterLeft;
  27.     private DcMotor motorShooterRight;
  28.     //Servos
  29.     private Servo servoTopCap;
  30.     private Servo servoBottomCap;
  31.     //Sensors
  32.     private ColorSensor SensorColor;
  33.  
  34.     //Voids
  35.     private void RunToPosition() {
  36.         motorFrontLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  37.         motorFrontRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  38.         motorBackLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  39.         motorBackRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  40.     }
  41.     private void RunUsingEncoder() {
  42.         motorFrontLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  43.         motorFrontRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  44.         motorBackLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  45.         motorBackRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  46.     }
  47.     private void Reset() {
  48.         motorFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  49.         motorFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  50.         motorBackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  51.         motorBackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  52.     }
  53.     private void Brake() {
  54.         motorFrontLeft.setPower(0);
  55.         motorFrontRight.setPower(0);
  56.         motorBackLeft.setPower(0);
  57.         motorBackRight.setPower(0);
  58.     }
  59.     private void PowerDrive() {
  60.         motorFrontLeft.setPower(drivePower);
  61.         motorFrontRight.setPower(drivePower);
  62.         motorBackRight.setPower(drivePower);
  63.         motorBackLeft.setPower(drivePower);
  64.     }
  65.     private void PowerCapture() {
  66.         motorFrontLeft.setPower(drivePower/2);
  67.         motorFrontRight.setPower(drivePower/2);
  68.         motorBackRight.setPower(drivePower/2);
  69.         motorBackLeft.setPower(drivePower/2);
  70.     }
  71.     private void PowerStrafe() {
  72.         motorFrontLeft.setPower(drivePower*3);
  73.         motorFrontRight.setPower(drivePower*3);
  74.         motorBackRight.setPower(drivePower*3);
  75.         motorBackLeft.setPower(drivePower*3);
  76.     }
  77.     private void Busy() {
  78.         while (motorFrontLeft.isBusy() && motorFrontRight.isBusy() && motorBackLeft.isBusy() && motorBackRight.isBusy())
  79.         {
  80.         }
  81.     }
  82.  
  83.  
  84.     //Values
  85.     private static final double drivePower = .15;
  86.     private static final double shooterPower = .83;
  87.     private static final int TileDrive = (int)(1200);
  88.     private static final int TileStrafe = (int)(2100);
  89.     private static final int RightAngleTurn = (int)(1200);
  90.  
  91.  
  92.     @Override
  93.     public void runOpMode() throws InterruptedException {
  94.  
  95.         //Hardware Map
  96.         {
  97.             motorFrontLeft = hardwareMap.dcMotor.get("FLM");
  98.             motorFrontRight = hardwareMap.dcMotor.get("FRM");
  99.             motorBackLeft = hardwareMap.dcMotor.get("BLM");
  100.             motorBackRight = hardwareMap.dcMotor.get("BRM");
  101.             motorConveyor = hardwareMap.dcMotor.get("CM");
  102.             motorShooterLeft = hardwareMap.dcMotor.get("SML");
  103.             motorShooterRight = hardwareMap.dcMotor.get("SMR");
  104.  
  105.             servoBottomCap = hardwareMap.servo.get("LF");
  106.             servoTopCap = hardwareMap.servo.get("RFS");
  107.  
  108.             SensorColor = hardwareMap.colorSensor.get("SensorColor");
  109.  
  110.             motorFrontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  111.             motorFrontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  112.             motorBackLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  113.             motorBackRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  114.         }
  115.  
  116.         {
  117.             motorBackRight.setDirection(DcMotor.Direction.REVERSE);
  118.             motorShooterLeft.setDirection(DcMotor.Direction.REVERSE);
  119.             motorFrontRight.setDirection(DcMotor.Direction.REVERSE);
  120.         }
  121.  
  122.         {
  123.             Reset();
  124.             servoTopCap.setPosition(1);
  125.             servoBottomCap.setPosition(.55);
  126.             SensorColor.enableLed(false);
  127.             RunUsingEncoder();
  128.             telemetry.addData("Say", "RoboticsAF Initiated");
  129.             telemetry.update();
  130.         }
  131.  
  132.         waitForStart();
  133.  
  134.  
  135. //Start Flywheels
  136.         motorShooterLeft.setPower(-.55);
  137.         motorShooterRight.setPower(-.55);
  138.  
  139. //Forwards to Shooting Distance
  140.  
  141.             motorBackLeft.setTargetPosition(motorBackLeft.getCurrentPosition() - (int)(TileDrive * 1.55));
  142.             motorBackRight.setTargetPosition(motorBackRight.getCurrentPosition() - (int)(TileDrive * 1.55));
  143.             motorFrontLeft.setTargetPosition(motorFrontLeft.getCurrentPosition() - (int)(TileDrive * 1.55));
  144.             motorFrontRight.setTargetPosition(motorFrontRight.getCurrentPosition() - (int)(TileDrive * 1.55));
  145.  
  146.             RunToPosition();
  147.             PowerDrive();
  148.             Busy();
  149.             Brake();
  150.             RunUsingEncoder();
  151.  
  152. //Shoot Particles
  153.             motorConveyor.setPower(-.8);
  154.             sleep(3000);
  155.             motorShooterLeft.setPower(0);
  156.             motorShooterRight.setPower(0);
  157.             motorConveyor.setPower(0);
  158.  
  159. //Strafe to Avoid Cap Ball
  160.             motorBackLeft.setTargetPosition(motorBackLeft.getCurrentPosition() - (int)(TileStrafe));
  161.             motorBackRight.setTargetPosition(motorBackRight.getCurrentPosition() + (int)(TileStrafe));
  162.             motorFrontLeft.setTargetPosition(motorFrontLeft.getCurrentPosition() + (int)(TileStrafe));
  163.             motorFrontRight.setTargetPosition(motorFrontRight.getCurrentPosition() - (int)(TileStrafe));
  164.  
  165.             RunToPosition();
  166.             PowerStrafe();
  167.             Busy();
  168.             Brake();
  169.             RunUsingEncoder();
  170.             sleep(250);
  171.  
  172. //Forwards to Beacon 1
  173.             motorBackLeft.setTargetPosition(motorBackLeft.getCurrentPosition() - (int)(TileDrive * 1));
  174.             motorBackRight.setTargetPosition(motorBackRight.getCurrentPosition() - (int)(TileDrive * 1));
  175.             motorFrontLeft.setTargetPosition(motorFrontLeft.getCurrentPosition() - (int)(TileDrive * 1));
  176.             motorFrontRight.setTargetPosition(motorFrontRight.getCurrentPosition() - (int)(TileDrive * 1));
  177.  
  178.             RunToPosition();
  179.             PowerDrive();
  180.             Busy();
  181.             Brake();
  182.             RunUsingEncoder();
  183.             sleep(250);
  184.  
  185. //Adjustment Turn
  186.             motorBackLeft.setTargetPosition(motorBackLeft.getCurrentPosition()+ (int)(RightAngleTurn/16));
  187.             motorBackRight.setTargetPosition(motorBackRight.getCurrentPosition()- (int)(RightAngleTurn/16));
  188.             motorFrontLeft.setTargetPosition(motorFrontLeft.getCurrentPosition()+ (int)(RightAngleTurn/16));
  189.             motorFrontRight.setTargetPosition(motorFrontRight.getCurrentPosition()- (int)(RightAngleTurn/16));
  190.             RunToPosition();
  191.             PowerDrive();
  192.             Busy();
  193.             Brake();
  194.             RunUsingEncoder();
  195.  
  196. //Strafe To Beacon 1
  197.             motorBackLeft.setTargetPosition(motorBackLeft.getCurrentPosition() - (int)(TileStrafe * 1.2));
  198.             motorBackRight.setTargetPosition(motorBackRight.getCurrentPosition() + (int)(TileStrafe * 1.2));
  199.             motorFrontLeft.setTargetPosition(motorFrontLeft.getCurrentPosition() + (int)(TileStrafe * 1.2));
  200.             motorFrontRight.setTargetPosition(motorFrontRight.getCurrentPosition() - (int)(TileStrafe * 1.2));
  201.             RunToPosition();
  202.             PowerStrafe();
  203.             Busy();
  204.             Brake();
  205.             RunUsingEncoder();
  206.             telemetry.addData("Blue", SensorColor.blue());
  207.             telemetry.addData("Red", SensorColor.red());
  208.             sleep(1000);
  209.  
  210. //Capture Beacon 1
  211.             if (SensorColor.red() > SensorColor.blue()) {
  212.  
  213.             telemetry.addData("Red Side", SensorColor.red());
  214.  
  215.             motorBackLeft.setTargetPosition(motorBackLeft.getCurrentPosition() - (int) (TileDrive * .4));
  216.             motorBackRight.setTargetPosition(motorBackRight.getCurrentPosition() - (int) (TileDrive * .4));
  217.             motorFrontLeft.setTargetPosition(motorFrontLeft.getCurrentPosition() - (int) (TileDrive * .4));
  218.             motorFrontRight.setTargetPosition(motorFrontRight.getCurrentPosition() - (int) (TileDrive * .4));
  219.             RunToPosition();
  220.             PowerCapture();
  221.             Busy();
  222.             Brake();
  223.             RunUsingEncoder();
  224.  
  225.             motorBackLeft.setTargetPosition(motorBackLeft.getCurrentPosition() - (int) (TileStrafe * .2));
  226.             motorBackRight.setTargetPosition(motorBackRight.getCurrentPosition() + (int) (TileStrafe * .2));
  227.             motorFrontLeft.setTargetPosition(motorFrontLeft.getCurrentPosition() + (int) (TileStrafe * .2));
  228.             motorFrontRight.setTargetPosition(motorFrontRight.getCurrentPosition() - (int) (TileStrafe * .2));
  229.             RunToPosition();
  230.             PowerStrafe();
  231.             Busy();
  232.             Brake();
  233.             RunUsingEncoder();
  234.  
  235.             motorBackLeft.setTargetPosition(motorBackLeft.getCurrentPosition() + (int) (TileDrive * .4));
  236.             motorBackRight.setTargetPosition(motorBackRight.getCurrentPosition() + (int) (TileDrive * .4));
  237.             motorFrontLeft.setTargetPosition(motorFrontLeft.getCurrentPosition() + (int) (TileDrive * .4));
  238.             motorFrontRight.setTargetPosition(motorFrontRight.getCurrentPosition() + (int) (TileDrive * .4));
  239.             RunToPosition();
  240.             PowerCapture();
  241.             Busy();
  242.             Brake();
  243.             RunUsingEncoder();
  244.  
  245.         } else {
  246.  
  247.             telemetry.addData("Blue Side", SensorColor.blue());
  248.  
  249.             motorBackLeft.setTargetPosition(motorBackLeft.getCurrentPosition() + (int) (TileDrive * .4));
  250.             motorBackRight.setTargetPosition(motorBackRight.getCurrentPosition() + (int) (TileDrive * .4));
  251.             motorFrontLeft.setTargetPosition(motorFrontLeft.getCurrentPosition() + (int) (TileDrive * .4));
  252.             motorFrontRight.setTargetPosition(motorFrontRight.getCurrentPosition() + (int) (TileDrive * .4));
  253.             RunToPosition();
  254.             PowerCapture();
  255.             Busy();
  256.             Brake();
  257.             RunUsingEncoder();
  258.  
  259.             motorBackLeft.setTargetPosition(motorBackLeft.getCurrentPosition() - (int) (TileStrafe * .2));
  260.             motorBackRight.setTargetPosition(motorBackRight.getCurrentPosition() + (int) (TileStrafe * .2));
  261.             motorFrontLeft.setTargetPosition(motorFrontLeft.getCurrentPosition() + (int) (TileStrafe * .2));
  262.             motorFrontRight.setTargetPosition(motorFrontRight.getCurrentPosition() - (int) (TileStrafe * .2));
  263.             RunToPosition();
  264.             PowerStrafe();
  265.             Busy();
  266.             Brake();
  267.             RunUsingEncoder();
  268.  
  269.             motorBackLeft.setTargetPosition(motorBackLeft.getCurrentPosition() - (int) (TileDrive * .4));
  270.             motorBackRight.setTargetPosition(motorBackRight.getCurrentPosition() - (int) (TileDrive * .4));
  271.             motorFrontLeft.setTargetPosition(motorFrontLeft.getCurrentPosition() - (int) (TileDrive * .4));
  272.             motorFrontRight.setTargetPosition(motorFrontRight.getCurrentPosition() - (int) (TileDrive * .4));
  273.             RunToPosition();
  274.             PowerCapture();
  275.             Busy();
  276.             Brake();
  277.             RunUsingEncoder();
  278.         }
  279.  
  280.  
  281. //Strafe Back
  282.             motorBackLeft.setTargetPosition(motorBackLeft.getCurrentPosition() + (int)(TileStrafe/2));
  283.             motorBackRight.setTargetPosition(motorBackRight.getCurrentPosition() - (int)(TileStrafe/2));
  284.             motorFrontLeft.setTargetPosition(motorFrontLeft.getCurrentPosition() - (int)(TileStrafe/2));
  285.             motorFrontRight.setTargetPosition(motorFrontRight.getCurrentPosition() + (int)(TileStrafe/2));
  286.  
  287.             RunToPosition();
  288.             PowerStrafe();
  289.             Busy();
  290.             Brake();
  291.             RunUsingEncoder();
  292.  
  293. //Backwards to Corner
  294.             motorBackLeft.setTargetPosition(motorBackLeft.getCurrentPosition() + (int)(TileDrive * 1.5));
  295.             motorBackRight.setTargetPosition(motorBackRight.getCurrentPosition() + (int)(TileDrive * 1.5));
  296.             motorFrontLeft.setTargetPosition(motorFrontLeft.getCurrentPosition() + (int)(TileDrive * 1.5));
  297.             motorFrontRight.setTargetPosition(motorFrontRight.getCurrentPosition() + (int)(TileDrive * 1.5));
  298.  
  299.             RunToPosition();
  300.             PowerDrive();
  301.             Busy();
  302.             Brake();
  303.             RunUsingEncoder();
  304.        
  305.         stop();
  306.  
  307.         }}
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!