/**
   * ************************************
   *
   * <p>Does the configuration required for basic drive. So, when switching to basic drive this gets
   * called and now the robot will work with basic drive.
   *
   * <p>************************************
   */
  public void setBasic() {

    current = profiles.BASIC;

    leftMotor1.changeControlMode(CANTalon.TalonControlMode.PercentVbus);
    rightMotor1.changeControlMode(CANTalon.TalonControlMode.PercentVbus);
  }
  public static void teleopPeriodic() {

    /*
    double newArmPos = gamepad.getRawAxis(1);
    if(Math.abs(newArmPos) <= ARM_DEADZONE) {
    	newArmPos = 0.0;
    }
    double newMotorPos = (newArmPos * ARM_SPEED_MULTIPLIER) + frontArmMotor.getPosition();
    //double newMotorPos = (newArmPos * ARM_SPEED_MULTIPLIER);
    frontArmMotor.set(newMotorPos);
    System.out.println("input = " + newArmPos + " target pos = " + newMotorPos + " enc pos = " + frontArmMotor.getPosition());
    */
    // PercentVbus test ONLY!!

    double armSpeed = gamepad.getRawAxis(1);
    if (Math.abs(armSpeed) < ARM_DEADZONE) {
      armSpeed = 0.0f;
    }
    armSpeed *= ARM_MULTIPLIER;
    double pos = frontArmMotor.getPosition();
    if (((pos > SOFT_ENCODER_LIMIT_1) && armSpeed < 0.0)
        || ((pos < SOFT_ENCODER_LIMIT_2) && armSpeed > 0.0)) armSpeed = 0.0;
    frontArmMotor.set(armSpeed);
    System.out.println("armSpeed = " + armSpeed + " enc pos = " + frontArmMotor.getPosition());

    // check for roller motion (right gamepad joystick)
    double rollerSpeed = gamepad.getRawAxis(3);
    if (Math.abs(rollerSpeed) < ROLLER_DEADZONE) {
      rollerSpeed = 0.0f;
    }
    rollerSpeed *= ARM_ROLLER_MULTIPLIER;
    frontArmRollerMotor.set(rollerSpeed);
  }
  /**
   * ************************************************
   *
   * <p>Basic move is non PID, so it is cubic drive in percentVBus mode. -1 to 1
   *
   * @param left is left drive value (+) for forward (-) for reverse
   * @param right is right drive value (+) for forward (-) for reverse
   *     <p>************************************************
   */
  public void moveBasic(double left, double right) {

    if (getState() != CANTalon.TalonControlMode.PercentVbus) setBasic();

    leftMotor1.set(left);
    rightMotor1.set(right);
  }
  /**
   * This function is run when the robot is first started up and should be used for any
   * initialization code.
   */
  public void robotInit() {
    chooser = new SendableChooser();
    chooser.addDefault("Default Auto", defaultAuto);
    chooser.addObject("My Auto", customAuto);
    SmartDashboard.putData("Auto choices", chooser);

    controlMethod = new SendableChooser();
    controlMethod.addDefault("Tank Control", tankControl);
    controlMethod.addObject("Stick Control", stickControl);
    controlMethod.addObject("Clayton Control", claytonControl);
    SmartDashboard.putData("Control method", controlMethod);

    rightMotor = new CANTalon(RIGHT_INDEX);
    rightSlave = new CANTalon(RIGHT_INDEX + 1);
    rightSlave.changeControlMode(CANTalon.TalonControlMode.Follower);
    rightSlave.set(RIGHT_INDEX);
    rightMotor.setInverted(true);

    leftMotor = new CANTalon(LEFT_INDEX);
    leftSlave = new CANTalon(LEFT_INDEX + 1);
    leftSlave.changeControlMode(CANTalon.TalonControlMode.Follower);
    leftSlave.set(LEFT_INDEX);

    intakeMotor = new CANTalon(INTAKE_INDEX);
    // Arm motor is currently disabled
    // armMotor = new CANTalon(ARM_INDEX);

    compressor = new Compressor(0);
    compressor.setClosedLoopControl(true);

    intakePneumatic = new DoubleSolenoid(1, 2);
    intakePneumatic.set(DoubleSolenoid.Value.kOff);

    gamepad = new Gamepad(0);
  }
  /**
   * ********************************
   *
   * <p>Configures encoders: - Sets if the encoders are inverted - Configures tick count on encoders
   * (ours are 360)
   *
   * <p>********************************
   */
  private void setupEncoders() {

    leftMotor1.reverseSensor(invertLeftEncoder);
    rightMotor1.reverseSensor(invertRightEncoder);

    leftMotor1.configEncoderCodesPerRev(360);
    rightMotor1.configEncoderCodesPerRev(360);
  }
Exemple #6
0
 public void startWinch() {
   // TODO Don't start if the limit switch is set
   if (!atLowerLimit()) {
     d_motor.set(0.75);
   } else {
     d_motor.set(0);
   }
 }
Exemple #7
0
  public void tankDrive() {
    double left = OI.getInstance().getLeftStick().getY();
    double right = OI.getInstance().getRightStick().getY();
    drive.tankDrive(left, right);

    frontRight.set(RobotMap.REAR_RIGHT_MOTOR);
    frontLeft.set(RobotMap.REAR_LEFT_MOTOR);
  }
  public static void getencoder() {

    PulseWidthpos = talon_arm.getPulseWidthPosition();
    PulseWidthus = talon_arm.getPulseWidthRiseToFallUs();
    periodus = talon_arm.getPulseWidthRiseToRiseUs();
    PulseWidthVel = talon_arm.getPulseWidthVelocity();
    FeedbackDeviceStatus sensorstaus =
        talon_arm.isSensorPresent(FeedbackDevice.CtreMagEncoder_Absolute);
  }
 public double getRightEncoder(String inFt) {
   if (inFt.equals("in")) {
     double value = RIGHT_MOTOR1.getEncPosition();
     return countsToInch(value);
   } else {
     double value = RIGHT_MOTOR1.getEncPosition();
     return countsToFeet(value);
   }
 }
  /**
   * Set the setpoint for the PIDController
   *
   * @param setpoint the desired setpoint
   */
  public synchronized void setSetpoint(double setpoint) {

    m_setpoint = setpoint;
    m_talon.set(setpoint);
    ;

    reportPosition(m_talon.getPosition());

    if (m_debugging) SmartDashboard.putNumber(m_name + " Talon Setpoint", m_setpoint);
  }
  /**
   * ************************************
   *
   * <p>Does the configuration required for speed drive. So, when switching to speed drive this gets
   * called and now the robot will work with speed drive.
   *
   * <p>************************************
   */
  public void setSpeedMode() {

    current = profiles.SPEED;

    leftMotor1.setProfile(0);
    rightMotor1.setProfile(0);

    leftMotor1.changeControlMode(CANTalon.TalonControlMode.Speed);
    rightMotor1.changeControlMode(CANTalon.TalonControlMode.Speed);
  }
  /**
   * ************************************************
   *
   * <p>PIDSpeed move is a velocity based PID. -1 to 1
   *
   * @param leftSpeed is left drive value (+) for forward (-) for reverse
   * @param rightSpeed is right drive value (+) for forward (-) for reverse
   *     <p>************************************************
   */
  public void movePIDSpeed(double leftSpeed, double rightSpeed) {

    if (getState() != CANTalon.TalonControlMode.Speed) setSpeedMode();

    leftMotor1.set(
        leftSpeed
            * MOTOR_TOP_RPM); // Multiply by the top RPM because the -1 to 1 is the percent of the
                              // top RPM you would like to travel
    rightMotor1.set(rightSpeed * MOTOR_TOP_RPM);
  }
  /**
   * ************************************
   *
   * <p>Does the configuration required for positional drive. So, when switching to positional drive
   * this gets called and now the robot will work with positional drive.
   *
   * <p>************************************
   */
  public void setPositional() {

    current = profiles.POSITIONAL;

    leftMotor1.setProfile(0);
    rightMotor1.setProfile(0);
    // rightMotor1.reverseSensor(true);

    leftMotor1.changeControlMode(CANTalon.TalonControlMode.Position);
    rightMotor1.changeControlMode(CANTalon.TalonControlMode.Position);
  }
 public MasterCANTalon(int port) {
   talon = new CANTalon(port);
   talon.setFeedbackDevice(FeedbackDevice.CtreMagEncoder_Relative);
   talon.reverseSensor(false);
   talon.setProfile(0);
   talon.setF(0.1722);
   talon.setP(0.1);
   talon.setI(0);
   talon.setD(0);
   talon.configNominalOutputVoltage(+0.0f, -0.0f);
   talon.configPeakOutputVoltage(+12.0f, -12.0f);
   talon.changeControlMode(TalonControlMode.Speed);
 }
  public DriveTrainSubsystem() {
    RIGHT_MOTOR1 = new CANTalon(RobotMap.DRIVERIGHT_MOTOR1);
    RIGHT_MOTOR2 = new CANTalon(RobotMap.DRIVERIGHT_MOTOR2);
    LEFT_MOTOR1 = new CANTalon(RobotMap.DRIVELEFT_MOTOR1);
    LEFT_MOTOR2 = new CANTalon(RobotMap.DRIVELEFT_MOTOR2);
    LEFT_MOTOR1.setFeedbackDevice(FeedbackDevice.QuadEncoder);
    LEFT_MOTOR1.setPosition(0);
    RIGHT_MOTOR1.setFeedbackDevice(FeedbackDevice.QuadEncoder);
    RIGHT_MOTOR1.setPosition(0);

    LiveWindow.addActuator("Drive", "R1", RIGHT_MOTOR1);
    LiveWindow.addActuator("Drive", "R2", RIGHT_MOTOR2);
    LiveWindow.addActuator("Drive", "L1", LEFT_MOTOR1);
    LiveWindow.addActuator("Drive", "L2", LEFT_MOTOR2);
  }
 /** This method resets the motor position sensor, typically an encoder. */
 public void resetPosition() {
   if (feedbackDeviceIsPot) {
     zeroPosition = super.getPosition();
   } else {
     super.setPosition(0.0);
   }
 } // resetPosition
Exemple #17
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);
    }
  }
  public static void init() {

    if (inited) {
      talon_arm = new CANTalon(talon_arm_id);
      talon_arm.setFeedbackDevice(FeedbackDevice.CtreMagEncoder_Absolute);
      JoyDrive.init();
    }
  }
 public static void Dashboard() {
   SmartDashboard.putNumber("arm_value", talon_arm.get());
   SmartDashboard.putInt("PulseWidthpos", PulseWidthpos);
   SmartDashboard.putInt("PulseWidthus", PulseWidthus);
   SmartDashboard.putInt("periodus", periodus);
   SmartDashboard.putInt("PulseWidthVel", PulseWidthVel);
   SmartDashboard.putNumber("val", JoyDrive.val);
 }
  /**
   * This method sets the output power of the motor controller.
   *
   * @param output specifies the output power for the motor controller in the range of -1.0 to 1.0.
   */
  public void setPower(double power) {
    if (softLowerLimitEnabled && power < 0.0 && getPosition() <= softLowerLimit
        || softUpperLimitEnabled && power > 0.0 && getPosition() >= softUpperLimit) {
      power = 0.0;
    }

    super.set(power);
  } // setPower
  private void initSmartDashboard() {

    SmartDashboard.putNumber(m_name + " P", m_talon.getP());
    SmartDashboard.putNumber(m_name + " I", m_talon.getI());
    SmartDashboard.putNumber(m_name + " D", m_talon.getD());
    SmartDashboard.putNumber(m_name + " F", m_talon.getF());
    SmartDashboard.putNumber(m_name + " I*100", (m_talon.getI()) * 100);

    SmartDashboard.putNumber(m_name + " Talon Setpoint", m_setpoint);
    SmartDashboard.putNumber(m_name + " Position", m_talon.getPosition());
    SmartDashboard.putNumber(m_name + " Tolerance", m_tolerance);
    SmartDashboard.putNumber(m_name + " Error", m_talon.getClosedLoopError());
    SmartDashboard.putNumber(m_name + " Output", m_talon.get());
    SmartDashboard.putBoolean(m_name + " Enabled", m_talon.isControlEnabled());
  }
  /**
   * ************************************************
   *
   * <p>PIDPositional move is a position based PID that will move to a position some distance away
   * from current location
   *
   * @param leftDistanceInches is left drive value (+) for forward (-) for reverse
   * @param rightDistanceInches is right drive value (+) for forward (-) for reverse
   *     <p>************************************************
   */
  public void movePIDPositional(double leftDistanceInches, double rightDistanceInches) {

    if (getState() != CANTalon.TalonControlMode.Position) setPositional();

    double travelDistanceLeft = getLeftDistanceInches() + leftDistanceInches;
    double travelDistanceRight = getRightDistanceInches() + rightDistanceInches;

    travelDistanceLeft *= -1;
    travelDistanceRight *= -1;

    travelDistanceLeft /= ((FINAL_RADIUS * (2 * Math.PI)));
    // travelDistanceLeft *= 360.0;

    travelDistanceRight /= ((FINAL_RADIUS * (2 * Math.PI)));
    // travelDistanceRight *= 360.0;

    leftMotor1.set(travelDistanceLeft);
    rightMotor1.set(travelDistanceRight);
  }
Exemple #23
0
  public static void disabledInit() {
    if (!initialized) initialize();

    // if exiting from teleop mode (game is over)...
    if (teleopMode) {
      System.out.println("FrontArmAssembly: exiting teleop, moving to coast mode");
      // relax front arm motor (coast mode)
      frontArmMotor.enableBrakeMode(false);
    }
  }
Exemple #24
0
  public void usePIDOutput(double output) {
    // Use output to drive your system, like a motor
    // e.g. yourMotor.set(output);

    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=OUTPUT
    hinge.pidWrite(output);

    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=OUTPUT

  }
Exemple #25
0
  // static initializer
  public static void initialize() {
    if (!initialized) {

      gamepad = new Joystick(GAMEPAD_ID);

      initialized = true;
      teleopMode = false;

      // create and initialize arm motor
      frontArmMotor = new CANTalon(FRONT_ARM_MOTOR_ID);
      if (frontArmMotor != null) {

        System.out.println("Initializing front arm motor (position control)...");

        // VERY IMPORTANT - resets talon faults to render them usable again!!
        frontArmMotor.clearStickyFaults();
        frontArmMotor.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder);
        frontArmMotor.changeControlMode(CANTalon.TalonControlMode.PercentVbus);

        // set brake mode
        frontArmMotor.enableBrakeMode(true);

        // initializes encoder to zero
        frontArmMotor.setPosition(0);
      } else System.out.println("ERROR: Front Arm motor not initialized!");

      // create and initialize roller motor
      frontArmRollerMotor = new CANTalon(FRONT_ARM_ROLLER_ID);
      if (frontArmRollerMotor != null) {

        System.out.println("Initializing front arm roller motor (PercentVbus control)...");

        frontArmRollerMotor.clearStickyFaults();

        // set up roller motor for percent Vbus control mode
        frontArmRollerMotor.changeControlMode(CANTalon.TalonControlMode.PercentVbus);
        frontArmRollerMotor.enableBrakeMode(false);

        // initializes speed of rollers to zero
        frontArmRollerMotor.set(0);

      } else System.out.println("ERROR: Front Arm roller motor not initialized!");
    }
  }
  /**
   * Set the PID Controller gain parameters. Set the proportional, integral, and differential
   * coefficients.
   *
   * @param p Proportional coefficient
   * @param i Integral coefficient
   * @param d Differential coefficient
   * @param f Feed forward coefficient
   */
  public synchronized void setPID(double p, double i, double d, double f) {
    m_P = p;
    m_I = i;
    m_D = d;
    m_F = f;

    m_talon.setProfile(0);
    m_talon.setP(p);
    m_talon.setI(i);
    m_talon.setD(d);
    m_talon.setF(f);

    if (m_debugging) {
      SmartDashboard.putNumber(m_name + " P", m_talon.getP());
      SmartDashboard.putNumber(m_name + " I", m_talon.getI());
      SmartDashboard.putNumber(m_name + " D", m_talon.getD());
      SmartDashboard.putNumber(m_name + " F", m_talon.getF());
    }
  }
  /** This function is called periodically during operator control */
  public void teleopPeriodic() {
    MotorOutput motorOutput = drivetrainJoystick.motionForGamepadInput(gamepad);

    leftMotor.set(motorOutput.left * MAX_SPEED);
    rightMotor.set(motorOutput.right * MAX_SPEED);

    double in = gamepad.getLeftTrigger();
    double out = gamepad.getRightTrigger();
    intakeMotor.set((in - out) * INTAKE_SPEED);

    double fore = gamepad.getButtonLeftBack() ? 1 : 0;
    double back = gamepad.getButtonRightBack() ? 1 : 0;
    // Don't run the arm motor because it's not properly fastened
    // armMotor.set((fore - back) * ARM_SPEED);

    if (gamepad.getButtonA()) {
      intakePneumatic.set(DoubleSolenoid.Value.kForward);
    } else if (gamepad.getButtonB()) {
      intakePneumatic.set(DoubleSolenoid.Value.kReverse);
    } else {
      intakePneumatic.set(DoubleSolenoid.Value.kOff);
    }
  }
  public SpeedController() {
    speedController.setFeedbackDevice(FeedbackDevice.QuadEncoder);
    speedController.configEncoderCodesPerRev(20);

    speedController.setP(0.75);
    speedController.setI(0.01);
    speedController.setD(0.0);

    speedController.setCloseLoopRampRate(.01);
  }
  /**
   * ***************************
   *
   * <p>Completes everything required for PID to work: - Sets encoder/potentiometer type - Sets the
   * P, I, D, and F terms - configures min and max voltages
   *
   * <p>***************************
   */
  private void setupPID() {
    leftMotor1.setFeedbackDevice(FeedbackDevice.QuadEncoder);
    rightMotor1.setFeedbackDevice(FeedbackDevice.QuadEncoder);

    leftMotor1.configNominalOutputVoltage(+0f, -0f);
    rightMotor1.configNominalOutputVoltage(+0f, -0f);

    leftMotor1.configPeakOutputVoltage(+12.0f, -12.0f);
    rightMotor1.configPeakOutputVoltage(+12.0f, -12.0f);

    // leftMotor1.

    setPID(0.62, 0.006, 0.0, 0.36341);
  }
 @Override
 public TalonSRX setStatusFrameRate(StatusFrameRate frameRate, int periodMillis) {
   talon.setStatusFrameRateMs(
       edu.wpi.first.wpilibj.CANTalon.StatusFrameRate.valueOf(frameRate.value()), periodMillis);
   double periodInSeconds = periodMillis / 1000.0;
   switch (frameRate) {
     case FEEDBACK:
       feedbackRateInSeconds = periodInSeconds;
       break;
     case QUADRATURE_ENCODER:
       quadratureRateInSeconds = periodInSeconds;
       break;
     case ANALOG_TEMPERATURE_BATTERY_VOLTAGE:
       analogRateInSeconds = periodInSeconds;
       break;
     case GENERAL:
       // nothing to set, since our code doesn't use the "general" frames
       break;
   }
   return this;
 }