API tools faq
paste
mrobotics8381

Untitled

mrobotics8381
Jan 10th, 2018
79
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Java 42.36 KB | None | 0 0
raw download clone embed print report
  1. /*
  2. Team M RelicTeleOp1 Class
  3. 2016-2017
  4. */
  5. package org.firstinspires.ftc.teamcode;
  6.  
  7. import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
  8. import com.qualcomm.robotcore.hardware.DcMotor;
  9. import com.qualcomm.robotcore.util.Range;
  10. import com.qualcomm.robotcore.util.RobotLog;
  11.  
  12. import org.firstinspires.ftc.robotcore.external.navigation.RelicRecoveryVuMark;
  13. import org.firstinspires.ftc.robotcore.internal.files.DataLogger;
  14. import org.firstinspires.ftc.teamcode.TeamMClasses.TeamMRelic4;
  15.  
  16. import java.io.IOException;
  17.  
  18. @Autonomous(name="Relic Auto", group="Autonomous")  // @Autonomous(...) is the other common choice
  19. // @Disabled
  20. public class Autonomous2 extends TeamMRelic4
  21. {
  22.  
  23.     DataLogger dl;
  24.  
  25.     // speeds //////////////////////////////////////////////////////////////////////////////////////
  26.     private final double STONE_DRIVE_SPEED = 0.5;
  27.     private final double FAST_DRIVE_SPEED = 0.9;
  28.  
  29.     // delays //////////////////////////////////////////////////////////////////////////////////////
  30.     private final double STOP_DELAY = 0.25; // delay before stopping and resetting encoders (sec.)
  31.     private final double HEADING_TURN_OFF_DIST = iTC(3); // distance before end of turn when turning off heading (counts)
  32.     private final int DRIVE_POS_TOLERANCE = iTC(0.1); // distance when we're considered "there" (counts)
  33.     private final double HEADING_TOLERANCE = 1; // (degrees)
  34.     private final double HEADING_TIME = 0.25; // must be at heading this long (sec.)
  35.  
  36.     // states //////////////////////////////////////////////////////////////////////////////////////
  37.  
  38.     private final int AUTO_INIT = 0;
  39.  
  40.     private final int JEWEL_SCORE = 100;
  41.  
  42.     // red side in which jewels are on different wall as cryptobox
  43.     private final int RED_JUDGES_DRIVE_TO_CRYPTOBOX = 200;
  44.  
  45.     // red side in which jewels are on same wall as cryptobox
  46.     private final int RED_AUDIENCE_DRIVE_TO_CRYPTOBOX = 300;
  47.  
  48.     // blue side in which jewels are on different wall as cryptobox
  49.     private final int BLUE_JUDGES_DRIVE_TO_CRYPTOBOX = 400;
  50.  
  51.     // blue side in which jewels are on same wall as cryptobox
  52.     private final int BLUE_AUDIENCE_DRIVE_TO_CRYPTOBOX = 500;
  53.  
  54.     private final int PLACE_GLYPH = 600;
  55.  
  56.     private final int GET_PILE_GLYPHS = 650;
  57.     private final int RETURN_FROM_PILE = 675;
  58.  
  59.     private final int PARK = 700;
  60.  
  61.     private final int AUTO_STOP = 1000;
  62.  
  63.     // other ///////////////////////////////////////////////////////////////////////////////////////
  64.     private int leftEncoderEndCounts = 0; // used to do drives w/ one motor
  65.     private int rightEncoderEndCounts = 0;
  66.  
  67.     private long startTime = 0;
  68.     private long currentTime = 0;
  69.  
  70.     /*
  71.      * Code to run ONCE when the driver hits INIT
  72.      */
  73.     @Override
  74.     public void init() {
  75.  
  76.         autonomous = true;
  77.  
  78.         super.init();
  79.        
  80.         // initialize servo positions //////////////////////////////////////////////////////////////
  81.         leftIntakePower = LEFT_INTAKE_STOP;
  82.         leftIntakeDirection = LEFT_INTAKE_IN_DIR;
  83.         rightIntakePower = RIGHT_INTAKE_STOP;
  84.         rightIntakeDirection = RIGHT_INTAKE_IN_DIR;
  85.         leftElevatorGearsPower = LEFT_ELEVATOR_GEARS_STOP;
  86.         leftElevatorGearsDirection = LEFT_ELEVATOR_GEARS_UP_DIR;
  87.         rightElevatorGearsPower = RIGHT_ELEVATOR_GEARS_STOP;
  88.         rightElevatorGearsDirection = RIGHT_ELEVATOR_GEARS_UP_DIR;
  89.         leftGlyphTransferPower = LEFT_GLYPH_TRANSFER_STOP;
  90.         leftGlyphTransferDirection = LEFT_GLYPH_TRANSFER_IN_DIR;
  91.         rightGlyphTransferPower = RIGHT_GLYPH_TRANSFER_STOP;
  92.         rightGlyphTransferDirection = RIGHT_GLYPH_TRANSFER_IN_DIR;
  93.         leftElevatorPosition = LEFT_ELEVATOR_CLOSE;
  94.         rightElevatorPosition = RIGHT_ELEVATOR_CLOSE;
  95.         jewelShoulderPosition = JEWEL_SHOULDER_HOME;
  96.         jewelElbowPosition = JEWEL_ELBOW_HOME;
  97.         whiskerPosition = WHISKER_INIT;
  98.  
  99.         try {
  100.             dl = new DataLogger("telemetryFile");
  101.             dl.addHeaderLine("loop time", "entrance bb", "back bb", "pause bb", "spatula bb",
  102.                     "left gears", "right gears", "heading", "angular v", "left enc", "right enc", "typewriter enc",
  103.                     "auto state", "glyph align", "glyph place", /*"left runmode", "right runmode", */ "type runmode", "stone heading gain");
  104.         } catch (IOException e) {
  105.             e.printStackTrace();
  106.         }
  107.     }
  108.  
  109.     /*
  110.      * Code to run REPEATEDLY after the driver hits INIT, but before they hit PLAY
  111.      */
  112.     @Override
  113.     public void init_loop() {
  114.  
  115.         super.init_loop();
  116.     }
  117.  
  118.     @Override
  119.     public void start() {
  120.  
  121.         super.start();
  122.  
  123.         relicTrackables.activate();
  124.         runVuforia = true;
  125.     }
  126.  
  127.     /*
  128.      * Code to run REPEATEDLY after the driver hits PLAY but before they hit STOP
  129.      */
  130.     @Override
  131.     public void loop() {
  132.  
  133.         super.loop();
  134.  
  135.         currentTime = System.nanoTime();
  136.         // emergency home
  137.  
  138.         // automatic ingestion + elevation of glyphs ///////////////////////////////////////////////
  139.         if(autoIngestionElevation) {
  140.             autoIngest();
  141.             autoElevator();
  142.             autoSpatula();
  143.         } else {
  144.             intakeEject();
  145.             elevatorGearsStop();
  146.             glyphTransferStop();
  147.         }
  148.  
  149.         // variables that read sensors/servos //////////////////////////////////////////////////////
  150.  
  151.         // other variables
  152.         double DRIVE_OFF_STONE_HEADING_GAIN;
  153.         if(blueSide) {
  154.             DRIVE_OFF_STONE_HEADING_GAIN = 0;
  155.         } else {
  156.             DRIVE_OFF_STONE_HEADING_GAIN = 0;
  157.         }
  158.  
  159.         // state machine
  160.         switch(loopState) {
  161.  
  162.             case AUTO_INIT:
  163.                 startTime = System.nanoTime();
  164.  
  165.                 spatula.setPower(SPATULA_MAX_SPEED);
  166.                 spatula.setTargetPosition(SPATULA_HOME_POSITION);
  167.                 autoTimeL = System.nanoTime();
  168.  
  169.                 stopAndResetEncoders();
  170.                 if((leftDrive.getMode() == DcMotor.RunMode.STOP_AND_RESET_ENCODER) &&
  171.                         (rightDrive.getMode() == DcMotor.RunMode.STOP_AND_RESET_ENCODER)) {
  172.                     loopState = JEWEL_SCORE; // was jewel_score
  173.                 }
  174.             break;
  175.  
  176.             ////////////////////////////////////////////////////////////////////////////////////////
  177.             // jewel score /////////////////////////////////////////////////////////////////////////
  178.             ////////////////////////////////////////////////////////////////////////////////////////
  179.  
  180.             case JEWEL_SCORE: // read vuforia
  181.                 autoTimeN = System.nanoTime();
  182.                 runToPosition();
  183.                 if(autoTimeN - autoTimeL > (1.5 * 1e9)) {
  184.                     autoTimeL = autoTimeN;
  185.                     if((leftDrive.getMode() == DcMotor.RunMode.RUN_TO_POSITION) &&
  186.                             (rightDrive.getMode() == DcMotor.RunMode.RUN_TO_POSITION)) {
  187.                         loopState++;
  188.                     }
  189.                 }
  190.             break;
  191.  
  192.             case JEWEL_SCORE + 1: // move jewel arm out
  193.                 jewelShoulderPosition = JEWEL_SHOULDER_OUT;
  194.  
  195.                 autoTimeN = System.nanoTime();
  196.                 if(autoTimeN - autoTimeL > (0.4 * 1e9)) { // .4 seconds
  197.                     autoTimeL = autoTimeN;
  198.                     loopState++;
  199.                 }
  200.             break;
  201.  
  202.             case JEWEL_SCORE + 2: // lower jewel arm
  203.                 jewelElbowPosition = JEWEL_ELBOW_DOWN;
  204.  
  205.                 autoTimeN = System.nanoTime();
  206.                 if(autoTimeN - autoTimeL > (0.2 * 1e9)) { // .2 seconds
  207.                     sumRed = 0;
  208.                     sumGreen = 0;
  209.                     sumBlue = 0;
  210.                     colorLoops = 0;
  211.  
  212.                     loopState++;
  213.                 }
  214.             break;
  215.  
  216.             case JEWEL_SCORE + 3: // integrate colors
  217.                 sumRed += readColorRed(jewelColorSensor);
  218.                 sumGreen += readColorGreen(jewelColorSensor);
  219.                 sumBlue += readColorBlue(jewelColorSensor);
  220.                 colorLoops++;
  221.                 if(colorLoops >= 5) { // read color sensor 5 times, then figure out color
  222.                     jewelRed = sumRed / colorLoops;
  223.                     jewelGreen = sumGreen / colorLoops;
  224.                     jewelBlue = sumBlue / colorLoops;
  225.                     currentJewelColor = determineJewelColor(jewelRed, jewelGreen, jewelBlue);
  226.  
  227.                     autoTimeL = System.nanoTime();
  228.                     loopState++;
  229.                 }
  230.             break;
  231.  
  232.             case JEWEL_SCORE + 4: // knock correct glyph
  233.                 if(blueSide) {
  234.                     if(currentJewelColor == JEWEL_RED) {
  235.                         jewelShoulderPosition = JEWEL_SHOULDER_LEFT_JEWEL;
  236.                     } else {
  237.                         jewelShoulderPosition = JEWEL_SHOULDER_RIGHT_JEWEL;
  238.                     }
  239.                 } else { // red side
  240.                     if (currentJewelColor == JEWEL_BLUE) {
  241.                         jewelShoulderPosition = JEWEL_SHOULDER_LEFT_JEWEL;
  242.                     } else {
  243.                         jewelShoulderPosition = JEWEL_SHOULDER_RIGHT_JEWEL;
  244.                     }
  245.                 }
  246.  
  247.                 autoTimeN = System.nanoTime();
  248.                 if(autoTimeN - autoTimeL > (0.3 * 1e9)) { // .3 seconds
  249.                     autoTimeL = autoTimeN;
  250.                     loopState++;
  251.                 }
  252.             break;
  253.  
  254.             case JEWEL_SCORE + 5: // move jewel arm back to center
  255.                 jewelShoulderPosition = JEWEL_SHOULDER_OUT;
  256.  
  257.                 autoTimeN = System.nanoTime();
  258.                 if(autoTimeN - autoTimeL > (0.2 * 1e9)) { // .2 seconds
  259.                     relicTrackables.deactivate();
  260.                     runVuforia = false; // stop vuforia
  261.  
  262.                     // determine column to place if didn't see vuMark
  263.                     if(vuMarkString.equals("")) {
  264.                         columnToPlace = MID_COL;
  265.                     }
  266.  
  267.                     autoTimeL = autoTimeN;
  268.                     loopState++;
  269.                 }
  270.             break;
  271.  
  272.             case JEWEL_SCORE + 6: // retract jewel forearm and starting driving off of stone
  273.                 // set targets (only done once)
  274.                 if(!blueSide && audienceSide) { // red audience
  275.                     leftDrive.setTargetPosition(iTC(29));
  276.                     rightDrive.setTargetPosition(iTC(42.5));
  277.                 } else if(audienceSide) { // blue audience
  278.                     leftDrive.setTargetPosition(iTC(42.5));
  279.                     rightDrive.setTargetPosition(iTC(29));
  280.                 } else if(!blueSide) { // red judges
  281.                     leftDrive.setTargetPosition(iTC(30));
  282.                     rightDrive.setTargetPosition(iTC(43.5)); // same turn amount
  283.                 } else { // blue judges
  284.                     leftDrive.setTargetPosition(iTC(44.5));
  285.                     rightDrive.setTargetPosition(iTC(31));
  286.                 }
  287.                 headingFollow(STONE_DRIVE_SPEED, 0, DRIVE_OFF_STONE_HEADING_GAIN, NORMAL_RAW_GYRO_GAIN);
  288.  
  289.                 jewelElbowPosition = JEWEL_ELBOW_HOME;
  290.  
  291.                 autoTimeN = System.nanoTime();
  292.                 if(autoTimeN - autoTimeL > (0.5 * 1e9)) { // .2 seconds
  293.                     autoTimeL = autoTimeN;
  294.                     loopState++;
  295.                 }
  296.             break;
  297.  
  298.             case JEWEL_SCORE + 7: // retract all of jewel arm and continue driving off of stone
  299.                 jewelShoulderPosition = JEWEL_SHOULDER_HOME;
  300.                 headingFollow(STONE_DRIVE_SPEED, 0, DRIVE_OFF_STONE_HEADING_GAIN, NORMAL_RAW_GYRO_GAIN);
  301.  
  302.                 autoTimeN = System.nanoTime();
  303.                 if(autoTimeN - autoTimeL > (0.3 * 1e9)) { // .2 seconds
  304.                     autoTimeL = autoTimeN;
  305.                     loopState++;
  306.                 }
  307.             break;
  308.  
  309.             case JEWEL_SCORE + 8: // finish drive off of stone
  310.                 headingFollow(STONE_DRIVE_SPEED, 0, DRIVE_OFF_STONE_HEADING_GAIN, NORMAL_RAW_GYRO_GAIN);
  311.                 if(has_drive_encoder_average_reached(iTC(17))) {
  312.                     loopState++;
  313.                 }
  314.             break;
  315.  
  316.             case JEWEL_SCORE + 9: // NOTE: drive run mode is still run using encoders
  317.                 if(blueSide && audienceSide) { // blue audience
  318.                     loopState = BLUE_AUDIENCE_DRIVE_TO_CRYPTOBOX;
  319.                 } else if(blueSide) { // blue judges
  320.                     loopState = BLUE_JUDGES_DRIVE_TO_CRYPTOBOX;
  321.                 } else if(audienceSide) { // red audience
  322.                     loopState = RED_AUDIENCE_DRIVE_TO_CRYPTOBOX;
  323.                 } else { // red judges
  324.                     loopState = RED_JUDGES_DRIVE_TO_CRYPTOBOX;
  325.                 }
  326.             break;
  327.  
  328.             ////////////////////////////////////////////////////////////////////////////////////////
  329.             // red judges //////////////////////////////////////////////////////////////////////////
  330.             ////////////////////////////////////////////////////////////////////////////////////////
  331.  
  332.             case RED_JUDGES_DRIVE_TO_CRYPTOBOX: // inherits distance targets from jewel_score
  333.                 headingFollow(FAST_DRIVE_SPEED, 0, NORMAL_HEADING_GAIN, NORMAL_RAW_GYRO_GAIN);
  334.                 if(has_drive_encoder_average_reached(iTC(30) - HEADING_TURN_OFF_DIST)) {
  335.                     loopState++;
  336.                 }
  337.             break;
  338.  
  339.             case RED_JUDGES_DRIVE_TO_CRYPTOBOX + 1: // turn with no heading gain
  340.                 headingFollow(FAST_DRIVE_SPEED, 0, 0, 0);
  341.                 if((Math.abs(leftEncoderCount - leftDrive.getTargetPosition()) < DRIVE_POS_TOLERANCE) &&
  342.                         (Math.abs(rightEncoderCount - rightDrive.getTargetPosition()) < DRIVE_POS_TOLERANCE)) {
  343.  
  344.                     leftDrive.setPower(0.0);
  345.                     rightDrive.setPower(0.0);
  346.  
  347.                     // numbers for long, fast drive toward cryptobox
  348.                     leftEncoderEndCounts = leftEncoderCount - iTC(44);
  349.                     rightEncoderEndCounts = rightEncoderCount - iTC(35);
  350.  
  351.                     loopState++;
  352.                 }
  353.             break;
  354.  
  355.             case RED_JUDGES_DRIVE_TO_CRYPTOBOX + 2: // long, fast drive toward cryptobox
  356.                 leftDrive.setTargetPosition(leftEncoderEndCounts);
  357.                 rightDrive.setTargetPosition(rightEncoderEndCounts);
  358.                 headingFollow(-FAST_DRIVE_SPEED, -55, NORMAL_HEADING_GAIN, NORMAL_RAW_GYRO_GAIN);
  359.                 // left encoder count is negative, left encoder end counts is negative, constant is positive
  360.                 if(leftEncoderCount < (leftEncoderEndCounts + HEADING_TURN_OFF_DIST)) {
  361.                     loopState++;
  362.                 }
  363.             break;
  364.  
  365.             case RED_JUDGES_DRIVE_TO_CRYPTOBOX + 3: // turn toward cryptobox
  366.                 headingFollow(-FAST_DRIVE_SPEED, 0, 0, 0);
  367.                 if((Math.abs(leftEncoderCount - leftDrive.getTargetPosition()) < DRIVE_POS_TOLERANCE) &&
  368.                         (Math.abs(rightEncoderCount - rightDrive.getTargetPosition()) < DRIVE_POS_TOLERANCE)) {
  369.  
  370.                     leftDrive.setPower(0.0);
  371.                     rightDrive.setPower(0.0);
  372.                     loopState++;
  373.                 }
  374.             break;
  375.  
  376.             case RED_JUDGES_DRIVE_TO_CRYPTOBOX + 4:
  377.                 stopAndResetEncoders();
  378.                 if(leftEncoderCount == 0) {
  379.                     loopState++;
  380.                 }
  381.             break;
  382.  
  383.             case RED_JUDGES_DRIVE_TO_CRYPTOBOX + 5:
  384.                 runUsingEncoders();
  385.                 if(leftDrive.getMode() == DcMotor.RunMode.RUN_USING_ENCODER) {
  386.                     loopState++;
  387.                 }
  388.             break;
  389.  
  390.             case RED_JUDGES_DRIVE_TO_CRYPTOBOX + 6:
  391.                 autoIngestionElevation = true;
  392.                 glyphAtSpatula = true;
  393.                 autoElevatorState = AUTO_ELEVATOR_SPATULA_STATE; // push glyph into spatula
  394.                 autonomousCanPlace = false; // don't place glyph yet
  395.  
  396.                 headingFollow(-0.4, -90, NORMAL_HEADING_GAIN, NORMAL_RAW_GYRO_GAIN);
  397.                 loopState++;
  398.             break;
  399.  
  400.             case RED_JUDGES_DRIVE_TO_CRYPTOBOX + 7:
  401.                 if((autoElevatorState == STOP_STATE_MACHINE) && (alignGlyphState == STOP_STATE_MACHINE)) { // start aligning if ready
  402.                     alignGlyphState = START_STATE_MACHINE;
  403.                 }
  404.  
  405.                 if(has_drive_encoder_average_reached(iTC(2.5))) { // stop after driving enough
  406.  
  407.                     leftDrive.setPower(0.0);
  408.                     rightDrive.setPower(0.0);
  409.  
  410.                     if(alignGlyphState != STOP_STATE_MACHINE) { // wait for align to start
  411.                         loopState = PLACE_GLYPH;
  412.                     }
  413.  
  414.                 } else {
  415.                     headingFollow(-0.4, -90, NORMAL_HEADING_GAIN, NORMAL_RAW_GYRO_GAIN);
  416.                 }
  417.             break;
  418.  
  419.             ////////////////////////////////////////////////////////////////////////////////////////
  420.             // red audience ////////////////////////////////////////////////////////////////////////
  421.             ////////////////////////////////////////////////////////////////////////////////////////
  422.  
  423.             case RED_AUDIENCE_DRIVE_TO_CRYPTOBOX: // inherits distance targets from jewel_score
  424.                 headingFollow(FAST_DRIVE_SPEED, 0, NORMAL_HEADING_GAIN, NORMAL_RAW_GYRO_GAIN);
  425.                 if(has_drive_encoder_average_reached(iTC(29) - HEADING_TURN_OFF_DIST)) { // 2 inches before turn
  426.                     loopState++;
  427.                 }
  428.             break;
  429.  
  430.             case RED_AUDIENCE_DRIVE_TO_CRYPTOBOX + 1: // turn with no heading gain
  431.                 headingFollow(FAST_DRIVE_SPEED, 0, 0, 0);
  432.                 if((Math.abs(leftEncoderCount - leftDrive.getTargetPosition()) < DRIVE_POS_TOLERANCE) &&
  433.                         (Math.abs(rightEncoderCount - rightDrive.getTargetPosition()) < DRIVE_POS_TOLERANCE)) {
  434.  
  435.                     leftDrive.setPower(0.0);
  436.                     rightDrive.setPower(0.0);
  437.  
  438.                     // numbers for long, fast drive toward cryptobox
  439.                     leftEncoderEndCounts = leftEncoderCount - iTC(40.5);
  440.                     rightEncoderEndCounts = rightEncoderCount - iTC(54);
  441.  
  442.                     loopState++;
  443.                 }
  444.             break;
  445.  
  446.             case RED_AUDIENCE_DRIVE_TO_CRYPTOBOX + 2: // long, fast drive toward cryptobox
  447.                 leftDrive.setTargetPosition(leftEncoderEndCounts);
  448.                 rightDrive.setTargetPosition(rightEncoderEndCounts);
  449.                 headingFollow(-FAST_DRIVE_SPEED, -55, NORMAL_HEADING_GAIN, NORMAL_RAW_GYRO_GAIN);
  450.                 // left encoder count is negative, left encoder end counts is negative, constant is positive
  451.                 if(leftEncoderCount < (leftEncoderEndCounts + HEADING_TURN_OFF_DIST)) {
  452.                     loopState++;
  453.                 }
  454.             break;
  455.  
  456.             case RED_AUDIENCE_DRIVE_TO_CRYPTOBOX + 3: // turn toward cryptobox
  457.                 headingFollow(-FAST_DRIVE_SPEED, 0, 0, 0);
  458.                 if((Math.abs(leftEncoderCount - leftDrive.getTargetPosition()) < DRIVE_POS_TOLERANCE) &&
  459.                         (Math.abs(rightEncoderCount - rightDrive.getTargetPosition()) < DRIVE_POS_TOLERANCE)) {
  460.  
  461.                     leftDrive.setPower(0.0);
  462.                     rightDrive.setPower(0.0);
  463.                     loopState++;
  464.                 }
  465.             break;
  466.  
  467.             case RED_AUDIENCE_DRIVE_TO_CRYPTOBOX + 4:
  468.                 stopAndResetEncoders();
  469.                 if(leftEncoderCount == 0) {
  470.                     loopState++;
  471.                 }
  472.             break;
  473.  
  474.             case RED_AUDIENCE_DRIVE_TO_CRYPTOBOX + 5:
  475.                 runUsingEncoders();
  476.                 if(leftDrive.getMode() == DcMotor.RunMode.RUN_USING_ENCODER) {
  477.                     loopState++;
  478.                 }
  479.             break;
  480.  
  481.             case RED_AUDIENCE_DRIVE_TO_CRYPTOBOX + 6:
  482.                 autoIngestionElevation = true;
  483.                 glyphAtSpatula = true;
  484.                 autoElevatorState = AUTO_ELEVATOR_SPATULA_STATE; // push glyph into spatula
  485.                 autonomousCanPlace = false; // don't place glyph yet
  486.  
  487.                 headingFollow(-0.4, 0, NORMAL_HEADING_GAIN, NORMAL_RAW_GYRO_GAIN);
  488.                 loopState++;
  489.             break;
  490.  
  491.             case RED_AUDIENCE_DRIVE_TO_CRYPTOBOX + 7:
  492.                 if((autoElevatorState == STOP_STATE_MACHINE) && (alignGlyphState == STOP_STATE_MACHINE)) { // start aligning if ready
  493.                     alignGlyphState = START_STATE_MACHINE;
  494.                 }
  495.  
  496.                 headingFollow(-0.4, 0, NORMAL_HEADING_GAIN, NORMAL_RAW_GYRO_GAIN);
  497.                 if(has_drive_encoder_average_reached(iTC(14.5))) {
  498.  
  499.                     leftDrive.setPower(0.0);
  500.                     rightDrive.setPower(0.0);
  501.                     loopState = PLACE_GLYPH;
  502.                 }
  503.             break;
  504.  
  505.             ////////////////////////////////////////////////////////////////////////////////////////
  506.             // blue judges /////////////////////////////////////////////////////////////////////////
  507.             ////////////////////////////////////////////////////////////////////////////////////////
  508.  
  509.             case BLUE_JUDGES_DRIVE_TO_CRYPTOBOX: // inherits distance targets from jewel_score
  510.                 headingFollow(FAST_DRIVE_SPEED, 0, NORMAL_HEADING_GAIN, NORMAL_RAW_GYRO_GAIN);
  511.                 if(has_drive_encoder_average_reached(iTC(31) - HEADING_TURN_OFF_DIST)) {
  512.                     loopState++;
  513.                 }
  514.             break;
  515.  
  516.             case BLUE_JUDGES_DRIVE_TO_CRYPTOBOX + 1: // turn with no heading gain
  517.                 headingFollow(FAST_DRIVE_SPEED, 0, 0, 0);
  518.                 if((Math.abs(leftEncoderCount - leftDrive.getTargetPosition()) < DRIVE_POS_TOLERANCE) &&
  519.                         (Math.abs(rightEncoderCount - rightDrive.getTargetPosition()) < DRIVE_POS_TOLERANCE)) {
  520.  
  521.                     leftDrive.setPower(0.0);
  522.                     rightDrive.setPower(0.0);
  523.  
  524.                     // numbers for long, fast drive toward cryptobox
  525.                     leftEncoderEndCounts = leftEncoderCount - iTC(34);
  526.                     rightEncoderEndCounts = rightEncoderCount - iTC(43);
  527.  
  528.                     loopState++;
  529.                 }
  530.             break;
  531.  
  532.             case BLUE_JUDGES_DRIVE_TO_CRYPTOBOX + 2: // long, fast drive toward cryptobox
  533.                 leftDrive.setTargetPosition(leftEncoderEndCounts);
  534.                 rightDrive.setTargetPosition(rightEncoderEndCounts);
  535.                 headingFollow(-FAST_DRIVE_SPEED, -55, NORMAL_HEADING_GAIN, NORMAL_RAW_GYRO_GAIN);
  536.                 // left encoder count is negative, left encoder end counts is negative, constant is positive
  537.                 if(leftEncoderCount < (leftEncoderEndCounts + HEADING_TURN_OFF_DIST)) {
  538.                     loopState++;
  539.                 }
  540.             break;
  541.  
  542.             case BLUE_JUDGES_DRIVE_TO_CRYPTOBOX + 3: // turn toward cryptobox
  543.                 headingFollow(-FAST_DRIVE_SPEED, 0, 0, 0);
  544.                 if((Math.abs(leftEncoderCount - leftDrive.getTargetPosition()) < DRIVE_POS_TOLERANCE) &&
  545.                         (Math.abs(rightEncoderCount - rightDrive.getTargetPosition()) < DRIVE_POS_TOLERANCE)) {
  546.  
  547.                     leftDrive.setPower(0.0);
  548.                     rightDrive.setPower(0.0);
  549.                     loopState++;
  550.                 }
  551.             break;
  552.  
  553.             case BLUE_JUDGES_DRIVE_TO_CRYPTOBOX + 4:
  554.                 stopAndResetEncoders();
  555.                 if(leftEncoderCount == 0) {
  556.                     loopState++;
  557.                 }
  558.             break;
  559.  
  560.             case BLUE_JUDGES_DRIVE_TO_CRYPTOBOX + 5:
  561.                 runUsingEncoders();
  562.                 if(leftDrive.getMode() == DcMotor.RunMode.RUN_USING_ENCODER) {
  563.                     loopState++;
  564.                 }
  565.             break;
  566.  
  567.             case BLUE_JUDGES_DRIVE_TO_CRYPTOBOX + 6:
  568.                 autoIngestionElevation = true;
  569.                 glyphAtSpatula = true;
  570.                 autoElevatorState = AUTO_ELEVATOR_SPATULA_STATE; // push glyph into spatula
  571.                 autonomousCanPlace = false; // don't place glyph yet
  572.  
  573.                 headingFollow(-0.4, 90, NORMAL_HEADING_GAIN, NORMAL_RAW_GYRO_GAIN);
  574.                 loopState++;
  575.             break;
  576.  
  577.             case BLUE_JUDGES_DRIVE_TO_CRYPTOBOX + 7:
  578.                 if((autoElevatorState == STOP_STATE_MACHINE) && (alignGlyphState == STOP_STATE_MACHINE)) { // start aligning if ready
  579.                     alignGlyphState = START_STATE_MACHINE;
  580.                 }
  581.  
  582.                 if(has_drive_encoder_average_reached(iTC(5.5))) { // stop after driving enough
  583.  
  584.                     leftDrive.setPower(0.0);
  585.                     rightDrive.setPower(0.0);
  586.  
  587.                     if(alignGlyphState != STOP_STATE_MACHINE) { // wait for align to start
  588.                         loopState = PLACE_GLYPH;
  589.                     }
  590.  
  591.                 } else {
  592.                     headingFollow(-0.4, 90, NORMAL_HEADING_GAIN, NORMAL_RAW_GYRO_GAIN);
  593.                 }
  594.             break;
  595.  
  596.             ////////////////////////////////////////////////////////////////////////////////////////
  597.             // blue audience ///////////////////////////////////////////////////////////////////////
  598.             ////////////////////////////////////////////////////////////////////////////////////////
  599.  
  600.             case BLUE_AUDIENCE_DRIVE_TO_CRYPTOBOX: // inherits distance targets from jewel_score
  601.                 headingFollow(FAST_DRIVE_SPEED, 0, NORMAL_HEADING_GAIN, NORMAL_RAW_GYRO_GAIN);
  602.                 if(has_drive_encoder_average_reached(iTC(29) - HEADING_TURN_OFF_DIST)) { // 2 inches before turn
  603.                     loopState++;
  604.                 }
  605.             break;
  606.  
  607.             case BLUE_AUDIENCE_DRIVE_TO_CRYPTOBOX + 1: // turn with no heading gain
  608.                 headingFollow(FAST_DRIVE_SPEED, 0, 0, 0);
  609.                 if((Math.abs(leftEncoderCount - leftDrive.getTargetPosition()) < DRIVE_POS_TOLERANCE) &&
  610.                         (Math.abs(rightEncoderCount - rightDrive.getTargetPosition()) < DRIVE_POS_TOLERANCE)) {
  611.  
  612.                     leftDrive.setPower(0.0);
  613.                     rightDrive.setPower(0.0);
  614.  
  615.                     // numbers for long, fast drive toward cryptobox
  616.                     leftEncoderEndCounts = leftEncoderCount - iTC(55);
  617.                     rightEncoderEndCounts = rightEncoderCount - iTC(41.5);
  618.  
  619.                     loopState++;
  620.                 }
  621.             break;
  622.  
  623.             case BLUE_AUDIENCE_DRIVE_TO_CRYPTOBOX + 2: // long, fast drive toward cryptobox
  624.                 leftDrive.setTargetPosition(leftEncoderEndCounts);
  625.                 rightDrive.setTargetPosition(rightEncoderEndCounts);
  626.                 headingFollow(-FAST_DRIVE_SPEED, -55, NORMAL_HEADING_GAIN, NORMAL_RAW_GYRO_GAIN);
  627.                 // left encoder count is negative, left encoder end counts is negative, constant is positive
  628.                 if(leftEncoderCount < (leftEncoderEndCounts + HEADING_TURN_OFF_DIST)) {
  629.                     loopState++;
  630.                 }
  631.             break;
  632.  
  633.             case BLUE_AUDIENCE_DRIVE_TO_CRYPTOBOX + 3: // turn toward cryptobox
  634.                 headingFollow(-FAST_DRIVE_SPEED, 0, 0, 0);
  635.                 if((Math.abs(leftEncoderCount - leftDrive.getTargetPosition()) < DRIVE_POS_TOLERANCE) &&
  636.                         (Math.abs(rightEncoderCount - rightDrive.getTargetPosition()) < DRIVE_POS_TOLERANCE)) {
  637.  
  638.                     leftDrive.setPower(0.0);
  639.                     rightDrive.setPower(0.0);
  640.                     loopState++;
  641.                 }
  642.             break;
  643.  
  644.             case BLUE_AUDIENCE_DRIVE_TO_CRYPTOBOX + 4:
  645.                 stopAndResetEncoders();
  646.                 if(leftEncoderCount == 0) {
  647.                     loopState++;
  648.                 }
  649.             break;
  650.  
  651.             case BLUE_AUDIENCE_DRIVE_TO_CRYPTOBOX + 5:
  652.                 runUsingEncoders();
  653.                 if(leftDrive.getMode() == DcMotor.RunMode.RUN_USING_ENCODER) {
  654.                     loopState++;
  655.                 }
  656.             break;
  657.  
  658.             case BLUE_AUDIENCE_DRIVE_TO_CRYPTOBOX + 6:
  659.                 autoIngestionElevation = true;
  660.                 glyphAtSpatula = true;
  661.                 autoElevatorState = AUTO_ELEVATOR_SPATULA_STATE; // push glyph into spatula
  662.                 autonomousCanPlace = false; // don't place glyph yet
  663.  
  664.                 headingFollow(-0.4, 0, NORMAL_HEADING_GAIN, NORMAL_RAW_GYRO_GAIN);
  665.                 loopState++;
  666.             break;
  667.  
  668.             case BLUE_AUDIENCE_DRIVE_TO_CRYPTOBOX + 7:
  669.                 if((autoElevatorState == STOP_STATE_MACHINE) && (alignGlyphState == STOP_STATE_MACHINE)) { // start aligning if ready
  670.                     alignGlyphState = START_STATE_MACHINE;
  671.                 }
  672.  
  673.                 headingFollow(-0.4, 0, NORMAL_HEADING_GAIN, NORMAL_RAW_GYRO_GAIN);
  674.                 if(has_drive_encoder_average_reached(iTC(12.5))) {
  675.  
  676.                     leftDrive.setPower(0.0);
  677.                     rightDrive.setPower(0.0);
  678.                     loopState = PLACE_GLYPH;
  679.                 }
  680.             break;
  681.  
  682.             ////////////////////////////////////////////////////////////////////////////////////////
  683.             // place glyph  ////////////////////////////////////////////////////////////////////////
  684.             ////////////////////////////////////////////////////////////////////////////////////////
  685.  
  686.             case PLACE_GLYPH:
  687.                 autonomousCanPlace = true; // start dropping
  688.                 loopState++;
  689.             break;
  690.  
  691.             case PLACE_GLYPH + 1:
  692.                 stopAndResetEncoders();
  693.                 if(leftEncoderCount == 0) { // done lifting glyph
  694.                     loopState++;
  695.                 }
  696.             break;
  697.  
  698.             case PLACE_GLYPH + 2:
  699.                 runUsingEncoders();
  700.                 if(leftDrive.getMode() == DcMotor.RunMode.RUN_USING_ENCODER) {
  701.                     loopState++;
  702.                 }
  703.             break;
  704.  
  705.             case PLACE_GLYPH + 3: // square up, align glyph, and place glyph
  706.                 /*double alignHeading1 = cryptoboxAlignHeading();
  707.                 headingFollow(0, alignHeading1, SQUARE_UP_HEADING_GAIN, NORMAL_RAW_GYRO_GAIN);*/
  708.                 if(placeGlyphState >= PLACE_GLYPH_VERTICAL_STATE) { // done
  709.  
  710.                     autoTimeL = System.nanoTime();
  711.                     loopState = PARK;
  712.                 }
  713.             break;
  714.  
  715.             ////////////////////////////////////////////////////////////////////////////////////////
  716.             // get glyphs from pile ////////////////////////////////////////////////////////////////
  717.             ////////////////////////////////////////////////////////////////////////////////////////
  718.  
  719.             case GET_PILE_GLYPHS:
  720.                 columnToPlace = MID_COL;
  721.                 autoTimeN = System.nanoTime();
  722.                 headingFollow(0.1, 0, NORMAL_HEADING_GAIN, NORMAL_RAW_GYRO_GAIN);
  723.                 if(autoTimeN - autoTimeL > (1 * 1e9)) {
  724.                     loopState++;
  725.                 }
  726.             break;
  727.  
  728.             case GET_PILE_GLYPHS + 1: // go fast initially
  729.                 led.setPower(LED_RED);
  730.                 headingFollow(FAST_DRIVE_SPEED, 0, NORMAL_HEADING_GAIN, NORMAL_RAW_GYRO_GAIN);
  731.                 if(has_drive_encoder_average_reached(iTC(13))) {
  732.                     loopState++;
  733.                 }
  734.             break;
  735.  
  736.             case GET_PILE_GLYPHS + 2: // finish up slow
  737.                 led.setPower(LED_BLUE);
  738.                 headingFollow(0.4, 0, NORMAL_HEADING_GAIN, NORMAL_RAW_GYRO_GAIN);
  739.                 if(has_drive_encoder_average_reached(iTC(21))) {
  740.                     wiggleState = START_STATE_MACHINE; // start wiggle
  741.                     autoIngestionElevation = true; // start ingesting
  742.                     loopState++;
  743.                 }
  744.             break;
  745.  
  746.             case GET_PILE_GLYPHS + 3:
  747.                 if(glyphAtBack) {
  748.                     wiggleState = STOP_STATE_MACHINE; // stop wiggling
  749.                     autoTimeL = System.nanoTime(); // start timer to go straight
  750.                     loopState++;
  751.                 } else if(has_drive_encoder_average_reached(iTC(33))) {
  752.                     wiggleState = STOP_STATE_MACHINE;
  753.                     loopState = RETURN_FROM_PILE;
  754.                 }
  755.             break;
  756.  
  757.             case GET_PILE_GLYPHS + 4:
  758.                 leftDrive.setPower(FINE_ADJUST_DRIVE_SPEED);
  759.                 rightDrive.setPower(FINE_ADJUST_DRIVE_SPEED);
  760.                 intakeEject();
  761.  
  762.                 autoTimeN = System.nanoTime();
  763.                 if(autoTimeN - autoTimeL > (0.5 * 1e9)) {
  764.                     leftDrive.setPower(0.0);
  765.                     rightDrive.setPower(0.0);
  766.                     loopState++;
  767.                 }
  768.             break;
  769.  
  770.             case GET_PILE_GLYPHS + 5:
  771.                 headingFollow(-0.9, 0, NORMAL_HEADING_GAIN, NORMAL_RAW_GYRO_GAIN);
  772.                 intakeEject();
  773.                 if(driveEncoderAvg < iTC(10)) { // about 5 inches away
  774.                     autoTimeL = System.nanoTime();
  775.                     loopState++;
  776.                 }
  777.             break;
  778.  
  779.             case GET_PILE_GLYPHS + 6: // ram
  780.                 headingFollow(-0.4, 0, SQUARE_UP_HEADING_GAIN, NORMAL_RAW_GYRO_GAIN);
  781.                 intakeEject();
  782.  
  783.                 autoTimeN = System.nanoTime();
  784.                 if(autoTimeN - autoTimeL > (1 * 1e9)) {
  785.                     loopState++;
  786.                 }
  787.             break;
  788.  
  789.             case GET_PILE_GLYPHS + 7:
  790.                 stopAndResetEncoders();
  791.                 if((autoElevatorState == STOP_STATE_MACHINE) && (alignGlyphState == STOP_STATE_MACHINE)) { // start aligning
  792.                     autoIngestionElevation = false;
  793.                     alignGlyphState = START_STATE_MACHINE;
  794.                     autonomousCanPlace = true; // place right away
  795.                 }
  796.                 if(leftEncoderCount == 0) {
  797.                     loopState++;
  798.                 }
  799.             break;
  800.  
  801.             case GET_PILE_GLYPHS + 8:
  802.                 runUsingEncoders();
  803.                 if((autoElevatorState == STOP_STATE_MACHINE) && (alignGlyphState == STOP_STATE_MACHINE)) { // start aligning
  804.                     autoIngestionElevation = false;
  805.                     alignGlyphState = START_STATE_MACHINE;
  806.                     autonomousCanPlace = true; // place right away
  807.                 }
  808.                 if(leftDrive.getMode() == DcMotor.RunMode.RUN_USING_ENCODER) {
  809.                     autoTimeL = System.nanoTime();
  810.                     loopState++;
  811.                 }
  812.             break;
  813.  
  814.             case GET_PILE_GLYPHS + 9:
  815.                 headingFollow(0.4, 0, SQUARE_UP_HEADING_GAIN, NORMAL_RAW_GYRO_GAIN);
  816.  
  817.                 autoTimeN = System.nanoTime();
  818.                 if(autoTimeN - autoTimeL > (0.3 * 1e9)) {
  819.                     loopState++;
  820.                 }
  821.             break;
  822.  
  823.             case GET_PILE_GLYPHS + 10:
  824.                 double alignHeading = cryptoboxAlignHeading();
  825.                 headingFollow(0, alignHeading, SQUARE_UP_HEADING_GAIN, NORMAL_RAW_GYRO_GAIN);
  826.                 if((alignGlyphState == STOP_STATE_MACHINE) && (placeGlyphState == STOP_STATE_MACHINE) && !glyphAtSpatula) { // done
  827.                     autoTimeL = System.nanoTime();
  828.                     loopState++;
  829.                 }
  830.             break;
  831.  
  832.             case GET_PILE_GLYPHS + 11:
  833.                 headingFollow(0.4, 0, NORMAL_HEADING_GAIN, NORMAL_RAW_GYRO_GAIN);
  834.  
  835.                 autoTimeN = System.nanoTime();
  836.                 if(autoTimeN - autoTimeL > (1 * 1e9)) {
  837.                     loopState = AUTO_STOP;
  838.                 }
  839.             break;
  840.  
  841.             case RETURN_FROM_PILE:
  842.                 autoIngestionElevation = true;
  843.                 intakeEject();
  844.                 headingFollow(-0.9, 0, NORMAL_HEADING_GAIN, NORMAL_RAW_GYRO_GAIN);
  845.                 if(driveEncoderAvg < iTC(10)) {
  846.                     loopState = AUTO_STOP;
  847.                 }
  848.             break;
  849.  
  850.             ////////////////////////////////////////////////////////////////////////////////////////
  851.             // park ////////////////////////////////////////////////////////////////////////////////
  852.             ////////////////////////////////////////////////////////////////////////////////////////
  853.  
  854.             case PARK:
  855.                 double alignHeading1 = cryptoboxAlignHeading();
  856.                 autoTimeN = System.nanoTime();
  857.                 headingFollow(0.1, alignHeading1, NORMAL_HEADING_GAIN, NORMAL_RAW_GYRO_GAIN);
  858.                 if(autoTimeN - autoTimeL > (1 * 1e9)) {
  859.                     autoTimeL = autoTimeN;
  860.                     loopState++;
  861.                 }
  862.             break;
  863.  
  864.             case PARK + 1:
  865.                 double alignHeading2 = cryptoboxAlignHeading();
  866.                 autoTimeN = System.nanoTime();
  867.                 headingFollow(-0.3, alignHeading2, NORMAL_HEADING_GAIN, NORMAL_RAW_GYRO_GAIN);
  868.                 if(autoTimeN - autoTimeL > (1 * 1e9)) {
  869.                     autoTimeL = autoTimeN;
  870.                     loopState++;
  871.                 }
  872.             break;
  873.  
  874.             case PARK + 2:
  875.                 double alignHeading3 = cryptoboxAlignHeading();
  876.                 autoTimeN = System.nanoTime();
  877.                 headingFollow(0.3, alignHeading3, NORMAL_HEADING_GAIN, NORMAL_RAW_GYRO_GAIN);
  878.                 if(autoTimeN - autoTimeL > (1 * 1e9)) {
  879.                     loopState = AUTO_STOP;
  880.                 }
  881.             break;
  882.  
  883.             ////////////////////////////////////////////////////////////////////////////////////////
  884.             // abort ///////////////////////////////////////////////////////////////////////////////
  885.             ////////////////////////////////////////////////////////////////////////////////////////
  886.  
  887.             case AUTO_STOP:
  888.                 leftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  889.                 rightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  890.                 homeMotorsState = START_STATE_MACHINE; // home
  891.                 loopState++;
  892.             break;
  893.  
  894.             case AUTO_STOP + 1:
  895.                 autoIngestionElevation = false;
  896.                 intakeStop();
  897.             break;
  898.  
  899.         }
  900.  
  901.         // vuforia /////////////////////////////////////////////////////////////////////////////////
  902.  
  903.         if(runVuforia) {
  904.             RelicRecoveryVuMark vuMark = RelicRecoveryVuMark.from(relicTemplate);
  905.             if (vuMark != RelicRecoveryVuMark.UNKNOWN) { // we see something
  906.                 // store to appropriate column
  907.                 if(vuMark == RelicRecoveryVuMark.LEFT) {
  908.                     columnToPlace = LEFT_COL;
  909.                 }
  910.                 if(vuMark == RelicRecoveryVuMark.CENTER) {
  911.                     columnToPlace = MID_COL;
  912.                 }
  913.                 if(vuMark == RelicRecoveryVuMark.RIGHT) {
  914.                     columnToPlace = RIGHT_COL;
  915.                 }
  916.                 // deactivate vuforia
  917.                 runVuforia = false;
  918.                 relicTrackables.deactivate();
  919.                 vuMarkString = vuMark.toString();
  920.             }
  921.         }
  922.  
  923.         if(alignGlyphState != STOP_STATE_MACHINE) {
  924.             alignGlyph(columnToPlace);
  925.         }
  926.  
  927.         if(placeGlyphState != STOP_STATE_MACHINE) {
  928.             placeGlyph();
  929.         }
  930.  
  931.         if(homeMotorsState != STOP_STATE_MACHINE) {
  932.             homeMotors();
  933.         }
  934.  
  935.         if(wiggleState != STOP_STATE_MACHINE) {
  936.             wiggle(WIGGLE_SPEED, WIGGLE_WIDTH);
  937.         }
  938.  
  939.         // servos //////////////////////////////////////////////////////////////////////////////////
  940.  
  941.         // set cr servos
  942.         setCRServo(leftIntake, leftIntakePower, leftIntakeDirection);
  943.         setCRServo(rightIntake, rightIntakePower, rightIntakeDirection);
  944.         setCRServo(leftElevatorGears, leftElevatorGearsPower, leftElevatorGearsDirection);
  945.         setCRServo(rightElevatorGears, rightElevatorGearsPower, rightElevatorGearsDirection);
  946.         setCRServo(leftGlyphTransfer, leftGlyphTransferPower, leftGlyphTransferDirection);
  947.         setCRServo(rightGlyphTransfer, rightGlyphTransferPower, rightGlyphTransferDirection);
  948.  
  949.         // set regular servos
  950.         jewelShoulderPosition = Range.clip(jewelShoulderPosition, SERVO_MIN_RANGE, SERVO_MAX_RANGE);
  951.         jewelElbowPosition = Range.clip(jewelElbowPosition, SERVO_MIN_RANGE, SERVO_MAX_RANGE);
  952.         leftElevatorPosition = Range.clip(leftElevatorPosition, SERVO_MIN_RANGE, SERVO_MAX_RANGE);
  953.         rightElevatorPosition = Range.clip(rightElevatorPosition, SERVO_MIN_RANGE, SERVO_MAX_RANGE);
  954.         // whiskerPosition = Range.clip(whiskerPosition, SERVO_MIN_RANGE, SERVO_MAX_RANGE);
  955.  
  956.         jewelShoulder.setPosition(jewelShoulderPosition);
  957.         jewelElbow.setPosition(jewelElbowPosition);
  958.         leftElevator.setPosition(leftElevatorPosition);
  959.         rightElevator.setPosition(rightElevatorPosition);
  960.         // whisker.setPosition(whiskerPosition);
  961.  
  962.         // telemetry ///////////////////////////////////////////////////////////////////////////////
  963.         telemetry.addData("@", "state: " + String.format("%d", loopState));
  964.         telemetry.addData("5", "vuforia: " + String.format("%s", vuMarkString));
  965.  
  966.         // log values //////////////////////////////////////////////////////////////////////////////
  967.         try {
  968.             dl.addDataLine(loopTimeMS, !glyphAtEntrance, !glyphAtBack, !glyphAtPause, !glyphAtSpatula,
  969.                     leftElevatorGearsPower, rightElevatorGearsPower, heading, angularV, leftEncoderCount, rightEncoderCount, typewriterEncoderCount,
  970.                     loopState, alignGlyphState, placeGlyphState, /*leftDriveRunMode, rightDriveRunMode,*/ typewriterRunMode, DRIVE_OFF_STONE_HEADING_GAIN);
  971.         } catch (IOException e) {
  972.             e.printStackTrace();
  973.         }
  974.  
  975.         RobotLog.vv("M Log", "loop time: %d, BBs: %b, %b, %b, %b, " +
  976.                     "elevator gears powers: %.2f, %.2f, heading: %.2f, angular v: %.2f, " +
  977.                     "typewriter enc: %d, state: %d, glyph states: %d, %d, typewriter runmode: %s", loopTimeMS,
  978.                     !glyphAtEntrance, !glyphAtBack, !glyphAtPause, !glyphAtSpatula,
  979.                     leftElevatorGearsPower, rightElevatorGearsPower, heading, angularV, typewriterEncoderCount,
  980.                     loopState, alignGlyphState, placeGlyphState, typewriterRunMode);
  981.     }
  982.  
  983.     /*
  984.      * Code to run ONCE after the driver hits STOP
  985.      */
  986.     @Override
  987.     public void stop() {
  988.         super.stop();
  989.         dl.close();
  990.     }
  991.    
  992.     // other methods
  993.  
  994. }
Advertisement
Add Comment
Please, Sign In to add comment
Public Pastes
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!