/**
   * ***************************
   *
   * <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);
  }
  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);
  }
  private static CANTalon initCANTalon(
      CANTalon talon,
      FeedbackDevice device,
      boolean reverseSensor,
      int codesPerRev,
      int acceptableErr,
      double rightDriveKp,
      double rightDriveKi,
      double rightDriveKd) {
    talon.reset();
    talon.enableZeroSensorPositionOnIndex(true, true);
    talon.setPosition(0);
    talon.setFeedbackDevice(device);
    talon.reverseSensor(reverseSensor);

    if (FeedbackDevice.QuadEncoder.equals(device)) talon.configEncoderCodesPerRev(codesPerRev);
    else if (FeedbackDevice.AnalogPot.equals(device)) talon.configPotentiometerTurns(codesPerRev);

    talon.configNominalOutputVoltage(NOMINAL_FORWARD_VOLTAGE, NOMINAL_REVERSE_VOLTAGE);
    talon.configPeakOutputVoltage(PEAK_FORWARD_VOLTAGE, PEAK_REVERSE_VOLTAGE);

    talon.setAllowableClosedLoopErr(acceptableErr);
    talon.setPID(rightDriveKp, rightDriveKi, rightDriveKd);
    return talon;
  }
  public static void init() {

    if (inited) {
      talon_arm = new CANTalon(talon_arm_id);
      talon_arm.setFeedbackDevice(FeedbackDevice.CtreMagEncoder_Absolute);
      JoyDrive.init();
    }
  }
  public SpeedController() {
    speedController.setFeedbackDevice(FeedbackDevice.QuadEncoder);
    speedController.configEncoderCodesPerRev(20);

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

    speedController.setCloseLoopRampRate(.01);
  }
 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);
 }
  // 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!");
    }
  }
 @Override
 public TalonSRX setFeedbackDevice(FeedbackDevice device) {
   talon.setFeedbackDevice(edu.wpi.first.wpilibj.CANTalon.FeedbackDevice.valueOf(device.value()));
   switch (device) {
     case ANALOG_POTENTIOMETER:
     case ANALOG_ENCODER:
       if (selectedAnalogInput != null) {
         selectedInput = selectedAnalogInput;
       } else {
         Strongback.logger(getClass())
             .error(
                 "Unable to use the analog input for feedback, since the Talon SRX (device "
                     + getDeviceID()
                     + ") was not instantiated with an analog input. Check how this device was created using Strongback's Hardware class.");
         selectedInput = NO_OP_SENSOR;
       }
       break;
     case QUADRATURE_ENCODER:
     case ENCODER_RISING:
       if (selectedEncoderInput != null) {
         selectedInput = selectedEncoderInput;
       } else {
         Strongback.logger(getClass())
             .error(
                 "Unable to use the quadrature encoder input for feedback, since the Talon SRX (device "
                     + getDeviceID()
                     + ") was not instantiated with an encoder input. Check how this device was created using Strongback's Hardware class.");
         selectedInput = NO_OP_SENSOR;
       }
       break;
     case ENCODER_FALLING:
       // for 2015 the Talon SRX firmware did not support the falling or rising mode ...
       selectedInput = NO_OP_SENSOR;
       break;
   }
   return this;
 }
 @Override
 public void setFeedbackDevice(FeedbackDevice devType) {
   super.setFeedbackDevice(devType);
   feedbackDeviceIsPot = devType == FeedbackDevice.AnalogPot;
 }
Exemple #10
0
  /** Initializes sensors and controllers */
  public static void init() {
    compressorSolenoid = new Solenoid(0, 1);
    // RightShooterTalon = new CANTalon(6);
    // LeftShooterTalon = new CANTalon(7);

    // PneumaticCanRight = new Solenoid(0);
    // PneumaticCanLeft = new Solenoid(1);

    // DRIVE
    driveCurrentManager = new NewCurrentManager(12);

    DriveFrontRightTalon = new CANTalon(RobotConstants.frontRightMotorPort);
    DriveMidRightTalon = new CANTalon(RobotConstants.midRightMotorPort);
    DriveRearRightTalon = new CANTalon(RobotConstants.rearRightMotorPort);
    DriveFrontLeftTalon = new CANTalon(RobotConstants.frontLeftMotorPort);
    DriveMidLeftTalon = new CANTalon(RobotConstants.midLeftMotorPort);
    DriveRearLeftTalon = new CANTalon(RobotConstants.rearLeftMotorPort);

    DriveSolenoid =
        new DoubleSolenoid(
            1, RobotConstants.driveRightSolenoidPortA, RobotConstants.driveRightSolenoidPortB);
    //		DriveLeftSolenoid = new
    // DoubleSolenoid(1,RobotConstants.driveRightSolenoidPortA,RobotConstants.driveRightSolenoidPortB);

    DriveFrontRightMotor =
        new Motor(
            DriveFrontRightTalon, null, null, 0.01, 0.0, 0.0, null); // Robot.driveCurrentManager);
    DriveMidRightMotor =
        new Motor(
            DriveMidRightTalon, null, null, 0.01, 0.0, 0.0, null); // Robot.driveCurrentManager);
    DriveFrontLeftMotor =
        new Motor(DriveFrontLeftTalon, null, null, 0.01, 0, 0, null); // Robot.driveCurrentManager);
    DriveRearRightMotor =
        new Motor(DriveRearRightTalon, null, null, 0.01, 0, 0, null); // Robot.driveCurrentManager);
    DriveMidLeftMotor =
        new Motor(
            DriveMidLeftTalon, null, null, 0.01, 0.0, 0.0, null); // Robot.driveCurrentManager);
    DriveRearLeftMotor =
        new Motor(DriveRearLeftTalon, null, null, 0.01, 0, 0, null); // Robot.driveCurrentManager);

    DriveEncoder = new Encoder(RobotConstants.DriveEncoderPortA, RobotConstants.DriveEncoderPortB);
    DriveEncoder.setDistancePerPulse(RobotConstants.encoderDistancePerPulse);

    // ROLLER
    ArmTalon = new CANTalon(RobotConstants.ArmTalonPort);
    RollerTalon = new CANTalon(RobotConstants.RollerTalonPort);

    ArmSolenoid = new Solenoid(1, RobotConstants.ArmSolenoidPort);

    ArmPotentiometer = new AnalogPotentiometer(RobotConstants.ArmPotentiometerPort);

    ArmMotor = new Motor(ArmTalon, null, ArmPotentiometer, 30, 0.0, 0.0, 0.001);
    RollerMotor = new Motor(RollerTalon, null, null, 0.01, 0.0, 0.0);

    // SCALE
    ScaleRightTalon = new CANTalon(RobotConstants.ScaleRightTalonPort);
    ScaleLeftTalon = new CANTalon(RobotConstants.ScaleLeftTalonPort);

    // ScaleRightEncoder = new
    // Encoder(RobotConstants.ScaleRightEncoderPortA,RobotConstants.ScaleRightEncoderPortB);
    // ScaleLeftEncoder = new
    // Encoder(RobotConstants.ScaleLeftEncoderPortA,RobotConstants.ScaleLeftEncoderPortB);

    // ScaleSolenoid = new Solenoid(1,RobotConstants.ScaleSolenoidPort);

    ScaleRightMotor = new Motor(ScaleRightTalon, null, null, 0.01, 0.0, 0.0);
    ScaleLeftMotor = new Motor(ScaleLeftTalon, null, null, 0.01, 0.0, 0.0);

    // SHOOTER
    ShooterLeftTalon = new CANTalon(RobotConstants.ShooterLeftTalonPort);
    ShooterRightTalon = new CANTalon(RobotConstants.ShooterRightTalonPort);
    ShooterLiftTalon = new CANTalon(RobotConstants.ShooterLiftTalonPort);
    ShooterLiftTalon.setFeedbackDevice(FeedbackDevice.CtreMagEncoder_Relative);

    //		ShooterLeftTalon.set(0.5);
    //		ShooterRightTalon.set(0.5);

    ShooterBrakeSolenoid = new Solenoid(1, RobotConstants.ShooterBrakeSolenoidPortB);
    //    	ShooterBrakeSolenoid = new DoubleSolenoid(1,5,1);
    //    	ShooterSolenoidA = new Solenoid(1,3);
    //    	ShooterSolenoidB = new Solenoid(0,0);
    ShooterSolenoid = new Solenoid(0, RobotConstants.ShooterSolenoidPort);

    //		ShooterEncoder = new
    // Encoder(RobotConstants.ShooterLiftEncoderPort,RobotConstants.ShooterBrakeSolenoidPort);

    ShooterLeftMotor = new Motor(ShooterLeftTalon, null, null, 0.01, 0.0, 0.0);
    ShooterRightMotor = new Motor(ShooterRightTalon, null, null, 0.01, 0.0, 0.0);
    //		last working pid for shooter with ball and surgical tubing
    //    	ShooterLiftMotor = new Motor (ShooterLiftTalon, null, null, -1.2, -1,-0.14, 0.001);
    // working better. hitting target faster
    //   	ShooterLiftMotor = new Motor (ShooterLiftTalon, null, null, -4.8, -1,-0.14, 0.001);
    ShooterLiftMotor = new Motor(ShooterLiftTalon, null, null, -4, -1.2, -0.2, 0.001);

    // EXTENDER
    ExtenderTalon = new CANTalon(RobotConstants.ExtenderTalonPort);
    ExtenderPotentiometer = new AnalogPotentiometer(RobotConstants.ExtenderPotentiometerPort);
    //  	ExtenderSolenoid = new Solenoid(1,RobotConstants.ExtenderSolenoidPortA);
    //  	ExtenderSolenoid = new Solenoid(1,3);

    ExtenderMotor = new Motor(ExtenderTalon, null, ExtenderPotentiometer, 15, 0, 0);

    //      ExtenderSolenoid = new Solenoid(1,1);

    // CONTROL
    LeftJoy = new Joystick(RobotConstants.rightJoyPort);
    RightJoy = new Joystick(RobotConstants.leftJoyPort);

    ScaleRightMotor = new Motor(ScaleRightTalon, ScaleRightEncoder, null, 0, 0, 0);
    ScaleLeftMotor = new Motor(ScaleRightTalon, ScaleRightEncoder, null, 0, 0, 0);
  }