Packer Robotics code help.

 this is the code page of ftc 14393 packer robotics 

Or launching system

To set up a ring launcher we have done some thing including the code to make this happen.


High Goal Subroutine - This subroutine is a combination of other subroutines that positions the bot directly in front of the high goal where it would shoot all three preloaded rings. 
Ring Stacks - The number of rings set for autonomous is randomized so we created an if-else statement that basically says if there are no rings shoot normally and place both wobbles on the field. One ring: shoot, pick up the one ring then shoot, and drop both wobbles. The four rings: shoot the loaded rings, pick up three out of the four, however, for the third one we were only able to drop one wobble due to the limitation of time.  
wobbledrop(): a wobble placed on a servo on the bot before auto changes position from 0 to .5
The Clawwwww: fingers.setPosition() changes the servo’s position that opens up to pick up a wobble; wobble.setTargetPostion() causes the motor to move the arm into a position that picks the wobble
Camera: 0 ring is less than 0 while any rings detected is greater than 0 (1 ring < 100 while 4 rings > 100) 
Turns- the four mecanum wheels must coordinate with one another to either strafe or move forward based on encoders that runs to a set position while the PID Control corrects the angles of the bot when shooting 



private void highgoal(){   
 Straight(.8, 3100);   
 flywheel.setPower(.67);    
Strafe (.8, -1200);    
//telemetry.addData("flywheel power", ((DcMotorEx) flywheel).getVelocity());    
//telemetry.update();     
//flywheel.setPower(.65);      
//((DcMotorEx) flywheel).setVelocity(1730);      
sleep (600);      
telemetry.addData("flywheel power", (flywheel.getPower()));        
telemetry.update();      
 shoot();    
sleep(700);    
//telemetry.addData("flywheel power", ((DcMotorEx) flywheel).getVelocity());    
//telemetry.update();    shoot();    
sleep(700);    
//telemetry.addData("flywheel power", ((DcMotorEx) flywheel).getVelocity());    
//telemetry.update();    
shoot();


Our Gathering system

This is our system for holding our rings , it has changed many times over the months with  the one shown above being one of the first. Their are multiple parts on the robot that make this work properly and smoothly ranging from a conveyor to a little thing that nudges the rings out to our spinning weal.

Our Teleop code. 

Here we put our teleop code for yall to dig apart. You are free to check our youtube channel and compare how the code functions to the videos listed their. 




   while (opModeIsActive()){

    if (slowmode ==1){

    backleft.setPower(-(gamepad1.left_stick_y + gamepad1.left_stick_x + gamepad1.right_stick_x));

    frontleft.setPower(-(gamepad1.left_stick_y - gamepad1.left_stick_x + gamepad1.right_stick_x));

    backright.setPower((gamepad1.left_stick_y - gamepad1.left_stick_x - gamepad1.right_stick_x));

    frontright.setPower((gamepad1.left_stick_y + gamepad1.left_stick_x - gamepad1.right_stick_x));

    }

    if (slowmode == 2){

    backleft.setPower(0.3*-(gamepad1.left_stick_y + gamepad1.left_stick_x + gamepad1.right_stick_x));

    frontleft.setPower(0.3*-(gamepad1.left_stick_y - gamepad1.left_stick_x + gamepad1.right_stick_x));

    backright.setPower(0.3*(gamepad1.left_stick_y - gamepad1.left_stick_x - gamepad1.right_stick_x));

    frontright.setPower(0.3*(gamepad1.left_stick_y + gamepad1.left_stick_x - gamepad1.right_stick_x)); 

    }

    if (gamepad1.right_bumper && slowmode ==1){

     slowmode = 2;

     sleep(250);

    }

     if (gamepad1.right_bumper && slowmode ==2){

     slowmode = 1;

     sleep(250);

    }

    if (gamepad1.left_bumper){

      ringpush.setPosition(1);

      sleep (500);

      ringpush.setPosition(0);

      

    }

    if (gamepad1.a && roller == 0){

     frontroller.setPower(-1);

     backroller.setPower(-1);

     roller = 1;

     sleep (150);

    }

    if (gamepad1.a && roller == 1){

     frontroller.setPower(0);

     backroller.setPower(0);

     roller = 0;

     sleep (150);

    } 
    if (gamepad2.x && fingpos == 0){

     fingers.setPosition(.6);

     sleep(250);

     fingpos = 1;

    }

    if (gamepad2.x && fingpos == 1){

     fingers.setPosition(1);

     sleep(250);

     fingpos = 0;

    }

    if (roller == 0){

     frontroller.setPower(gamepad1.right_trigger);

    backroller.setPower(gamepad1.right_trigger);

    }

    

    if (flyon == 0){

    flywheel.setPower(gamepad2.left_trigger);

    }

    
    if ((flyon ==0) && (gamepad2.right_bumper)){

     flyon = 1;

     sleep (250);

    }

    if ((flyon ==1) && (gamepad2.right_bumper)){

     flyon = 0;

     sleep (250);

    }

    
    if ((flyon ==1) && (gamepad2.y)){

     flywheel.setPower(.7);

     
    }

    if ((flyon == 1) && (gamepad2.a)){

     flywheel.setPower(.57);

    }

    if ((flyon == 1) && (gamepad2.left_bumper)){

     flywheel.setPower(0.64);

     
    }

    //wobble.setPower(0.3*(gamepad2.left_stick_y));

    if ((wobpos == 1) && (gamepad2.dpad_down)){

      wobble.setPower(.7);

     wobble.setTargetPosition(540);

     wobble.setMode(DcMotor.RunMode.RUN_TO_POSITION);

      while (!(!frontright.isBusy() || isStopRequested())) {

      }

      wobpos = 2;

    }

    if ((wobpos == 2) && (gamepad2.dpad_down)){

     wobble.setPower(.7);

     wobble.setTargetPosition(1230);

     wobble.setMode(DcMotor.RunMode.RUN_TO_POSITION);

      while (!(!frontright.isBusy() || isStopRequested())) {

      }

      wobpos = 3;

    }

    if ((wobpos == 3) && (gamepad2.dpad_up)){

     wobble.setPower(.7);

     wobble.setTargetPosition(540);

     wobble.setMode(DcMotor.RunMode.RUN_TO_POSITION);

while (!(!frontright.isBusy() || isStopRequested())) {

      }

      wobpos = 2;

    }



    if ((gamepad2.dpad_left)){

     sleep (200);

     wobble.setPower(.7);

     wobble.setTargetPosition(150 + wobble.getCurrentPosition());

     wobble.setMode(DcMotor.RunMode.RUN_TO_POSITION);

      while (!(!frontright.isBusy() || isStopRequested())) {

      }

     

    }

    if ((gamepad2.dpad_right)){

     sleep (200);

     wobble.setPower(.7);

     wobble.setTargetPosition(wobble.getCurrentPosition()-150);

     wobble.setMode(DcMotor.RunMode.RUN_TO_POSITION);

      while (!(!frontright.isBusy() || isStopRequested())) {

      }

     

    }

    if (gamepad2.b) {

     flylever.setPosition(0);

     sleep(250);

     flylever.setPosition(0.5);

   } 
   

    telemetry.addData("Flywheel Mode", flyon);

    telemetry.addData("Slowmode", slowmode);

    telemetry.addData("Position", wobble.getCurrentPosition());

    telemetry.update();

   }

  }

 }




Our Autonomous code.

Here we put our teleop code for yall to dig apart. You are free to check our youtube channel and compare how the code functions to the videos listed their. 


    highgoal();

     

     

     

       if (stackx < 0) {

    // Add code to move bot to Zone 1

    Straight (.5, 600);

    turn (-90);

    Straight (.5, -500);

    wobbledrop();

    Straight (.5, 500);

    turn (0);

    fingers.setPosition(.6);

    wobble.setPower(.7);

    wobble.setTargetPosition(1230);

    wobble.setMode(DcMotor.RunMode.RUN_TO_POSITION);

      while (!(!frontright.isBusy() || isStopRequested())) {

      }

    Strafe (.6, -850);  

    Straight (.8, -1900);

    fingers.setPosition(1);

    sleep (1000);

    Straight (.8, 1900);

    Strafe (.6, 500);

    turn (-90);

    fingers.setPosition(.6);

    Straight (.8, 600);

fingers.setPosition(1);

   } else if (stackx > 100) {

    

     frontroller.setPower(-1);

    backroller.setPower(-1);

    Straight (.2, -1600);

    Straight (.5, 1400);

    ringpush.setPosition(1);

    sleep (500);

    ringpush.setPosition(0);

    frontroller.setPower(1);

    backroller.setPower(1);

    sleep (1500);

    frontroller.setPower(0);

    backroller.setPower(0);

    //turnx (0);

    //Strafe (.5, -500);

    flywheel.setPower(.7);

    sleep (750);

    shoot();

    sleep(500);

    shoot();

    sleep(500);

    shoot();

    flywheel.setPower(0);

    //turn (0);

    // Add code to move bot to Zone 3

    Straight (.8, 3700);

    turn (-90);

    Straight (.8, -500);

    wobbledrop();

    Straight (1, 600);

Strafe(1, 3400);

   } else {

    

    frontroller.setPower(-.7);

    backroller.setPower(-.7);

    Straight (.5, -1000);

    Straight (.5, 900);

    ringpush.setPosition(1);

    sleep (500);

    ringpush.setPosition(0);

    frontroller.setPower(0);

    //Strafe (.5, -500);

    //turnx (-11);

    backroller.setPower(0);

    //turn (0);

    flywheel.setPower(.7);

    sleep (2000);

    shoot();

    flywheel.setPower(0);

    //turnx (0);

    // Add code to move bot to Zone 2

    Straight (.5, 2500);

    turn (-90);

    Straight (.5, 600);

    wobbledrop();

    Straight(.5, 500);

    turn (-20);

    fingers.setPosition(.6);

    wobble.setPower(.7);

    wobble.setTargetPosition(1230);

    wobble.setMode(DcMotor.RunMode.RUN_TO_POSITION);

while (!(!frontright.isBusy() || isStopRequested())) {

      }

    

   }

   Straight (1, -3800);

   fingers.setPosition(1);

   sleep (500);

   turn(179);

   Straight (1,-2500);

   fingers.setPosition(.6);

   Straight (1, 500);

   

   

      }

       while (opModeIsActive()) {

    }



    if (tfod != null) {

      tfod.shutdown();

    }

  }



 private void shoot(){

 

  flylever.setPosition(0);

  sleep(500);

  flylever.setPosition(0.5);

  }

  
  private void wobbledrop(){

   wobdrop.setPosition (0.5);

   sleep (2000);

  }

 private void highgoal(){

  Straight(.5, 3400);

  Strafe (.5, -1000);

  //telemetry.addData("flywheel power", ((DcMotorEx) flywheel).getVelocity());

  //telemetry.update();

   flywheel.setPower(.7);

   //((DcMotorEx) flywheel).setVelocity(1730);

   sleep (700);

   telemetry.addData("flywheel power", (flywheel.getPower()));

    telemetry.update();

   shoot();

  sleep(800);

  //telemetry.addData("flywheel power", ((DcMotorEx) flywheel).getVelocity());

  //telemetry.update();

  shoot();

  sleep(800);

  //telemetry.addData("flywheel power", ((DcMotorEx) flywheel).getVelocity());

  //telemetry.update();

  shoot();

  sleep(500);

  flywheel.setPower(0);

  //Straight(.8, 3400);

  //Strafe (.5, 1000);


Subroutines: 
highgoal - allows the bot to shoot in autonomous      

straight - moves the bot forward or backward if position negative ( set power, set position) 
strafe - allows the bot to strafe (same as straight) 
shoot - uses a servo to push the rings when flywheel is on