SHARE
TWEET

asdfasdfasdf

a guest Jul 17th, 2019 71 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. package org.firstinspires.ftc.teamcode.Skystone.Test;
  2. import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
  3. import com.qualcomm.robotcore.eventloop.opmode.Disabled;
  4. import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
  5.  
  6. import org.firstinspires.ftc.robotcore.external.Telemetry;
  7. import org.firstinspires.ftc.teamcode.Skystone.Robot;
  8.  
  9. @Autonomous(name="kinematics Test", group="Linear Opmode")
  10. public class kinematicsTest extends LinearOpMode
  11. {
  12.     @Override
  13.     public void runOpMode(){
  14.         Robot robot = new Robot(hardwareMap,telemetry,this);
  15.         robot.resetEncoders();
  16.         waitForStart();
  17.         robot.intializeIMU();
  18.         robot.changeRunModeToUsingEncoder();
  19.  
  20.         double fLPower;
  21.         double fRPower;
  22.         double bLPower;
  23.         double bRPower;
  24.  
  25.         double powerScaleFactor = 0.9;
  26.  
  27.         while (opModeIsActive()){
  28.  
  29.  
  30.             //double[] position = robot.getPosUsingIncrements();
  31.  
  32.             fLPower = (-gamepad1.left_stick_y + gamepad1.right_stick_x + gamepad1.left_stick_x)*powerScaleFactor;
  33.             fRPower = (-gamepad1.left_stick_y - gamepad1.right_stick_x - gamepad1.left_stick_x)*powerScaleFactor;
  34.             bLPower = (-gamepad1.left_stick_y + gamepad1.right_stick_x - gamepad1.left_stick_x)*powerScaleFactor;
  35.             bRPower = (-gamepad1.left_stick_y - gamepad1.right_stick_x + gamepad1.left_stick_x)*powerScaleFactor;
  36.  
  37.             if(gamepad1.right_trigger!=0){
  38.                 fLPower = (gamepad1.right_trigger)*powerScaleFactor;
  39.                 fRPower = (-gamepad1.right_trigger)*powerScaleFactor;
  40.                 bLPower = (-gamepad1.right_trigger)*powerScaleFactor;
  41.                 bRPower = (gamepad1.right_trigger)*powerScaleFactor;
  42.             }else if(gamepad1.left_trigger!=0){
  43.                 fLPower = (-gamepad1.left_trigger)*powerScaleFactor;
  44.                 fRPower = (gamepad1.left_trigger)*powerScaleFactor;
  45.                 bLPower = (gamepad1.left_trigger)*powerScaleFactor;
  46.                 bRPower = (-gamepad1.left_trigger)*powerScaleFactor;
  47.             }
  48.             //Straight D-Pad move
  49.             if (gamepad1.dpad_up) {
  50.                 fLPower = (gamepad1.left_stick_y)+powerScaleFactor;
  51.                 bLPower = (gamepad1.left_stick_y)+powerScaleFactor;
  52.                 fRPower = (gamepad1.right_stick_y+powerScaleFactor);
  53.                 bRPower = (gamepad1.right_stick_y+powerScaleFactor);
  54.             } else if (gamepad1.dpad_down) {
  55.                 fLPower = (gamepad1.left_stick_y)-powerScaleFactor;
  56.                 bLPower = (gamepad1.left_stick_y)-powerScaleFactor;
  57.                 fRPower = (gamepad1.right_stick_y-powerScaleFactor);
  58.                 bRPower = (gamepad1.right_stick_y)-powerScaleFactor;
  59.             } else if (gamepad1.dpad_right) {
  60.                 fLPower = (gamepad1.right_stick_y)+powerScaleFactor;
  61.                 bLPower = (gamepad1.right_stick_y)+powerScaleFactor;
  62.                 fRPower = (gamepad1.left_stick_y)-powerScaleFactor;
  63.                 bRPower = (gamepad1.left_stick_y)-powerScaleFactor;
  64.             } else if (gamepad1.dpad_left) {
  65.                 fRPower = (gamepad1.right_stick_y)+powerScaleFactor;
  66.                 bRPower = (gamepad1.right_stick_y)+powerScaleFactor;
  67.                 fLPower = (gamepad1.left_stick_y)-powerScaleFactor;
  68.                 bLPower = (gamepad1.left_stick_y)-powerScaleFactor;
  69.             } else if (gamepad1.a){
  70.                 telemetry.addLine("x " + position[0]);
  71.                 telemetry.addLine("y " + position[1]);
  72.                 telemetry.addLine("angle: " + position[2]);
  73.                 telemetry.update();
  74.             }
  75.  
  76.             robot.fLeft.setPower(fLPower);
  77.             robot.bLeft.setPower(bLPower);
  78.             robot.bRight.setPower(bRPower);
  79.             robot.fRight.setPower(fRPower);
  80.  
  81.         }
  82.     }
  83. }
RAW Paste Data
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
 
Top