示例#1
0
  /** This function is called periodically during operator control */
  public void teleopPeriodic() {

    robotDrive.mecanumDrive_Polar();
    lifter.upLift();
    lifter.downLift();
    lifter.QuickRaise();
    lifter.toteLowering(); // lowers 6 totes and a can at .1 speed
    // lifter.manualUplift();
    // lifter.manualDownlift();
    servo.runBolt();
    pusher.PushandRetract(); // push one button to push and retract
    pusher.Push(); // full push with one button press
    pusher.Retract(); // full retract with one button press
    // pusher.ManualPush();
    // pusher.ManualRetract();

    SmartDashboard.putBoolean("switch 1", DigitalInput1.get());
    SmartDashboard.putBoolean("switch 2", DigitalInput2.get());
    SmartDashboard.putBoolean("switch 3", DigitalInput3.get());
    SmartDashboard.putBoolean("switch 4", DigitalInput4.get());
    SmartDashboard.putBoolean("switch 5", DigitalInput5.get());
    SmartDashboard.putBoolean("switch 6", DigitalInput6.get());
    SmartDashboard.putNumber("Lifting Talon Speed", LifterTalon.get());
    SmartDashboard.putNumber("Pushing Talon Speed", PushingTalon.get());
    SmartDashboard.putNumber("servo position zero means brake is on", servo.get());
  }
示例#2
0
 public void changeSpeed(double Y, double Z) {
   leftSpeed = (((Y * yBoost) - (Z * zBoost)) * limiter);
   rightSpeed = (((Y * yBoost * -1) - (Z * zBoost)) * limiter);
   talon1.set(leftSpeed);
   talon2.set(leftSpeed);
   talon3.set(rightSpeed);
   talon4.set(rightSpeed);
 }
 public void RotateFrame(double s, double distance, double dir) {
   reset();
   left.set(dir * s);
   right.set(dir * s);
   do {
     SmartDashboard.putNumber("left distance", lEncoder.getDistance());
     SmartDashboard.putNumber("right distance", rEncoder.getDistance());
   } while (rEncoder.getDistance() <= distance);
   left.set(0);
   right.set(0);
 }
 /** This function is called periodically during operator control */
 public void teleopPeriodic() {
   double x = joy1.getX();
   double y = -joy1.getY();
   double r = joy1.getZ();
   if (Math.abs(x) < .05) x = 0;
   if (Math.abs(y) < .05) y = 0;
   if (Math.abs(r) < .05) r = 0;
   drive(x / 2, y / 2, r / 2);
   shooter1.set(joy2.getY());
   shooter2.set(joy2.getY());
 }
 public void DriveForward(double s, double distance) {
   reset();
   left.set(s);
   right.set(-1 * s);
   do {
     SmartDashboard.putNumber("left distance", lEncoder.getDistance());
     SmartDashboard.putNumber("right distance", rEncoder.getDistance());
   } while (rEncoder.getDistance() <= distance);
   left.set(0);
   right.set(0);
   reset();
 }
示例#6
0
  /** This function is called periodically during test mode */
  public void testPeriodic() {

    driveTrainTester.run();
    SmartDashboard.putBoolean("switch 1", DigitalInput1.get());
    SmartDashboard.putBoolean("switch 2", DigitalInput2.get());
    SmartDashboard.putBoolean("switch 3", DigitalInput3.get());
    SmartDashboard.putBoolean("switch 4", DigitalInput4.get());
    SmartDashboard.putBoolean("switch 5", DigitalInput5.get());
    SmartDashboard.putBoolean("switch 6", DigitalInput6.get());
    SmartDashboard.putNumber("Lifting Talon Speed", LifterTalon.get());
    SmartDashboard.putNumber("Pushing Talon Speed", PushingTalon.get());
  }
  public void arcadeDrive() {
    lT = OI.getDriveStick().getLeftX() - OI.getDriveStick().getLeftY();
    rT = OI.getDriveStick().getLeftX() + OI.getDriveStick().getLeftY();

    // maxV = Math.max(Math.abs(lT), Math.abs(rT));

    if (maxV > 1) {
      lT = lT / maxV;
      rT = rT / maxV;
    }

    left.set(lT);
    right.set(rT);
    SmartDashboard.putNumber("left distance while driving", lEncoder.getDistance());
    SmartDashboard.putNumber("right distance while driving", rEncoder.getDistance());
  }
 public void stopMotor() {
   if (RobotMap.IS_REAL_BOT == true) {
     kickerTalon.set(0.0);
   } else if (RobotMap.IS_REAL_BOT == false) {
     kickerJaguar.set(0.0);
   }
 }
示例#9
0
  public void teleopPeriodic() {

    double throttle = (driveStick.getThrottle() + 1) / -2;
    double xAxis = throttle * (Math.abs(driveStick.getX()) > 0.15 ? driveStick.getX() : 0.0);
    double yAxis = throttle * (Math.abs(driveStick.getY()) > 0.15 ? driveStick.getY() : 0.0);
    double rotation =
        throttle * (Math.abs(driveStick.getTwist()) > 0.15 ? driveStick.getTwist() : 0.0);

    // Mecanum drive
    drive.mecanumDrive_Cartesian(xAxis, yAxis, rotation, 0);

    // Elevator control
    elevator.set(driveStick.getRawButton(5) ? -1.0 : driveStick.getRawButton(3) ? 1.0 : 0.0);

    // Intake control
    intakeTensionMotor.set(
        driveStick.getRawButton(2) ? -1.0 : driveStick.getRawButton(7) ? 1.0 : 0.0);
    if (Math.abs(controlStick.getTwist()) > 0) {
      double toteTurnSpeed = -controlStick.getTwist();
      intakeMotors[0].set(
          toteTurnSpeed > 0 ? Math.pow(toteTurnSpeed, 2) : -Math.pow(toteTurnSpeed, 2));
      intakeMotors[1].set(
          toteTurnSpeed > 0 ? Math.pow(toteTurnSpeed, 2) : -Math.pow(toteTurnSpeed, 2));
    } else {
      double intakeSpeed =
          driveStick.getRawButton(1) ? -1.0 : driveStick.getRawButton(8) ? 0.4 : 0.0;
      intakeMotors[0].set(-intakeSpeed);
      intakeMotors[1].set(intakeSpeed);
    }
  }
示例#10
0
 public void drive(
     double xAxis,
     double yAxis,
     double mid) { // xaxis rotates, yaxis forward and back, mid goes side to side
   hDrive.arcadeDrive(yAxis, xAxis);
   middleMotor1.set(-mid);
 }
示例#11
0
 private void scooperControl(double speed) {
   if (isAtUpperLimit() && speed > 0) {
     speed = 0;
   } else if (isAtLowerLimit() && speed < 0) {
     speed = 0;
   }
   scooperMotor.set(speed);
 }
  /*
   *
   * @param direction -1 to draw the arm back, +1 to rotate motor forward away from arm.
   *
   */
  public void reload(int direction, double backSpeed, double frontSpeed) {
    final int REVERSE = -1;

    if (direction == DRAW_KICKER_BACK) {
      if (RobotMap.IS_REAL_BOT == true) {
        kickerTalon.set(
            REVERSE
                * (backSpeed)); // TODO: Check to make sure the motors will draw back when recieving
        // a positive voltage.
      } else if (RobotMap.IS_REAL_BOT == false) {
        kickerJaguar.set(REVERSE * backSpeed);
      }
    } else if (direction == ROTATE_KICKER_FORWARD) {
      if (RobotMap.IS_REAL_BOT == true) {
        kickerTalon.set(frontSpeed);
      } else if (RobotMap.IS_REAL_BOT == false) {
        kickerJaguar.set(frontSpeed);
      }
    }
  }
示例#13
0
  protected void execute() {

    if (time.get() < 3.0) {
      talonLeft.set(1);
      talonRight.set(1);
    } else if (time.get() < 5.0) {
      talonLeft.set(0);
      talonRight.set(0);
      soleLiftDown.set(true);
    } else if (time.get() < 6.0) {
      talonLeft.set(-1);
      talonRight.set(-1);
    } else talonLeft.set(1);
    talonRight.set(1);
  }
  public ShooterSubsystem() {
    kickerLatch = new DoubleSolenoid(RobotMap.KICKER_LATCH_RELEASE, RobotMap.KICKER_LATCH_RELOAD);
    kickerLatch.set(DoubleSolenoid.Value.kForward);

    if (RobotMap.IS_REAL_BOT == true) {
      kickerTalon = new Talon(RobotMap.KICKER_MOTOR);
      kickerTalon.set(0.0);
    } else if (RobotMap.IS_REAL_BOT == false) {
      kickerJaguar = new Jaguar(RobotMap.KICKER_MOTOR);
      kickerJaguar.set(0.0);
    }

    kickerLimitSwitch = new DigitalInput(RobotMap.KICKER_RETRACTED_SWITCH);
    kickerMotorReturnSwitch = new DigitalInput(RobotMap.KICKER_MOTOR_RETURN_SWITCH);
  }
  /** This function is called once each time the robot enters operator control. */
  public void operatorControl() {
    drivetrain.setSafetyEnabled(true);
    compressor.start();
    while (isOperatorControl() && isEnabled()) {
      // The throttle goes from 1 to -1, bottom to top. This
      // scales it to go 0 to 1, bottom to top.
      double throttle = (-leftStick.getRawAxis(3) + 1) / 2;

      double drive = deadband(leftStick.getY()) * throttle;
      double steer = deadband(-rightStick.getX()) * throttle;
      drivetrain.arcadeDrive(drive, steer);

      boolean shouldSpinWheels = leftStick.getRawButton(1);
      boolean shouldFire = shouldSpinWheels && rightStick.getRawButton(1);
      shooterWheels.set(shouldSpinWheels ? -1 : 0);
      shooter.set(shouldFire);

      Timer.delay(0.005); // wait for a motor update time
    }
  }
 public void shoot(float speed) {
   kickerTalon.set(speed);
 }
示例#17
0
 public void disable() {
   leftMotor.set(0.0);
   rightMotor.set(0.0);
   rightEncoder.reset();
   leftEncoder.reset();
 } // end disable
  public void gathererSpark(Joystick input, PowerDistributionPanel pdp) {
    switch (gatherState) {
      case 0:
        if (input.getRawButton(7)) {
          gatherLift.set(0.6);
          System.out.println("Gatherer up");
          gatherState = 1;
        } else if (input.getRawButton(8)) {
          gatherLift.set(-0.6);
          System.out.println("Gatherer down");
          gatherState = 4;
        } else if (input.getRawButton(5)) {

        }
        break;
      case 1:
        if (pdp.getCurrent(13) > 1.25) {
          System.out.println("Motor Running");
          gatherState = 2;
        } else if (gatherCount > 3) {
          gatherState = 3;
          System.out.println("Gather Timeout");
        }
        gatherCount++;
        break;
      case 2:
        if (pdp.getCurrent(13) <= 1.25) {
          gatherLift.set(0.0);
          System.out.println("Top limit");
          gatherState = 3;
        }
        break;
      case 3:
        if (!input.getRawButton(7)) {
          gatherCount = 0;
          gatherState = 0;
        }
        break;
      case 4:
        if (pdp.getCurrent(13) > 1.25) {
          System.out.println("Motor Running");
          gatherState = 5;
        } else if (gatherCount > 3) {
          gatherState = 6;
          System.out.println("Gather Timeout");
        }
        gatherCount++;
        break;
      case 5:
        if (pdp.getCurrent(13) <= 1.25) {
          gatherLift.set(0.0);
          System.out.println("Bottom limit");
          gatherState = 6;
        }
        break;
      case 6:
        if (!input.getRawButton(8)) {
          gatherCount = 0;
          gatherState = 0;
        }
        break;
    }
  }
 /**
  * Reads input from the joystick in order to control the lifting part of the gatherer, which
  * operates on a three-tier elevator-like system. Three different buttons are read from the
  * joystick in order to move the {@link Talon} to one of the three tiers. Update periodically.
  *
  * @param input The joystick that will be used to control the gatherer.
  */
 public void gathererTalon(Joystick input) {
   if (gatherLift == null) {
     System.err.println(
         "The gatherer motor controllers have not been initialized. Call this class's method \"initGatherers()\" in order to do so.");
     return;
   }
   switch (gatherState) {
     case 0: // checking for input
       if (input.getRawButton(Wiring.BTN_GATHERER_TO_TOP) && diGathererTop.get()) {
         gatherLift.set(0.6);
         System.out.println("Gatherer up");
         gatherCount = 0;
         gatherState = 1;
       } else if (input.getRawButton(Wiring.BTN_GATHERER_TO_BOT) && diGathererBot.get()) {
         gatherLift.set(-0.6);
         System.out.println("Gatherer down");
         gatherCount = 0;
         gatherState = 3;
       } else if (input.getRawButton(Wiring.BTN_GATHERER_TO_MID)) {
         if (!diGathererTop.get()) {
           gatherLift.set(-0.6);
           System.out.println("Gatherer down");
           gatherCount = 0;
           gatherState = 5;
         } else if (!diGathererBot.get()) {
           gatherLift.set(0.6);
           System.out.println("Gatherer up");
           gatherCount = 0;
           gatherState = 6;
         }
       }
       break;
     case 1: // should be going up, wait until it hits the top
       if (!diGathererTop.get()) {
         gatherLift.set(0.0);
         System.out.println("Top limit");
         gatherState = 2;
       }
       break;
     case 2: // at top, waiting for driver to stop holding the button
       if (!input.getRawButton(Wiring.BTN_GATHERER_TO_TOP)) {
         gatherState = 0;
       }
       break;
     case 3: // should be going down, wait until it hist the bottom
       if (!diGathererBot.get()) {
         gatherLift.set(0.0);
         System.out.println("Bottom limit");
         gatherState = 4;
       }
       break;
     case 4: // at bottom, waiting for driver to stop holding the button
       if (!input.getRawButton(Wiring.BTN_GATHERER_TO_BOT)) {
         gatherState = 0;
       }
       break;
     case 5: // go down to mid
       gatherLift.set(-0.6);
       if (counter.get() >= 1) {
         gatherLift.set(0.0); // stahp
         counter.reset();
         gatherState = 0;
       }
       break;
     case 6: // go up to mid
       gatherLift.set(0.6);
       if (counter.get() >= 1) {
         gatherLift.set(0.0); // stahp
         counter.reset();
         gatherState = 0;
       }
       break;
   }
 }
示例#20
0
 public void doNothing() {
   mainMotor.set(0.0);
 }
示例#21
0
 public void setCollectorMotor(double speed) {
   collectorTalon.set(speed);
 }
示例#22
0
 public void go() {
   mainMotor.set(.3);
 }
示例#23
0
 public void goVariable(double speed) {
   mainMotor.set(speed);
   SmartDashboard.putNumber("motorspeed", mainMotor.get());
 }
示例#24
0
 public void setElevatorMotor(double speed) {
   moverUpperTalon.set(speed);
 }
示例#25
0
 public void In() {
   Intake.set(in);
 }
示例#26
0
 public void Out() {
   Intake.set(out);
 }
示例#27
0
 public void Stop() {
   Intake.set(0);
 }
示例#28
0
 public void drive(double x, double y, double r) {
   front.set(x + r / 2);
   left.set(-x / 2 + y + r);
   right.set(-x / 2 - y + r);
 }
示例#29
0
 public void setLassoSpeed(double speed) {
   lasso.set(speed);
 }
示例#30
0
  /** This function is called periodically during autonomous */
  public void autonomousPeriodic() {

    // Start Timer
    MainTimer = timerMain.get();

    // Robot Aligns Left
    if (DigitalInput6.get() == false // Ensure pusher is fully retracted
        && DigitalInput7.get() == false
        && DigitalInput8.get() == true
        && b_leftAutonomous == true
        && b_rightAutonomous == false
        && b_centerAutonomous == false
        && b_upAutonomous == false
        && b_downAutonomous == true
        && b_leftDriveAutonomous == false) {

      LifterTalon.set(autoInitialDownSpeed);
      b_leftAutonomous = true;
      b_rightAutonomous = false;
      b_centerAutonomous = false;
      b_upAutonomous = true;
      b_downAutonomous = false;
      b_leftDriveAutonomous = false;
    }

    // Lifter Descends To LimitSwitch 1, lifter immediately ascends, reset booleans
    if (DigitalInput1.get() == false
        && b_leftAutonomous == true
        && b_rightAutonomous == false
        && b_centerAutonomous == false
        && b_upAutonomous == true
        && b_downAutonomous == false
        && b_leftDriveAutonomous == false) {

      LifterTalon.set(autoInitialUpSpeed);
      b_leftAutonomous = true;
      b_rightAutonomous = false;
      b_centerAutonomous = false;
      b_upAutonomous = false;
      b_downAutonomous = false;
      b_leftDriveAutonomous = false;
    }

    // Lifter ascends to LimitSwitch 4, stops motor, reset booleans
    if (DigitalInput4.get() == false
        && b_leftAutonomous == true
        && b_rightAutonomous == false
        && b_centerAutonomous == false
        && b_upAutonomous == false
        && b_downAutonomous == false
        && b_leftDriveAutonomous == false) {

      LifterTalon.set(autoSpeedStop);

      b_leftAutonomous = false;
      b_rightAutonomous = false;
      b_centerAutonomous = false;
      b_upAutonomous = false;
      b_downAutonomous = false;
      b_leftDriveAutonomous = true;
      // Delay one second
      Timer.delay(1);
      timer1Left.reset();
      timer1Left.start();
    }

    // Begin driving no limit switch since we might drive while lifting the can TBD.
    if (b_leftDriveAutonomous == true
        && b_leftAutonomous == false
        && b_rightAutonomous == false
        && b_centerAutonomous == false
        && b_upAutonomous == false
        && b_downAutonomous == false) {

      // Orchestration A Left
      if (controlChuteLeft[0] == true) {
        while ((autoTimerStart1Left = timer1Left.get()) < orch_A_Left_Time) {
          robotDrive.mecanumDrive_Polar(
              orch_A_Left_Mag,
              orch_A_Left_Dir,
              orch_A_Left_Rot); // Magnitude (speed), Direction (degrees), Rotation (Turning)
        }

        controlChuteLeft[0] = false;
        controlChuteLeft[1] = true;

        timer2Left.reset();
        timer2Left.start();
      }

      //  Orchestration B  Left
      if (controlChuteLeft[1] == true) {
        while ((autoTimerStart2Left = timer2Left.get()) < orch_B_Left_Time) {
          robotDrive.mecanumDrive_Polar(
              orch_B_Left_Mag,
              orch_B_Left_Dir,
              orch_B_Left_Rot); // Magnitude (speed), Direction (degrees), Rotation (Turning)
        }

        controlChuteLeft[0] = false;
        controlChuteLeft[1] = false;
      }

      // Stop mecanumDrive
      if (controlChuteLeft[0] == false && controlChuteLeft[1] == false) {

        robotDrive.mecanumDrive_Polar(
            stop_mecanum_Mag,
            stop_mecanum_Dir,
            stop_mecanum_Rot); // Magnitude (speed), Direction (degrees), Rotation (Turning)

        b_leftDriveAutonomous = false;
        b_leftAutonomous = false;
        b_rightAutonomous = false;
        b_centerAutonomous = false;
        b_upAutonomous = false;
        b_downAutonomous = false;
      }
    }

    // Robot Aligns Right
    if (DigitalInput6.get() == false // pusher is fully retracted
        && DigitalInput7.get() == true
        && DigitalInput8.get() == false
        && b_leftAutonomous == false
        && b_rightAutonomous == true
        && b_centerAutonomous == false
        && b_upAutonomous == false
        && b_downAutonomous == true
        && b_rightDriveAutonomous == false) {

      LifterTalon.set(autoInitialDownSpeed);

      b_leftAutonomous = false;
      b_rightAutonomous = true;
      b_centerAutonomous = false;
      b_upAutonomous = true;
      b_downAutonomous = false;
      b_rightDriveAutonomous = false;
    }

    // Lifter Descends To LimitSwitch 1, lifter immediately ascends, reset booleans
    if (DigitalInput1.get() == false
        && b_leftAutonomous == false
        && b_rightAutonomous == true
        && b_centerAutonomous == false
        && b_upAutonomous == true
        && b_downAutonomous == false
        && b_rightDriveAutonomous == false) {

      LifterTalon.set(autoInitialUpSpeed);
      b_leftAutonomous = false;
      b_rightAutonomous = true;
      b_centerAutonomous = false;
      b_upAutonomous = false;
      b_downAutonomous = false;
      b_rightDriveAutonomous = false;
    }

    // Lifter ascends to LimitSwitch 4, stops motor, reset booleans
    if (DigitalInput4.get() == false
        && b_leftAutonomous == false
        && b_rightAutonomous == true
        && b_centerAutonomous == false
        && b_upAutonomous == false
        && b_downAutonomous == false
        && b_rightDriveAutonomous == false) {

      LifterTalon.set(autoSpeedStop);
      b_leftAutonomous = false;
      b_rightAutonomous = false;
      b_centerAutonomous = false;
      b_upAutonomous = false;
      b_downAutonomous = false;
      b_rightDriveAutonomous = true;

      // Delay one second
      Timer.delay(1);
      timer1Right.reset();
      timer1Right.start();
    }

    // Begin driving no limit switch since we might drive while lifting the can TBD.
    if (b_rightDriveAutonomous == true
        && b_leftAutonomous == false
        && b_rightAutonomous == false
        && b_centerAutonomous == false
        && b_upAutonomous == false
        && b_downAutonomous == false) {

      // Orchestration A Right
      if (controlChuteRight[0] == true) {
        while ((autoTimerStart1Right = timer1Right.get()) < orch_A_Right_Time) {
          robotDrive.mecanumDrive_Polar(
              orch_A_Right_Mag,
              orch_A_Right_Dir,
              orch_A_Right_Rot); // Magnitude (speed), Direction (degrees), Rotation (Turning)
        }

        controlChuteRight[0] = false;
        controlChuteRight[1] = true;

        timer2Right.reset();
        timer2Right.start();
      }

      // Orchestration B  Right
      if (controlChuteRight[1] == true) {
        while ((autoTimerStart2Right = timer2Right.get()) < orch_B_Right_Time) {
          robotDrive.mecanumDrive_Polar(
              orch_B_Right_Mag,
              orch_B_Right_Dir,
              orch_B_Right_Rot); // Magnitude (speed), Direction (degrees), Rotation (Turning)
        }

        controlChuteRight[0] = false;
        controlChuteRight[1] = false;
      }

      // Stop mecanumDrive
      if (controlChuteRight[0] == false && controlChuteRight[1] == false) {

        robotDrive.mecanumDrive_Polar(
            stop_mecanum_Mag,
            stop_mecanum_Dir,
            stop_mecanum_Rot); // Magnitude (speed), Direction (degrees), Rotation (Turning)

        b_rightDriveAutonomous = false;
        b_leftAutonomous = false;
        b_rightAutonomous = false;
        b_centerAutonomous = false;
        b_upAutonomous = false;
        b_downAutonomous = false;
      }
    } // End Robot Aligns Right

    // Robot Aligns Center
    if (DigitalInput6.get() == false // pusher is fully retracted
        && DigitalInput7.get() == true
        && DigitalInput8.get() == true
        && b_leftAutonomous == false
        && b_rightAutonomous == false
        && b_centerAutonomous == true
        && b_upAutonomous == false
        && b_downAutonomous == true
        && b_centerDriveAutonomous == false) {

      LifterTalon.set(autoInitialDownSpeed);
      b_leftAutonomous = false;
      b_rightAutonomous = false;
      b_centerAutonomous = true;
      b_upAutonomous = true;
      b_downAutonomous = false;
      b_centerDriveAutonomous = false;
    }

    // Lifter Descends To LimitSwitch 1, lifter immediately ascends, reset booleans
    if (DigitalInput1.get() == false
        && b_leftAutonomous == false
        && b_rightAutonomous == false
        && b_centerAutonomous == true
        && b_upAutonomous == true
        && b_downAutonomous == false
        && b_centerDriveAutonomous == false) {

      LifterTalon.set(autoInitialUpSpeed);
      b_leftAutonomous = false;
      b_rightAutonomous = false;
      b_centerAutonomous = true;
      b_upAutonomous = false;
      b_downAutonomous = false;
      b_centerDriveAutonomous = false;
    }

    // Lifter ascends to LimitSwitch 4, stops motor, reset booleans
    if (DigitalInput4.get() == false
        && b_leftAutonomous == false
        && b_rightAutonomous == false
        && b_centerAutonomous == true
        && b_upAutonomous == false
        && b_downAutonomous == false
        && b_centerDriveAutonomous == false) {

      LifterTalon.set(autoSpeedStop);
      b_leftAutonomous = false;
      b_rightAutonomous = false;
      b_centerAutonomous = false;
      b_upAutonomous = false;
      b_downAutonomous = false;
      b_centerDriveAutonomous = true;

      // Delay one second
      Timer.delay(1);
      timer1Center.reset();
      timer1Center.start();
    }

    // Begin driving no limit switch since we might drive while lifting the can TBD.
    if (b_centerDriveAutonomous == true
        && b_leftAutonomous == false
        && b_rightAutonomous == false
        && b_centerAutonomous == false
        && b_upAutonomous == false
        && b_downAutonomous == false) {

      // Orchestration A Center
      if (controlChuteCenter[0] == true) {
        while ((autoTimerStart1Center = timer1Center.get()) < orch_A_Center_Time) {
          robotDrive.mecanumDrive_Polar(
              orch_A_Center_Mag,
              orch_A_Center_Dir,
              orch_A_Center_Rot); // Magnitude (speed), Direction (degrees), Rotation (Turning)
        }

        controlChuteCenter[0] = false;
        controlChuteCenter[1] = true;
        timer2Center.reset();
        timer2Center.start();
      }

      // Orchestration B Center
      if (controlChuteCenter[1] == true) {
        while ((autoTimerStart2Center = timer2Center.get()) < orch_B_Center_Time) {
          robotDrive.mecanumDrive_Polar(
              orch_B_Center_Mag,
              orch_B_Center_Dir,
              orch_B_Center_Rot); // Magnitude (speed), Direction (degrees), Rotation (Turning)
        }

        controlChuteCenter[0] = false;
        controlChuteCenter[1] = false;
      }

      // Stop mecanumDrive Center
      if (controlChuteCenter[0] == false && controlChuteCenter[1] == false) {

        robotDrive.mecanumDrive_Polar(
            stop_mecanum_Mag,
            stop_mecanum_Dir,
            stop_mecanum_Rot); // Magnitude (speed), Direction (degrees), Rotation (Turning)

        b_centerDriveAutonomous = false;
        b_leftAutonomous = false;
        b_rightAutonomous = false;
        b_centerAutonomous = false;
        b_upAutonomous = false;
        b_downAutonomous = false;
      }
    } // End Robot Aligns Center

    SmartDashboard.putBoolean("switch 1", DigitalInput1.get());
    SmartDashboard.putBoolean("switch 2", DigitalInput2.get());
    SmartDashboard.putBoolean("switch 3", DigitalInput3.get());
    SmartDashboard.putBoolean("switch 4", DigitalInput4.get());
    SmartDashboard.putBoolean("switch 5", DigitalInput5.get());
    SmartDashboard.putBoolean("switch 6", DigitalInput6.get());
    SmartDashboard.putNumber("Lifting Talon Speed", LifterTalon.get());
    SmartDashboard.putNumber("Pushing Talon Speed", PushingTalon.get());
    SmartDashboard.putNumber("servo position zero means brake is on", servo.get());
    SmartDashboard.putBoolean("Position switch Red", DigitalInput7.get());
    SmartDashboard.putBoolean("Position switch Green", DigitalInput8.get());
    SmartDashboard.putNumber("MainTimer", MainTimer);
    SmartDashboard.putNumber("autoTimerStart1Left", autoTimerStart1Left);
    SmartDashboard.putNumber("autoTimerStart2Left", autoTimerStart2Left);
    SmartDashboard.putNumber("autoTimerStart1Right", autoTimerStart1Right);
    SmartDashboard.putNumber("autoTimerStart2Right", autoTimerStart2Right);
    SmartDashboard.putNumber("autoTimerStart1Center", autoTimerStart1Center);
    SmartDashboard.putNumber("autoTimerStart2Center", autoTimerStart2Center);
  } // End autonomousPeriodic