daily pastebin goal
83%
SHARE
TWEET

Untitled

a guest Mar 13th, 2018 61 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.LinearOpMode;
  4. import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
  5. import com.qualcomm.robotcore.hardware.CRServo;
  6. import com.qualcomm.robotcore.hardware.DcMotor;
  7. import com.qualcomm.robotcore.hardware.DcMotorSimple;
  8. import com.qualcomm.robotcore.hardware.Servo;
  9. import com.qualcomm.robotcore.util.ElapsedTime;
  10. import com.qualcomm.robotcore.util.Range;
  11. import java.util.concurrent.TimeUnit;
  12.  
  13.  
  14. @TeleOp(name="TeleOP 3.0")
  15. public class TeleOP_3_0  extends LinearOpMode {
  16.  
  17.     private ElapsedTime runtime = new ElapsedTime();
  18.     private DcMotor Roata_Stanga_Fata = null;
  19.     private DcMotor Roata_Stanga_Spate = null;
  20.     private DcMotor Roata_Dreapta_Fata = null;
  21.     private DcMotor Roata_Dreapta_Spate = null;
  22.     private DcMotor Brats = null;
  23.     private DcMotor Bratd = null;
  24.     private DcMotor motorplatforma = null;
  25.     private DcMotor motorrelic = null;
  26.     private Servo servo_stanga=null;
  27.     private Servo servo_dreapta=null;
  28.     private Servo bratsecund=null;
  29.     private CRServo bratmingi=null;
  30.     private CRServo bratroti=null;
  31.     double pozs = 0;
  32.     double pozd = 0;
  33.     //double pozclasics = 0.68;
  34.     //double pozclasicd = 0.3;
  35.  
  36.     @Override
  37.     public void runOpMode() throws InterruptedException {
  38.         telemetry.addData("Status", "Initialized");
  39.         telemetry.update();
  40.  
  41.         Roata_Stanga_Fata = hardwareMap.get(DcMotor.class, "sf");
  42.         Roata_Stanga_Spate = hardwareMap.get(DcMotor.class, "ss");
  43.         Roata_Dreapta_Fata = hardwareMap.get(DcMotor.class, "df");
  44.         Roata_Dreapta_Spate = hardwareMap.get(DcMotor.class, "ds");
  45.         Brats = hardwareMap.get(DcMotor.class, "brats");
  46.         Bratd = hardwareMap.get(DcMotor.class, "bratd");
  47.         motorplatforma = hardwareMap.get(DcMotor.class, "platforma");
  48.         motorrelic=hardwareMap.get(DcMotor.class,"motorrelic");
  49.  
  50.         bratsecund = hardwareMap.get(Servo.class, "bratsecund");
  51.         servo_stanga = hardwareMap.get(Servo.class, "servo_stanga");
  52.         servo_dreapta = hardwareMap.get(Servo.class, "servo_dreapta");
  53.  
  54.         bratmingi = hardwareMap.get(CRServo.class, "bratmingi");
  55.         bratroti=hardwareMap.get(CRServo.class,"bratroti");
  56.  
  57.         Roata_Stanga_Fata.setDirection(DcMotor.Direction.REVERSE);
  58.         Roata_Stanga_Spate.setDirection(DcMotor.Direction.REVERSE);
  59.         Roata_Dreapta_Fata.setDirection(DcMotor.Direction.FORWARD);
  60.         Roata_Dreapta_Spate.setDirection(DcMotor.Direction.FORWARD);
  61.         Bratd.setDirection(DcMotor.Direction.FORWARD);
  62.         Brats.setDirection(DcMotor.Direction.FORWARD);
  63.  
  64.         Bratd.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  65.         Brats.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  66.  
  67.         bratroti.setPower(0);
  68.         bratmingi.setPower(0.3);
  69.         bratsecund.setPosition(1.8);
  70.  
  71.         waitForStart();
  72.         runtime.reset();
  73.  
  74.         while (opModeIsActive()) {
  75.  
  76.             double RoataStangaFata;
  77.             double RoataDreaptaFata;
  78.             double RoataStangaSpate;
  79.             double RoataDreaptaSpate;
  80.             double y1 = -gamepad1.left_stick_y;
  81.             double x1  =  gamepad1.left_stick_x;
  82.             double x2  =  gamepad1.right_stick_x;
  83.             boolean r=false;
  84.  
  85.             //Miscare
  86.             RoataStangaFata    = Range.clip(x1 + y1 + x2, -0.5, 0.5) ;
  87.             RoataDreaptaFata   = Range.clip(-x1 + y1  - x2, -0.5, 0.5) ;
  88.             RoataStangaSpate   = Range.clip(-x1 + y1 + x2, -0.5, 0.5) ;
  89.             RoataDreaptaSpate   = Range.clip(x1 + y1 - x2, -0.5, 0.5) ;
  90.             /*
  91.             if(r==false) {
  92.                 if(gamepad1.dpad_right) {
  93.                     r = true;
  94.                 }
  95.                 RoataStangaFata    = Range.clip(x1 + y1 + x2, -0.5, 0.5) ;
  96.                 RoataDreaptaFata   = Range.clip(-x1 + y1  - x2, -0.5, 0.5) ;
  97.                 RoataStangaSpate   = Range.clip(-x1 + y1 + x2, -0.5, 0.5) ;
  98.                 RoataDreaptaSpate   = Range.clip(x1 + y1 - x2, -0.5, 0.5) ;
  99.             }
  100.             else
  101.             {
  102.                 if(gamepad1.dpad_right) {
  103.                     r = false;
  104.                 }
  105.                 RoataStangaFata    = Range.clip(x1 + y1 + x2, -1, 1) ;
  106.                 RoataDreaptaFata   = Range.clip(-x1 + y1  - x2, -1, 1) ;
  107.                 RoataStangaSpate   = Range.clip(-x1 + y1 + x2, -1, 1) ;
  108.                 RoataDreaptaSpate   = Range.clip(x1 + y1 - x2, -1, 1) ;
  109.             }
  110.             */
  111.             Roata_Stanga_Fata.setPower(RoataStangaFata);
  112.             Roata_Stanga_Spate.setPower(RoataStangaSpate);
  113.             Roata_Dreapta_Fata.setPower(RoataDreaptaFata);
  114.             Roata_Dreapta_Spate.setPower(RoataDreaptaSpate);
  115.  
  116.             //Platforma cuburi
  117.             if (gamepad2.x) {
  118.                 motorplatforma.setPower(1);
  119.                 pozs=0.56;
  120.                 pozd=0.42;
  121.                 servo_dreapta.setPosition(pozd);
  122.                 servo_stanga.setPosition(pozs);
  123.             }
  124.             else if(gamepad2.y) {
  125.                 motorplatforma.setPower(-1);
  126.             }
  127.             else {
  128.                 motorplatforma.setPower(0);
  129.             }
  130.             //Servouri
  131.             if (gamepad2.a) {
  132.                 pozd=1.8;
  133.                 pozs=0;
  134.                 servo_dreapta.setPosition(pozd);
  135.                 servo_stanga.setPosition(pozs);
  136.             }
  137.             if (gamepad2.b) {
  138.                 pozd=0.32;
  139.                 pozs=0.66;
  140.                 servo_dreapta.setPosition(pozd);
  141.                 servo_stanga.setPosition(pozs);
  142.             }
  143.  
  144.             //Roti cuburi
  145.             if(gamepad1.a){
  146.                 Bratd.setPower(-gamepad1.right_trigger);
  147.                 Brats.setPower(-gamepad1.left_trigger);
  148.             }
  149.             else{
  150.                 Bratd.setPower(0);
  151.                 Brats.setPower(0);
  152.             }
  153.             if (gamepad1.x){
  154.                 Bratd.setPower(1);
  155.                 Brats.setPower(1);
  156.             }
  157.             else{
  158.                 Bratd.setPower(0);
  159.                 Brats.setPower(0);
  160.             }
  161.  
  162.  
  163.             telemetry.addData("Speed", x1+x2+y1);
  164.             telemetry.addData("Status", "Run Time: " + runtime.toString());
  165.             telemetry.addData("Motors", "Stanga Fata (%.2f), Dreapta Fata (%.2f), Stanga Spate (%.2f), Dreapta Spate (%.2f)",
  166.                     RoataStangaFata, RoataStangaFata, RoataStangaSpate, RoataDreaptaSpate);
  167.             telemetry.update();
  168.  
  169.         }
  170.     }
  171.  
  172.  
  173. }
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