API tools faq
paste
Advertisement
akg1729

Untitled

akg1729
Feb 9th, 2019
102
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Java 23.83 KB | None | 0 0
raw download clone embed print report
  1. package org.firstinspires.ftc.teamcode.OutOfOrder14235.Autonomous;
  2.  
  3. import com.qualcomm.hardware.bosch.BNO055IMU;
  4. import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cGyro;
  5. import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
  6. import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
  7. import com.qualcomm.robotcore.hardware.DcMotor;
  8. import com.qualcomm.robotcore.util.ElapsedTime;
  9. import com.qualcomm.robotcore.util.Range;
  10.  
  11. import org.firstinspires.ftc.robotcore.external.ClassFactory;
  12. import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
  13. import org.firstinspires.ftc.robotcore.external.navigation.Acceleration;
  14. import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
  15. import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
  16. import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
  17. import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
  18. import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
  19. import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;
  20.  
  21. import java.util.List;
  22. import java.util.Locale;
  23.  
  24. @Autonomous
  25. public class LeagueChampsAutonomousCraterSampleOnLanderMRGYRO extends LinearOpMode{
  26.     HardwareRobot robot;
  27.     ModernRoboticsI2cGyro gyro;                    // Additional Gyro device
  28.  
  29.  
  30.     static final double     COUNTS_PER_MOTOR_REV    = 1440 ;    // eg: TETRIX Motor Encoder
  31.     static final double     DRIVE_GEAR_REDUCTION    = 1.0 ;     // This is < 1.0 if geared UP
  32.     static final double     WHEEL_DIAMETER_INCHES   = 4.0 ;     // For figuring circumference
  33.     static final double     COUNTS_PER_INCH         = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) /
  34.             (WHEEL_DIAMETER_INCHES * 3.1415);
  35.  
  36.     // These constants define the desired driving/control characteristics
  37.     // The can/should be tweaked to suite the specific robot drive train.
  38.     static final double     DRIVE_SPEED             = 0.7;     // Nominal speed for better accuracy.
  39.     static final double     TURN_SPEED              = 0.5;     // Nominal half speed for better accuracy.
  40.  
  41.     static final double     HEADING_THRESHOLD       = 1 ;      // As tight as we can make it with an integer gyro
  42.     static final double     P_TURN_COEFF            = 0.1;     // Larger is more responsive, but also less stable
  43.     static final double     P_DRIVE_COEFF           = 0.15;     // Larger is more responsive, but also less stable
  44.  
  45.     enum MineralPosition
  46.     {
  47.         LEFT, CENTER, RIGHT;
  48.     }
  49.     private ElapsedTime runtime  = new ElapsedTime();
  50.  
  51.     private static final String TFOD_MODEL_ASSET = "RoverRuckus.tflite";
  52.     private static final String LABEL_GOLD_MINERAL = "Gold Mineral";
  53.     private static final String LABEL_SILVER_MINERAL = "Silver Mineral";
  54.     private static final String VUFORIA_KEY = "AXykNg//////AAABmdVQDVwq9kAEsspJU9r8u8VmSeZBFzHTHr6fsWSjYKVlHaAw6uE0fxEJ0zCNaIbGmpOSWf0NY/pFNh4N5uYYtL99ymMWhR2tfuIBXgo7T4m8ht7lStZtjHjmcmO0nQBzzGCm74gw+CDYvRbfDYtr95fNuoMIcyZUiv2TpUcsbebE+fT6HEfyGXyF1j4d6CEzWc1Qhdy+nCCC3kO/5oDt8usf3ryOzBgFW/l4l+YEqk1LVw1vrx4+DhiqQ87ohJDybGab6FvqxC2Hlryx0p7BdmwCtQqfaRD8s8icv7XUR09Xlij02Z5iRe/7+aJc44fxmu3xTB17y3r8Er0YmqVvU3EChH5p0+SiHl+z36p78c1J";
  55.     private VuforiaLocalizer vuforia;
  56.     private TFObjectDetector tfod;
  57.     BNO055IMU               imu;
  58.     private Orientation angles;
  59.     private Acceleration gravity;
  60.  
  61.     MineralPosition position;
  62.     public void runOpMode() {
  63.         robot = new HardwareRobot();
  64.         robot.init(hardwareMap);
  65.         imu = hardwareMap.get(BNO055IMU.class, "imuBase");
  66.  
  67.         robot.linearActuator.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  68.         robot.linearActuator.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  69.         robot.linExt.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  70.         robot.linExt.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  71.         gyro =  hardwareMap.get(ModernRoboticsI2cGyro.class, "gyro");
  72.  
  73.         // Ensure the robot it stationary, then reset the encoders and calibrate the gyro.
  74.         robot.leftWheel.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  75.         robot.rightWheel.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  76.         // Send telemetry message to alert driver that we are calibrating;
  77.         telemetry.addData(">", "Calibrating Gyro");    //
  78.         telemetry.update();
  79.  
  80.         gyro.calibrate();
  81.  
  82.         // make sure the gyro is calibrated before continuing
  83.         while (!isStopRequested() && gyro.isCalibrating())  {
  84.             sleep(50);
  85.             idle();
  86.         }
  87.  
  88.         telemetry.addData(">", "Gyro Done.");
  89.         telemetry.update();
  90.  
  91.         robot.leftWheel.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  92.         robot.rightWheel.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  93.  
  94.         initVuforia();
  95.         robot.markerDropper.setPosition(.3);
  96.         if (ClassFactory.getInstance().canCreateTFObjectDetector()) {
  97.             initTfod();
  98.         } else telemetry.addData("Sorry!", "This device is not compatible with TFOD");
  99.  
  100.         BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
  101.         parameters.angleUnit            = BNO055IMU.AngleUnit.DEGREES;
  102.         parameters.accelUnit            = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
  103.         parameters.loggingEnabled       = true;
  104.         parameters.useExternalCrystal   = true;
  105.         parameters.mode                 = BNO055IMU.SensorMode.IMU;
  106.         parameters.loggingTag           = "IMU";
  107.         imu                             = hardwareMap.get(BNO055IMU.class, "imu");
  108.  
  109.         imu.initialize(parameters);
  110.         /** Wait for the game to begin */
  111.         telemetry.addData(">", "Press Play to start tracking");
  112.         telemetry.update();
  113.  
  114.  
  115.  
  116.  
  117.  
  118.  
  119.         telemetry.addData("Mode", "waiting for start DO NOT START UNTIL READY TO GO MSG");
  120.         telemetry.update();
  121.         telemetry.addLine("AUTONOMOUS READY TO GO");
  122.         telemetry.update();
  123.         waitForStart();
  124.         gyro.resetZAxisIntegrator();
  125.         if (opModeIsActive()) {
  126.             /* Activate Tensor Flow Object Detection. */
  127.             if (tfod != null) {
  128.                 tfod.activate();
  129.             }
  130.  
  131.             while (opModeIsActive()) {
  132.                 if (tfod != null) {
  133.                     // getUpdatedRecognitions() will return null if no new information is available since
  134.                     // the last time that call was made.
  135.                     List<Recognition> updatedRecognitions = tfod.getUpdatedRecognitions();
  136.                     if (updatedRecognitions != null) {
  137.                         telemetry.addData("# Object Detected", updatedRecognitions.size());
  138.                         if (updatedRecognitions.size() == 3) {
  139.                             int goldMineralX = -1;
  140.                             int silverMineral1X = -1;
  141.                             int silverMineral2X = -1;
  142.                             for (Recognition recognition : updatedRecognitions) {
  143.                                 if (recognition.getLabel().equals(LABEL_GOLD_MINERAL)) {
  144.                                     goldMineralX = (int) recognition.getLeft();
  145.                                 } else if (silverMineral1X == -1) {
  146.                                     silverMineral1X = (int) recognition.getLeft();
  147.                                 } else {
  148.                                     silverMineral2X = (int) recognition.getLeft();
  149.                                 }
  150.                             }
  151.  
  152.                             if (goldMineralX != -1 && silverMineral1X != -1 && silverMineral2X != -1) {
  153.                                 if (goldMineralX < silverMineral1X && goldMineralX < silverMineral2X) {
  154.                                     position = LeagueChampsAutonomousCraterSampleOnLanderMRGYRO.MineralPosition.LEFT;
  155.                                     telemetry.addData("Gold Mineral Position", "Left");
  156.                                     telemetry.update();
  157.                                     break;
  158.  
  159.  
  160.                                 } else if (goldMineralX > silverMineral1X && goldMineralX > silverMineral2X) {
  161.                                     position = LeagueChampsAutonomousCraterSampleOnLanderMRGYRO.MineralPosition.RIGHT;
  162.  
  163.                                     telemetry.addData("Gold Mineral Position", "Right");
  164.                                     telemetry.update();
  165.                                     break;
  166.  
  167.  
  168.                                 } else  if (goldMineralX > silverMineral1X && goldMineralX < silverMineral2X || goldMineralX < silverMineral1X && goldMineralX > silverMineral2X ) {
  169.                                     position = LeagueChampsAutonomousCraterSampleOnLanderMRGYRO.MineralPosition.CENTER;
  170.  
  171.                                     telemetry.addData("Gold Mineral Position", "Center");
  172.                                     telemetry.update();
  173.                                     break;
  174.  
  175.                                 }
  176.                                 else{
  177.                                     Meet3AutonomousDepotHardCodedIMUSampleOnLander.MineralPosition position = Meet3AutonomousDepotHardCodedIMUSampleOnLander.MineralPosition.LEFT;
  178.                                     telemetry.addData("Gold Mineral Position", "NOT DETECTED: GUESSING LEFT");
  179.                                     telemetry.update();
  180.                                     break;
  181.  
  182.                                 }
  183.                             }
  184.  
  185.                         }
  186.                         telemetry.update();
  187.                     }
  188.                 }
  189.             }
  190.         }
  191.  
  192.         if (tfod != null) {
  193.             tfod.shutdown();
  194.         }
  195.  
  196.         robot.linearActuator.setTargetPosition(-11630);
  197.         robot.linearActuator.setPower(.9);
  198.  
  199.         while (opModeIsActive() && robot.linearActuator.isBusy())
  200.         {
  201.             telemetry.addData("LINEAR ACTUATOR ENCODER", robot.linearActuator.getCurrentPosition() + "  busy=" + robot.linearActuator.isBusy());
  202.             telemetry.update();
  203.             idle();
  204.         }
  205.  
  206.         robot.linearActuator.setPower(0.0);
  207.         sleep(300);
  208.         while(robot.distanceBottom.getDistance(DistanceUnit.CM) >= 8.7 ){
  209.  
  210.             robot.linearActuator.setPower(.3);
  211.  
  212.         }
  213.         robot.linearActuator.setPower(0.0);
  214.         sleep(300);
  215.         telemetry.addData("Delatched!", 1);
  216.         telemetry.update();
  217.  
  218.         StopDriving();
  219.         DriveForward(.83);
  220.         sleep(1200);
  221.         StopDriving();
  222.         ShiftRight(.8);
  223.         sleep(660);
  224.         StopDriving();
  225.         DriveBackward(.8);
  226.         sleep(1300);
  227.         StopDriving();
  228.         gyroTurn(.4,0);
  229.         StopDriving();
  230.         telemetry.addData("Checkpoint Reached!!", 1);
  231.         telemetry.update();
  232.  
  233.         if(position == LeagueChampsAutonomousCraterSampleOnLanderMRGYRO.MineralPosition.LEFT ){
  234.  
  235.             ShiftRight(.7);
  236.             sleep(1000);
  237.             StopDriving();
  238.             DriveForward(1);
  239.             sleep(1400);
  240.             StopDriving();
  241.  
  242.             ShiftRight(.9);
  243.             sleep(1000);
  244.             StopDriving();
  245.             ShiftRight(-.9);
  246.             sleep(1150);
  247.             StopDriving();
  248.  
  249.             gyroTurn(.5,0);
  250.                 DriveForward(1);
  251.                 sleep(5000);
  252.  
  253.  
  254.             StopDriving();//needs work
  255.  
  256.         }
  257.         else if(position == LeagueChampsAutonomousCraterSampleOnLanderMRGYRO.MineralPosition.RIGHT){
  258.  
  259.             ShiftRight(.5);
  260.             sleep(1750);
  261.             StopDriving();
  262.             DriveForward(-1);
  263.             sleep(1500);
  264.             StopDriving();
  265.             ShiftRight(1);
  266.             sleep(1000);
  267.             StopDriving();
  268.             ShiftRight(-.9);
  269.             sleep(1150);
  270.             StopDriving();
  271.  
  272.             gyroTurn(.5,0);
  273.             DriveForward(1);
  274.             sleep(7000);
  275.  
  276.  
  277.             StopDriving();//
  278.  
  279.  
  280.         }
  281.         else if(position == LeagueChampsAutonomousCraterSampleOnLanderMRGYRO.MineralPosition.CENTER){
  282.             ShiftRight(1);
  283.             sleep(1200);
  284.             StopDriving();
  285.             ShiftRight(-.9);
  286.             sleep(1150);
  287.             StopDriving();
  288.  
  289.             gyroTurn(.5,0);
  290.             DriveForward(1);
  291.             sleep(6000);
  292.  
  293.  
  294.             StopDriving();//
  295.         }
  296.  
  297.  
  298.  
  299.     }
  300.  
  301.     public void waiting(long millis){
  302.         long t = System.currentTimeMillis();
  303.         while(opModeIsActive()&&System.currentTimeMillis()-t<millis){
  304.  
  305.         }
  306.     }
  307.  
  308.  
  309.     /**
  310.      * Initialize the Vuforia localization engine.
  311.      */
  312.     private void initVuforia() {
  313.         /*
  314.          * Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine.
  315.          */
  316.         VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
  317.  
  318.         parameters.vuforiaLicenseKey = VUFORIA_KEY;
  319.         parameters.cameraName = hardwareMap.get(WebcamName.class, "Webcam 1");
  320.  
  321.         //  Instantiate the Vuforia engine
  322.         vuforia = ClassFactory.getInstance().createVuforia(parameters);
  323.  
  324.         // Loading trackables is not necessary for the Tensor Flow Object Detection engine.
  325.     }
  326.  
  327.     /**
  328.      * Initialize the Tensor Flow Object Detection engine.
  329.      */
  330.     private void initTfod() {
  331.         int tfodMonitorViewId = hardwareMap.appContext.getResources().getIdentifier(
  332.                 "tfodMonitorViewId", "id", hardwareMap.appContext.getPackageName());
  333.         TFObjectDetector.Parameters tfodParameters = new TFObjectDetector.Parameters(tfodMonitorViewId);
  334.         tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia);
  335.         tfod.loadModelFromAsset(TFOD_MODEL_ASSET, LABEL_GOLD_MINERAL, LABEL_SILVER_MINERAL);
  336.     }
  337.     double DRIVE_POWER = 1.0;
  338.     public void TurnLeft(double lPower, double rPower){
  339.         robot.leftWheel.setPower(lPower);
  340.         robot.rightWheel.setPower(rPower);
  341.  
  342.     }
  343.     public void StopMoving (){
  344.         robot.leftWheel.setPower(0);
  345.         robot.rightWheel.setPower(0);
  346.         robot.centerWheel.setPower(0);
  347.     }
  348.     public void TurnRight(double lPower, double rPower){
  349.         robot.leftWheel.setPower(lPower);
  350.         robot.rightWheel.setPower(rPower);
  351.  
  352.     }
  353.  
  354.     public void DriveForward(double power){
  355.         robot.leftWheel.setPower(power);
  356.         robot.rightWheel.setPower(power);
  357.  
  358.     }
  359.     public void DriveBackward(double power){
  360.         robot.leftWheel.setPower(-power);
  361.         robot.rightWheel.setPower(-power);
  362.  
  363.     }
  364.     public void ShiftLeft(double power){
  365.         robot.centerWheel.setPower(power);
  366.     }
  367.     public void ShiftRight(double power){
  368.         robot.centerWheel.setPower(-power);
  369.     }
  370.     public void StopDriving(){
  371.         robot.leftWheel.setPower(0);
  372.         robot.rightWheel.setPower(0);
  373.         robot.centerWheel.setPower(0);
  374.  
  375.     }
  376.     void sendTelemetry()
  377.     {
  378.         telemetry.addData("Status", imu.getSystemStatus().toString());
  379.         telemetry.addData("Calib", imu.getCalibrationStatus().toString());
  380.         telemetry.addData("Heading", formatAngle(angles.angleUnit, angles.firstAngle));
  381.         telemetry.addData("Roll", formatAngle(angles.angleUnit, angles.secondAngle));
  382.         telemetry.addData("Pitch", formatAngle(angles.angleUnit, angles.thirdAngle));
  383.         telemetry.addData("Grav", gravity.toString());
  384.         telemetry.update();
  385.     }
  386.  
  387.     String formatAngle(AngleUnit angleUnit, double angle)
  388.     {
  389.         return formatDegrees(AngleUnit.DEGREES.fromUnit(angleUnit, angle));
  390.     }
  391.  
  392.     String formatDegrees(double degrees)
  393.     {
  394.         return String.format(Locale.getDefault(), "%.1f", AngleUnit.DEGREES.normalize(degrees));
  395.     }
  396.  
  397.  
  398.     /**
  399.      *  Method to drive on a fixed compass bearing (angle), based on encoder counts.
  400.      *  Move will stop if either of these conditions occur:
  401.      *  1) Move gets to the desired position
  402.      *  2) Driver stops the opmode running.
  403.      *
  404.      * @param speed      Target speed for forward motion.  Should allow for _/- variance for adjusting heading
  405.      * @param distance   Distance (in inches) to move from current position.  Negative distance means move backwards.
  406.      * @param angle      Absolute Angle (in Degrees) relative to last gyro reset.
  407.      *                   0 = fwd. +ve is CCW from fwd. -ve is CW from forward.
  408.      *                   If a relative angle is required, add/subtract from current heading.
  409.      */
  410.     public void gyroDrive ( double speed,
  411.                             double distance,
  412.                             double angle) {
  413.  
  414.         int     newLeftTarget;
  415.         int     newRightTarget;
  416.         int     moveCounts;
  417.         double  max;
  418.         double  error;
  419.         double  steer;
  420.         double  leftSpeed;
  421.         double  rightSpeed;
  422.  
  423.         // Ensure that the opmode is still active
  424.         if (opModeIsActive()) {
  425.  
  426.             // Determine new target position, and pass to motor controller
  427.             moveCounts = (int)(distance * COUNTS_PER_INCH);
  428.             newLeftTarget = robot.leftWheel.getCurrentPosition() + moveCounts;
  429.             newRightTarget = robot.rightWheel.getCurrentPosition() + moveCounts;
  430.  
  431.             // Set Target and Turn On RUN_TO_POSITION
  432.             robot.leftWheel.setTargetPosition(newLeftTarget);
  433.             robot.rightWheel.setTargetPosition(newRightTarget);
  434.  
  435.             robot.leftWheel.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  436.             robot.rightWheel.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  437.  
  438.             // start motion.
  439.             speed = Range.clip(Math.abs(speed), 0.0, 1.0);
  440.             robot.leftWheel.setPower(speed);
  441.             robot.rightWheel.setPower(speed);
  442.  
  443.             // keep looping while we are still active, and BOTH motors are running.
  444.             while (opModeIsActive() &&
  445.                     (robot.leftWheel.isBusy() && robot.rightWheel.isBusy())) {
  446.  
  447.                 // adjust relative speed based on heading error.
  448.                 error = getError(angle);
  449.                 steer = getSteer(error, P_DRIVE_COEFF);
  450.  
  451.                 // if driving in reverse, the motor correction also needs to be reversed
  452.                 if (distance < 0)
  453.                     steer *= -1.0;
  454.  
  455.                 leftSpeed = speed - steer;
  456.                 rightSpeed = speed + steer;
  457.  
  458.                 // Normalize speeds if either one exceeds +/- 1.0;
  459.                 max = Math.max(Math.abs(leftSpeed), Math.abs(rightSpeed));
  460.                 if (max > 1.0)
  461.                 {
  462.                     leftSpeed /= max;
  463.                     rightSpeed /= max;
  464.                 }
  465.  
  466.                 robot.leftWheel.setPower(leftSpeed);
  467.                 robot.rightWheel.setPower(rightSpeed);
  468.  
  469.                 // Display drive status for the driver.
  470.                 telemetry.addData("Err/St",  "%5.1f/%5.1f",  error, steer);
  471.                 telemetry.addData("Target",  "%7d:%7d",      newLeftTarget,  newRightTarget);
  472.                 telemetry.addData("Actual",  "%7d:%7d",      robot.leftWheel.getCurrentPosition(),
  473.                         robot.rightWheel.getCurrentPosition());
  474.                 telemetry.addData("Speed",   "%5.2f:%5.2f",  leftSpeed, rightSpeed);
  475.                 telemetry.update();
  476.             }
  477.  
  478.             // Stop all motion;
  479.             robot.leftWheel.setPower(0);
  480.             robot.rightWheel.setPower(0);
  481.  
  482.             // Turn off RUN_TO_POSITION
  483.             robot.leftWheel.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  484.             robot.rightWheel.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  485.         }
  486.     }
  487.  
  488.     /**
  489.      *  Method to spin on central axis to point in a new direction.
  490.      *  Move will stop if either of these conditions occur:
  491.      *  1) Move gets to the heading (angle)
  492.      *  2) Driver stops the opmode running.
  493.      *
  494.      * @param speed Desired speed of turn.
  495.      * @param angle      Absolute Angle (in Degrees) relative to last gyro reset.
  496.      *                   0 = fwd. +ve is CCW from fwd. -ve is CW from forward.
  497.      *                   If a relative angle is required, add/subtract from current heading.
  498.      **/
  499.     public void gyroTurn (  double speed, double angle) {
  500.  
  501.         // keep looping while we are still active, and not on heading.
  502.         while (opModeIsActive() && !onHeading(speed, angle, P_TURN_COEFF)) {
  503.             // Update telemetry & Allow time for other processes to run.
  504.             telemetry.update();
  505.         }
  506.     }
  507.  
  508.     /**
  509.      *  Method to obtain & hold a heading for a finite amount of time
  510.      *  Move will stop once the requested time has elapsed
  511.      *
  512.      * @param speed      Desired speed of turn.
  513.      * @param angle      Absolute Angle (in Degrees) relative to last gyro reset.
  514.      *                   0 = fwd. +ve is CCW from fwd. -ve is CW from forward.
  515.      *                   If a relative angle is required, add/subtract from current heading.
  516.      * @param holdTime   Length of time (in seconds) to hold the specified heading.
  517.      */
  518.     public void gyroHold( double speed, double angle, double holdTime) {
  519.  
  520.         ElapsedTime holdTimer = new ElapsedTime();
  521.  
  522.         // keep looping while we have time remaining.
  523.         holdTimer.reset();
  524.         while (opModeIsActive() && (holdTimer.time() < holdTime)) {
  525.             // Update telemetry & Allow time for other processes to run.
  526.             onHeading(speed, angle, P_TURN_COEFF);
  527.             telemetry.update();
  528.         }
  529.  
  530.         // Stop all motion;
  531.         robot.leftWheel.setPower(0);
  532.         robot.rightWheel.setPower(0);
  533.     }
  534.  
  535.     /**
  536.      * Perform one cycle of closed loop heading control.
  537.      *
  538.      * @param speed     Desired speed of turn.
  539.      * @param angle     Absolute Angle (in Degrees) relative to last gyro reset.
  540.      *                  0 = fwd. +ve is CCW from fwd. -ve is CW from forward.
  541.      *                  If a relative angle is required, add/subtract from current heading.
  542.      * @param PCoeff    Proportional Gain coefficient
  543.      * @return
  544.      */
  545.     boolean onHeading(double speed, double angle, double PCoeff) {
  546.         double   error ;
  547.         double   steer ;
  548.         boolean  onTarget = false ;
  549.         double leftSpeed;
  550.         double rightSpeed;
  551.  
  552.         // determine turn power based on +/- error
  553.         error = getError(angle);
  554.  
  555.         if (Math.abs(error) <= HEADING_THRESHOLD) {
  556.             steer = 0.0;
  557.             leftSpeed  = 0.0;
  558.             rightSpeed = 0.0;
  559.             onTarget = true;
  560.         }
  561.         else {
  562.             steer = getSteer(error, PCoeff);
  563.             rightSpeed  = speed * steer;
  564.             leftSpeed   = -rightSpeed;
  565.         }
  566.  
  567.         // Send desired speeds to motors.
  568.         robot.leftWheel.setPower(leftSpeed);
  569.         robot.rightWheel.setPower(rightSpeed);
  570.  
  571.         // Display it for the driver.
  572.         telemetry.addData("Target", "%5.2f", angle);
  573.         telemetry.addData("Err/St", "%5.2f/%5.2f", error, steer);
  574.         telemetry.addData("Speed.", "%5.2f:%5.2f", leftSpeed, rightSpeed);
  575.  
  576.         return onTarget;
  577.     }
  578.  
  579.     /**
  580.      * getError determines the error between the target angle and the robot's current heading
  581.      * @param   targetAngle  Desired angle (relative to global reference established at last Gyro Reset).
  582.      * @return  error angle: Degrees in the range +/- 180. Centered on the robot's frame of reference
  583.      *          +ve error means the robot should turn LEFT (CCW) to reduce error.
  584.      */
  585.     public double getError(double targetAngle) {
  586.  
  587.         double robotError;
  588.  
  589.         // calculate error in -179 to +180 range  (
  590.         robotError = targetAngle - gyro.getIntegratedZValue();
  591.         while (robotError > 180)  robotError -= 360;
  592.         while (robotError <= -180) robotError += 360;
  593.         return robotError;
  594.     }
  595.  
  596.     /**
  597.      * returns desired steering force.  +/- 1 range.  +ve = steer left
  598.      * @param error   Error angle in robot relative degrees
  599.      * @param PCoeff  Proportional Gain Coefficient
  600.      * @return
  601.      */
  602.     public double getSteer(double error, double PCoeff) {
  603.         return Range.clip(error * PCoeff, -1, 1);
  604.     }
  605.  
  606.  
  607. }
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!