Public Pastes
SHARE
TWEET

teleop

a guest Nov 22nd, 2016 16 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. package org.firstinspires.ftc.teamcode;
  2.  
  3. import com.qualcomm.robotcore.eventloop.opmode.Disabled;
  4. import com.qualcomm.robotcore.eventloop.opmode.OpMode;
  5. import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
  6. import com.qualcomm.robotcore.util.ElapsedTime;
  7. import com.qualcomm.robotcore.util.Range;
  8.  
  9.  
  10. @TeleOp
  11. //@Disabled
  12. public class Teleop1 extends OpMode
  13. {
  14.     /* Declare OpMode members. */
  15.     RobotHardware robot         = new RobotHardware();
  16.     private ElapsedTime runtime = new ElapsedTime();
  17.  
  18.     /*
  19.      * Code to run ONCE when the driver hits INIT
  20.      */
  21.     @Override
  22.     public void init() {
  23.  
  24.         telemetry.addData("Status", "Initialized");
  25.         updateTelemetry(telemetry);
  26.  
  27.         robot.init(hardwareMap);
  28.  
  29.         robot.buttonPushPosition = 0;
  30.  
  31.     }
  32.  
  33.     /*
  34.      * Code to run REPEATEDLY after the driver hits INIT, but before they hit PLAY
  35.      */
  36.     @Override
  37.     public void init_loop() {
  38.  
  39.     }
  40.  
  41.     /*
  42.      * Code to run ONCE when the driver hits PLAY
  43.      */
  44.     @Override
  45.     public void start() {
  46.         runtime.reset();
  47.     }
  48.  
  49.     {
  50.  
  51.         /*
  52.          * Gamepad 1
  53.          *
  54.          * Gamepad 1 controls the motors via the left and right sticks
  55.          */
  56.  
  57.         // tank drive
  58.         // note that if y equal -1 then joystick is pushed all of the way forward.
  59.         float left = -gamepad1.left_stick_y;
  60.         float right = -gamepad1.right_stick_y;
  61.  
  62.         // clip the right/left values so that the values never exceed +/- 1
  63.         right = Range.clip(right, -1, 1);
  64.         left = Range.clip(left, -1, 1);
  65.  
  66.         // scale the joystick value to make it easier to control
  67.         // the robot more precisely at slower speeds.
  68.         right = (float) scaleInput(right);
  69.         left = (float) scaleInput(left);
  70.  
  71.         // write the values to the motors
  72.         robot.driveRight.setPower(right);
  73.         robot.driveLeft.setPower(left);
  74.  
  75.         if (gamepad1.a) {
  76.             robot.buttonPushPosition += robot.buttonPushDelta;
  77.  
  78.     }
  79.         if (gamepad1.b) {
  80.             robot.buttonPushPosition -= robot.buttonPushDelta;
  81.         }
  82.  
  83.         /*
  84.          * Gamepad 2
  85.          *
  86.          * Gamepad 2 controls the linear slide and shooter system
  87.          */
  88.  
  89.  
  90.         if (gamepad2.a) {
  91.             robot.shooterIntake.setPower(1);
  92.         } else {
  93.                 robot.shooterIntake.setPower(0);
  94.         }
  95.  
  96.  
  97.         if (gamepad2.b) {
  98.             robot.shooterIntake.setPower(-1);
  99.         } else {
  100.             robot.shooterIntake.setPower(0);
  101.         }
  102.  
  103.  
  104.         if (gamepad2.x) {
  105.             robot.shooterLaunch.setPower(1);
  106.         } else {
  107.             robot.shooterLaunch.setPower(0);
  108.         }
  109.  
  110.  
  111.         if (gamepad2.y) {
  112.             robot.shooterLaunch.setPower(-1);
  113.         } else {
  114.             robot.shooterLaunch.setPower(0);
  115.         }
  116.  
  117.  
  118.         if (gamepad2.left_bumper) {
  119.             robot.linearSlide.setPower(-1);
  120.         } else {
  121.             robot.linearSlide.setPower(0);
  122.         }
  123.  
  124.  
  125.         if (gamepad2.right_bumper) {
  126.             robot.linearSlide.setPower(1);
  127.         } else {
  128.             robot.linearSlide.setPower(0);
  129.         }
  130.  
  131.     }
  132.     /*
  133.      * Code to run REPEATEDLY after the driver hits PLAY but before they hit STOP
  134.      */
  135.     @Override
  136.     public void loop() {
  137.         telemetry.addData("Status", "Running: " + runtime.toString());
  138.  
  139.         // eg: Run wheels in tank mode (note: The joystick goes negative when pushed forwards)
  140.         // leftMotor.setPower(-gamepad1.left_stick_y);
  141.         // rightMotor.setPower(-gamepad1.right_stick_y);
  142.     }
  143.  
  144.     /*
  145.      * Code to run ONCE after the driver hits STOP
  146.      */
  147.     @Override
  148.     public void stop() {
  149.     }
  150.  
  151.     double scaleInput(double dVal)  {
  152.         double[] scaleArray = { 0.0, 0.05, 0.09, 0.10, 0.12, 0.15, 0.18, 0.24,
  153.                 0.30, 0.36, 0.43, 0.50, 0.60, 0.72, 0.85, 1.00, 1.00 };
  154.  
  155.         // get the corresponding index for the scaleInput array.
  156.         int index = (int) (dVal * 16.0);
  157.  
  158.         // index should be positive.
  159.         if (index < 0) {
  160.             index = -index;
  161.         }
  162.  
  163.         // index cannot exceed size of array minus 1.
  164.         if (index > 16) {
  165.             index = 16;
  166.         }
  167.  
  168.         // get value from the array.
  169.         double dScale = 0.0;
  170.         if (dVal < 0) {
  171.             dScale = -scaleArray[index];
  172.         } else {
  173.             dScale = scaleArray[index];
  174.         }
  175.  
  176.         // return scaled value.
  177.         return dScale;
  178.     }
  179.  
  180. }
RAW Paste Data
Top