/** This function is called periodically during test mode */
 public void testPeriodic() {
   jag1.set(0);
   jag2.set(0);
   jag3.set(0);
   jag4.set(0);
   System.out.println(compressor.getPressureSwitchValue());
 }
  public void driveToBridge() {
    System.out.println("Val: " + potentiometer.getVoltage());

    if (Autonomousflag) {
      if (potentiometer.getVoltage() < 5) { // line
        shoot();
        Timer.delay(7);
        shoot.set(0);
        collect.set(0);

        driveTrain.tankDrive(-speed, speed);
        Timer.delay(1);
        driveTrain.tankDrive(speed, speed);
        Timer.delay(1.2);

        driveTrain.tankDrive(0, 0);
        Timer.delay(0.9);
        bridge.set(-1);

      } else if (potentiometer.getVoltage() > 5) { // left
        shoot();
      }
      Autonomousflag = false;
    } else {
      driveTrain.tankDrive(0, 0);
    }
  }
 /**
  * Method for closing the control loop.
  *
  * @param cmd Reference signal (setpoint)
  * @param act Feedback signal (measured response)
  */
 public void DoControl(double cmd, double act) {
   // Handle relay version first
   if (relay != null) {
     if (cmd - act > tolerance) {
       relay.set(Relay.Value.kForward);
     } else if (cmd - act < -tolerance) {
       relay.set(Relay.Value.kReverse);
     } else {
       relay.set(Relay.Value.kOff);
     }
   } else {
     if (cmd - act > tolerance) {
       if (reverseCmd) {
         jag.set(-speed);
       } else {
         jag.set(speed);
       }
     } else if (cmd - act < -tolerance) {
       if (reverseCmd) {
         jag.set(speed);
       } else {
         jag.set(-speed);
       }
     } else {
       jag.set(0.0);
     }
   }
 }
Exemple #4
0
  /** Handles periodic operations. Should be called periodically. */
  public void loop() {

    ds.setDigitalOut(8, limitSwitch.get());

    final int tolerance =
        10; // the number of degrees, in either direction, of tolerance for motor movement

    if (!isAccepting) {
      if (topTarget > topEncoder.get() && Math.abs(topTarget - topEncoder.get()) > tolerance) {
        int error = Math.abs(topTarget - topEncoder.get());

        topMotor.set(-limit(error / ERROR_CONVERSION));
        // SmartDashboard.log("Backwards", "Gripper Top Motor");
      } else if (topTarget < topEncoder.get()
          && Math.abs(topTarget - topEncoder.get()) > tolerance) {
        double error = Math.abs(topTarget - topEncoder.get());

        topMotor.set(limit(error / ERROR_CONVERSION));
        // SmartDashboard.log("Forward", "Gripper Top Motor");
      } else {
        topMotor.set(0);
      }

      if (bottomTarget > bottomEncoder.get()
          && Math.abs(bottomTarget - bottomEncoder.get()) > tolerance) {
        double error = Math.abs(bottomTarget - bottomEncoder.get());
        bottomMotor.set(-limit(error / ERROR_CONVERSION));
        // SmartDashboard.log("Backwards", "Gripper Bottom Motor");
      } else if (bottomTarget < bottomEncoder.get()
          && Math.abs(bottomTarget - bottomEncoder.get()) > tolerance) {
        double error = Math.abs(bottomTarget - bottomEncoder.get());
        bottomMotor.set(limit(error / ERROR_CONVERSION));
        // SmartDashboard.log("Forward", "Gripper Bottom Motor");
      } else {
        bottomMotor.set(0);
        // SmartDashboard.log("Stopped", "Gripper Bottom Motor");
      }
    }

    // if the gripper is in accept mode, it needs to stop when the limit switch is pressed
    if (isAccepting && !limitSwitch.get()) {
      // and also by simply stopping them
      topMotor.set(0);
      bottomMotor.set(0);
      isAccepting = false;
      // stop the motors by making the target positions equal to the current positions
      topTarget = topEncoder.get();
      bottomTarget = bottomEncoder.get();
    } else if (isAccepting && limitSwitch.get()) {
      // if it is accepting and the limit switch is not pressed
      topMotor.set(1);
      bottomMotor.set(-1);
    }
  }
 public void stopMotor() {
   if (RobotMap.IS_REAL_BOT == true) {
     kickerTalon.set(0.0);
   } else if (RobotMap.IS_REAL_BOT == false) {
     kickerJaguar.set(0.0);
   }
 }
  /*
   * requires a magnitude between -1 and 1 inclusive:
   * assumes that the angle is in degrees
   * calculates and sets the motor speeds for a given polar vector
   * allows for rotation while driving [-1,1]
   */
  private void driveMecanum() {

    /*
     * Does the computation of each motor speed value ever exceed the
     * range of -1.0, 1.0?  Just curious.
     *
     * - Mr. Ward
     */

    // RRLogger.logDebug(this.getClass(),"driveMecanum()", "driveMecanum()" );

    frontLeftMotor.set(-(l_magnitude + rotation) * Math.cos(Math.toRadians((l_angle + 45))));
    frontRightMotor.set((l_magnitude - rotation) * Math.sin(Math.toRadians(l_angle + 45)));
    backLeftMotor.set(-(l_magnitude + rotation) * Math.sin(Math.toRadians(l_angle + 45)));
    backRightMotor.set((l_magnitude - rotation) * Math.cos(Math.toRadians(l_angle + 45)));
  }
  /*
   *
   * @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);
      }
    }
  }
  private void driveCartesianTank() {
    // sensitivity
    double tuneForward = 1 * sensitivity;
    double tuneRight = 1 * sensitivity;
    double tuneClockwise = .75 * sensitivity;

    double Yf = (forward + right_forward) / 2;
    double Yt = (forward - right_forward) / 2;

    // can change this to also use the average of X sticks
    // currently does not use right x value
    double front_left = tuneForward * Yf + tuneClockwise * Yt + tuneRight * right;
    double front_right = tuneForward * Yf - tuneClockwise * Yt - tuneRight * right;
    double rear_left = tuneForward * Yf + tuneClockwise * Yt - tuneRight * right;
    double rear_right = tuneForward * Yf - tuneClockwise * Yt + tuneRight * right;

    // Normalize
    double max = Math.abs(front_left);

    if (Math.abs(front_right) > max) {
      max = Math.abs(front_right);
    }
    if (Math.abs(rear_left) > max) {
      max = Math.abs(rear_left);
    }
    if (Math.abs(rear_right) > max) {
      max = Math.abs(rear_right);
    }

    if (max > 1) {
      front_left /= max;
      front_right /= max;
      rear_left /= max;
      rear_right /= max;
    }

    frontLeftMotor.set(front_left);
    frontRightMotor.set(-front_right);
    backLeftMotor.set(rear_left);
    backRightMotor.set(-rear_right);
  }
  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);
  }
  /*
   * Uses a different or modified computation to resolve motor speeds
   * without trigonometric functions
   * Hopefully this will provide increased efficiency as well as
   * more precise driving
   */
  private void driveCartesianTest() {
    double front_left = forward + clockwise + right;
    double front_right = forward - clockwise - right;
    double rear_left = forward + clockwise - right;
    double rear_right = forward - clockwise + right;

    double max = Math.abs(front_left);

    max = Math.max(Math.abs(front_right), max);
    max = Math.max(Math.abs(rear_left), max);
    max = Math.max(Math.abs(rear_right), max);

    if (max > 1) {
      front_left /= max;
      front_right /= max;
      rear_left /= max;
      rear_right /= max;
    }

    frontLeftMotor.set(front_left);
    frontRightMotor.set(-front_right);
    backLeftMotor.set(rear_left);
    backRightMotor.set(-rear_right);
  }
Exemple #11
0
  public void teleopPeriodic() {
    watchdog.feed(); // feed the watchdog
    // Toggle drive mode
    if (!driveToggle && left.getRawButton(2)) {
      driveMode = (driveMode + 1) % 3;
      driveToggle = true;
    } else if (driveToggle && !left.getRawButton(2)) driveToggle = false;

    // Print drive mode to DS & send values to Jaguars
    switch (driveMode) {
      case 0:
        dsLCD.println(DriverStationLCD.Line.kMain6, 1, "Drive mode: Tank  ");
        red.set(left.getY() + gamePad.getY());
        black.set(-right.getY() - gamePad.getThrottle());
        break;
      case 1:
        dsLCD.println(DriverStationLCD.Line.kMain6, 1, "Drive mode: Arcade");
        red.set(right.getX() - right.getY());
        black.set(right.getX() + right.getY());
        break;
      case 2:
        dsLCD.println(DriverStationLCD.Line.kMain6, 1, "Drive mode: Kaj   ");
        red.set(left.getX() - right.getY());
        black.set(left.getX() + right.getY());
        break;
    } /**/
    dsLCD.println(
        DriverStationLCD.Line.kUser3,
        1,
        "1" + photoreceptorL.get() + "2" + photoreceptorM.get() + "3" + photoreceptorR.get());
    dsLCD.println(DriverStationLCD.Line.kUser4, 1, "Encoder in 4,5: " + leftEncoder.get() + "    ");
    dsLCD.println(
        DriverStationLCD.Line.kUser5, 1, "Encoder in 6,7: " + rightEncoder.get() + "    ");

    dsLCD.updateLCD();
  }
 /**
  * Sets the motor to move forward at a specific speed.
  *
  * @param speed the desired speed to move at
  */
 public void forward(int speed) {
   jag.set(speed);
   currentState = Component.ComponentState.COMPONENT_FORWARD;
 }
 public void stop() {
   jag1.set(0);
   jag2.set(0);
   jag3.set(0);
   jag4.set(0);
 }
Exemple #14
0
 /**
  * Directly specify motor speed.
  *
  * @param cmd speed command to issue to the jaguars.
  */
 private void SetMotors(double cmd) {
   motor1.set(-cmd);
   motor2.set(-cmd);
 }
Exemple #15
0
 public void shooterSet(double speed) {
   shootingMotor.set(speed);
   shootingMotor2.set(speed);
 }
Exemple #16
0
 public void stopShooter() {
   shootingMotor.set(0);
   shootingMotor2.set(0);
 }
 public void stop() {
   frontLeftMotor.set(0);
   frontRightMotor.set(0);
   backLeftMotor.set(0);
   backRightMotor.set(0);
 }
 /*
  * Drives the left and right wheels separately, with its repsective xbox stick
  * Allows for more precise rotation
  */
 private void driveTank() {
   frontLeftMotor.set(-(l_magnitude + rotation) * Math.cos(Math.toRadians((l_angle + 45))));
   frontRightMotor.set((r_magnitude - rotation) * Math.sin(Math.toRadians(r_angle + 45)));
   backLeftMotor.set(-(l_magnitude + rotation) * Math.sin(Math.toRadians(l_angle + 45)));
   backRightMotor.set((r_magnitude - rotation) * Math.cos(Math.toRadians(r_angle + 45)));
 }
 /**
  * Sets the motor to move backwards at a specific speed.
  *
  * @param speed the desired to move at
  */
 public void reverse(int speed) {
   jag.set(-speed);
   currentState = Component.ComponentState.COMPONENT_REVERSE;
 }
 /**
  * Sets the PWM value of the motor component.
  *
  * @param speed the motor speed between -1.0 and 1.0
  */
 public void set(double speed) {
   jag.set(speed);
 }
 public void shoot() {
   shoot.set(1); // Full power
   Timer.delay(6);
   collect.set(-1); // FIXED mechanical
 }
Exemple #22
0
 /** This function is called periodically during autonomous */
 public void autonomousPeriodic() {
   red.set(1);
   black.set(1);
 }
 public void run(double value) {
   shooter.set(value);
 }
 public void autoForward() {
   jag1.set(-AUTO_MOVE_FORWARD_SPEED); // - (gyro.getAngle()/AUTO_GYRO_REDUCTION));
   jag2.set(-AUTO_MOVE_FORWARD_SPEED); // - (gyro.getAngle()/AUTO_GYRO_REDUCTION));
   jag3.set(AUTO_MOVE_FORWARD_SPEED); // - (gyro.getAngle()/AUTO_GYRO_REDUCTION));
   jag4.set(AUTO_MOVE_FORWARD_SPEED); // - (gyro.getAngle()/AUTO_GYRO_REDUCTION));
 }
 public void adjustHood(double value) {
   hood.set(value);
 }