Ejemplo n.º 1
0
  /**
   * Constructor
   *
   * @param topJaguarChannel The PWM channel for the Jaguar for the top motor
   * @param bottomJaguarChannel The PWM channel for the Jaguar for the bottom motor;
   * @param topEncoderA The Digital I/O channel for channel A of the encoder for the top motor
   * @param topEncoderB The Digital I/O channel for channel B of the encoder for the top motor
   * @param bottomEncoderA The Digital I/O channel for channel A of the encoder for the bottom motor
   * @param bottomEncoderB The Digital I/O channel for channel B of the encoder for the bottom motor
   * @param limitSwitchChannel The Digital I/O channel for the gripper limit switch
   */
  public Gripper(
      int topJaguarChannel,
      int bottomJaguarChannel,
      int topEncoderA,
      int topEncoderB,
      int bottomEncoderA,
      int bottomEncoderB,
      int limitSwitchChannel) {
    topMotor = new Jaguar(topJaguarChannel);
    bottomMotor = new Jaguar(bottomJaguarChannel);

    topEncoder = new Encoder(topEncoderA, topEncoderB);
    topEncoder.start();
    topEncoder.reset();
    bottomEncoder = new Encoder(bottomEncoderA, bottomEncoderB);
    bottomEncoder.start();
    bottomEncoder.reset();

    topTarget = 0;
    bottomTarget = 0;

    limitSwitch = new DigitalInput(limitSwitchChannel);

    ds = DriverStation.getInstance();
  }
Ejemplo n.º 2
0
  /**
   * This function is run when the robot is first started up and should be used for any
   * initialization code.
   */
  public void robotInit() {
    black = new Jaguar(4, 1);
    red = new Jaguar(4, 2);
    leftEncoder = new Encoder(4, 5);
    rightEncoder = new Encoder(6, 7);
    left = new Joystick(1);
    right = new Joystick(2);
    gamePad = new Joystick(3);
    watchdog = Watchdog.getInstance();
    dsLCD = DriverStationLCD.getInstance();
    photoreceptorL = new DigitalInput(4, 1);
    photoreceptorM = new DigitalInput(4, 2);
    photoreceptorR = new DigitalInput(4, 3);
    camera = AxisCamera.getInstance();
    driveMode = 0; // 0 = Tank; 1 = Arcade; 2 = Kaj
    driveToggle = false;
    cruiseControl = false;

    camera.writeResolution(AxisCamera.ResolutionT.k160x120);
    camera.writeWhiteBalance(AxisCamera.WhiteBalanceT.hold);
    camera.writeExposureControl(AxisCamera.ExposureT.hold);
    camera.writeExposurePriority(AxisCamera.ExposurePriorityT.frameRate);

    leftEncoder.start();
    rightEncoder.start();
  }
Ejemplo n.º 3
0
  public static void init() {
    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
    driveLeftSpeedController = new Talon(9);
    LiveWindow.addActuator("Drive", "Left Speed Controller", (Talon) driveLeftSpeedController);

    driveRightSpeedController = new Talon(8);
    LiveWindow.addActuator("Drive", "Right Speed Controller", (Talon) driveRightSpeedController);

    driveRobotDrive21 = new RobotDrive(driveLeftSpeedController, driveRightSpeedController);

    driveRobotDrive21.setSafetyEnabled(true);
    driveRobotDrive21.setExpiration(0.1);
    driveRobotDrive21.setSensitivity(0.5);
    driveRobotDrive21.setMaxOutput(1.0);

    driveLeftEncoder = new Encoder(2, 3, false, EncodingType.k4X);
    LiveWindow.addSensor("Drive", "LeftEncoder", driveLeftEncoder);
    driveLeftEncoder.setDistancePerPulse(1.0);
    driveLeftEncoder.setPIDSourceType(PIDSourceType.kRate);
    driveRightEncoder = new Encoder(0, 1, false, EncodingType.k4X);
    LiveWindow.addSensor("Drive", "RightEncoder", driveRightEncoder);
    driveRightEncoder.setDistancePerPulse(1.0);
    driveRightEncoder.setPIDSourceType(PIDSourceType.kRate);

    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
  }
Ejemplo n.º 4
0
 /** The log method puts interesting information to the SmartDashboard. */
 public void log() {
   SmartDashboard.putNumber("Left Distance", left_encoder.getDistance());
   SmartDashboard.putNumber("Right Distance", right_encoder.getDistance());
   SmartDashboard.putNumber("Left Speed", left_encoder.getRate());
   SmartDashboard.putNumber("Right Speed", right_encoder.getRate());
   SmartDashboard.putNumber("Gyro", gyro.getAngle());
 }
Ejemplo n.º 5
0
 public void enable() {
   rightEncoder.reset();
   leftEncoder.reset();
   rightEncoder.start();
   leftEncoder.start();
   //       pidRight.enable();
   //       pidLeft.enable();
 } // end enable
Ejemplo n.º 6
0
 static {
   // stealerEncoder.setDistancePerPulse(.0245);
   stealerEncoder.setMaxPeriod(1 /* seconds */);
   stealerEncoder.reset();
   lassoCounter.reset();
   controller.setOutputRange(-0.2, 0.2);
   controller.setAbsoluteTolerance(50);
 }
Ejemplo n.º 7
0
 // basic arcadeDrive: y=forward/backward speed, x=left/right speed
 public void arcadeDrive(double y, double x) {
   SmartDashboard.putNumber("RightEncoder", rightEncoder.get());
   SmartDashboard.putNumber("LeftEncoder", leftEncoder.get());
   Preferences p = Preferences.getInstance();
   final boolean kReverseDirection = p.getBoolean("DriveTrainReverseDirection", false);
   robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, kReverseDirection);
   robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearRight, kReverseDirection);
   robotDrive.arcadeDrive(y, x);
 } // end arcadeDrive
Ejemplo n.º 8
0
 // starts encoders
 public DriveTrain() {
   leftEncoder.start();
   rightEncoder.start();
   gyro.reset();
   Preferences p = Preferences.getInstance();
   if (!p.containsKey("DriveTrainShootLimitVoltage")) {
     p.putDouble("DriveTrainShootLimitVoltage", kDefaultShootLimitVoltage);
   }
   if (!p.containsKey("DriveTrainQuickTurnP")) {
     p.putDouble("DriveTrainQuickTurnP", kDefaultQuickTurnProportion);
   }
   if (!p.containsKey("DriveTrainQuickTurnDeadband")) {
     p.putDouble("DriveTrainQuickTurnDeadband", kDefaultQuickTurnDeadband);
   }
   if (!p.containsKey("DriveTrainReverseDirection")) {
     p.putBoolean("DriveTrainReverseDirection", kDefaultReverseDirection);
   }
   if (!p.containsKey("LeftEncoderRatio")) {
     p.putDouble("LeftEncoderRatio", 4.875);
   }
   if (!p.containsKey("RightEncoderRatio")) {
     p.putDouble("RightEncoderRatio", 4.875);
   }
   if (!p.containsKey("Encoder_kP")) {
     p.putDouble("Encoder_kP", 0.05);
   }
   if (!p.containsKey("OutputMin")) {
     p.putDouble("OutputMin", .2);
   }
   if (!p.containsKey("DistBuffer")) {
     p.putDouble("DistBuffer", 1);
   }
   if (!p.containsKey("Gyro_kP")) {
     p.putDouble("Gyro_kP", 1 / 18);
   }
   if (!p.containsKey("AngleBuffer")) {
     p.putDouble("AngleBuffer", 4.5);
   }
   if (!p.containsKey("AutoDist_0")) {
     p.putDouble("AutoDist_0", 0.0);
   }
   if (!p.containsKey("AutoDist_1")) {
     p.putDouble("AutoDist_1", 0.0);
   }
   if (!p.containsKey("AutoDist_2")) {
     p.putDouble("AutoDist_2", 0.0);
   }
   if (!p.containsKey("AutoDist_3")) {
     p.putDouble("AutoDist_3", 0.0);
   }
   if (!p.containsKey("ShiftWhenTurningThreshold")) {
     p.putDouble("ShiftWhenTurningThreshold", 0.5);
   }
   if (!p.containsKey("ShiftWhenTurningDelay")) {
     p.putDouble("ShiftWhenTurningDelay", 1.5);
   }
 } // end constructor
Ejemplo n.º 9
0
 public void disable() {
   setSetpoint(0.0);
   //       pidRight.disable();
   //       pidLeft.disable();
   leftMotor.set(0.0);
   rightMotor.set(0.0);
   rightEncoder.reset();
   leftEncoder.reset();
 } // end disable
Ejemplo n.º 10
0
 public double getRPM() {
   if (Timer.getFPGATimestamp() > RPMUpdatePeriod + encoderMarkTime) {
     double encoderCPS =
         (enc.get() - encoderMarkCounts) / (Timer.getFPGATimestamp() - encoderMarkTime);
     encoderRPM = (encoderCPS * 60) / 16;
     encoderMarkCounts = enc.get();
     encoderMarkTime = Timer.getFPGATimestamp();
   }
   return encoderRPM;
 }
Ejemplo n.º 11
0
 public void enable() {
   rightEncoder.reset();
   leftEncoder.reset();
   rightEncoder.start();
   leftEncoder.start();
   gyro.reset();
   final boolean kReverseDirection =
       Preferences.getInstance().getBoolean("DriveTrainReverseDirection", false);
   robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, kReverseDirection);
   robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearRight, kReverseDirection);
 } // end enable
Ejemplo n.º 12
0
 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);
 }
Ejemplo n.º 13
0
 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();
 }
Ejemplo n.º 14
0
  // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
  public static void init() {
    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
    driveTrainLeftFront = new Talon(1, 1);
    LiveWindow.addActuator("Drive Train", "Left Front", (Talon) driveTrainLeftFront);

    driveTrainRightFront = new Talon(1, 2);
    LiveWindow.addActuator("Drive Train", "Right Front", (Talon) driveTrainRightFront);

    driveTrainLeftRear = new Talon(1, 3);
    LiveWindow.addActuator("Drive Train", "Left Rear", (Talon) driveTrainLeftRear);

    driveTrainRightRear = new Talon(1, 4);
    LiveWindow.addActuator("Drive Train", "Right Rear", (Talon) driveTrainRightRear);

    driveTrainRobotDrive =
        new RobotDrive(
            driveTrainLeftFront, driveTrainLeftRear, driveTrainRightFront, driveTrainRightRear);

    driveTrainRobotDrive.setSafetyEnabled(true);
    driveTrainRobotDrive.setExpiration(0.1);
    driveTrainRobotDrive.setSensitivity(0.5);
    driveTrainRobotDrive.setMaxOutput(1.0);
    driveTrainRobotDrive.setInvertedMotor(RobotDrive.MotorType.kFrontLeft, true);
    driveTrainRobotDrive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, true);
    driveTrainRobotDrive.setInvertedMotor(RobotDrive.MotorType.kFrontRight, true);
    driveTrainRobotDrive.setInvertedMotor(RobotDrive.MotorType.kRearRight, true);
    armWheelsShaftMotor = new Talon(1, 5);
    LiveWindow.addActuator("Arm Wheels", "Shaft Motor", (Talon) armWheelsShaftMotor);

    platformMotor = new Talon(1, 6);
    LiveWindow.addActuator("Platform", "Motor", (Talon) platformMotor);

    platformShaftEncoder = new Encoder(1, 1, 1, 2, false, EncodingType.k4X);
    LiveWindow.addSensor("Platform", "Shaft Encoder", platformShaftEncoder);
    platformShaftEncoder.setDistancePerPulse(1.0);
    platformShaftEncoder.setPIDSourceParameter(PIDSourceParameter.kRate);
    platformShaftEncoder.start();
    platformLimitSwitchFront = new DigitalInput(1, 3);
    LiveWindow.addSensor("Platform", "Limit Switch Front", platformLimitSwitchFront);

    platformLimitSwitchBack = new DigitalInput(1, 4);
    LiveWindow.addSensor("Platform", "Limit Switch Back", platformLimitSwitchBack);

    platformTestFrontTestLimitSwitch = new DigitalInput(1, 5);
    LiveWindow.addSensor(
        "Platform Test", "Front Test Limit Switch", platformTestFrontTestLimitSwitch);

    platformTestTestMotor = new Talon(1, 7);
    LiveWindow.addActuator("Platform Test", "Test Motor", (Talon) platformTestTestMotor);

    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
  }
Ejemplo n.º 15
0
 private double m_shoot() {
   if (encoder.get() >= desired || shootTime.get() > m_timeToShoot) {
     shooting = false;
     retracting = true;
     shootTime.stop();
     retractTime.start();
     retractTime.reset();
     return .1;
   } else { // encoder < desired && shootTime < 0.2
     error = desired - encoder.get();
     return -1;
   }
 }
Ejemplo n.º 16
0
 // lowers elevator partway for last pull-up at end
 public void lowerElevatorPartway() {
   if (climberEncoder.get() > lowerElevatorPartwayLimit) {
     if (!getError(false) || climberEncoder.get() > errorUpperLimit) {
       climberMotor.set(-elevatorSpeed);
     } else {
       if (!getUpperLimit()) {
         climberMotor.set(elevatorSpeed);
       }
     }
   } else {
     climberMotor.set(0);
   }
   putData();
 }
Ejemplo n.º 17
0
 private Drivetrain() {
   pdp = new PowerDistributionPanel();
   left = new Talon(kLeftDriveMotorPWM);
   right = new Talon(kRightDriveMotorPWM);
   lEncoder = new Encoder(kLeftDriveEncoderA, kLeftDriveEncoderB, false, EncodingType.k1X);
   lEncoder.setDistancePerPulse(kDistancePerPulse);
   lEncoder.reset();
   rEncoder = new Encoder(kRightDriveEncoderA, kRightDriveEncoderB, false, EncodingType.k1X);
   rEncoder.setDistancePerPulse(kDistancePerPulse);
   rEncoder.reset();
   lP = new PID("left", left, rEncoder);
   rP = new PID("Right", right, rEncoder);
   lP.setAbsoluteTolerance(1.0);
   rP.setAbsoluteTolerance(1.0);
 }
Ejemplo n.º 18
0
  public static void init() {
    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
    driveTrainLeftFront = new Talon(0);
    LiveWindow.addActuator("DriveTrain", "Left Front", (Talon) driveTrainLeftFront);

    driveTrainLeftRear = new Talon(1);
    LiveWindow.addActuator("DriveTrain", "Left Rear", (Talon) driveTrainLeftRear);

    driveTrainRightFront = new Talon(2);
    LiveWindow.addActuator("DriveTrain", "Right Front", (Talon) driveTrainRightFront);

    driveTrainRightRear = new Talon(3);
    LiveWindow.addActuator("DriveTrain", "Right Rear", (Talon) driveTrainRightRear);

    driveTrainHolonomicDrive =
        new RobotDrive(
            driveTrainLeftFront, driveTrainLeftRear, driveTrainRightFront, driveTrainRightRear);

    driveTrainHolonomicDrive.setSafetyEnabled(false);
    driveTrainHolonomicDrive.setExpiration(0.1);
    driveTrainHolonomicDrive.setSensitivity(0.5);
    driveTrainHolonomicDrive.setMaxOutput(1.0);

    driveTrainHolonomicDrive.setInvertedMotor(RobotDrive.MotorType.kFrontRight, true);
    driveTrainHolonomicDrive.setInvertedMotor(RobotDrive.MotorType.kRearRight, true);
    driveTrainGyro = new AnalogGyro(0);
    LiveWindow.addSensor("DriveTrain", "Gyro", driveTrainGyro);
    driveTrainGyro.setSensitivity(0.007);
    forkliftMotor = new Talon(4);
    LiveWindow.addActuator("Forklift", "Motor", (Talon) forkliftMotor);

    forkliftEncoder = new Encoder(0, 1, false, EncodingType.k4X);
    LiveWindow.addSensor("Forklift", "Encoder", forkliftEncoder);
    forkliftEncoder.setDistancePerPulse(0.012);
    forkliftEncoder.setPIDSourceType(PIDSourceType.kDisplacement);
    toteCaptureLeftWheel = new Talon(8);
    LiveWindow.addActuator("Tote Capture", "Left Wheel", (Talon) toteCaptureLeftWheel);

    toteCaptureRightWheel = new Talon(9);
    LiveWindow.addActuator("Tote Capture", "Right Wheel", (Talon) toteCaptureRightWheel);

    containerCaptureMotor = new Talon(10);
    LiveWindow.addActuator("Container Capture", "Motor", (Talon) containerCaptureMotor);

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

    powerDistributionPanel = new PowerDistributionPanel();
  }
Ejemplo n.º 19
0
  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());
  }
Ejemplo n.º 20
0
  public Shooter(int encoderA, int encoderB, int victor1, int victor2, int victor3) {

    encoder = new Encoder(encoderA, encoderB);
    encoder.start();
    encoder.reset();
    motor1 = new Victor(victor1); // 2
    motor2 = new Victor(victor2); // 5
    motor3 = new Victor(victor3); // 4

    time = new Timer();
    shootTime = new Timer();
    retractTime = new Timer();

    time.start();
    time.reset();
  }
Ejemplo n.º 21
0
  public void robotInit() {
    j1 = new Joystick(1);
    j2 = new Joystick(0);
    shooter = new Victor(2);
    shooter2 = new Victor(4);
    intake = new Victor(3);
    right = new Victor(0);
    left = new Victor(1);
    rd = new RobotDrive(left, right);
    light = new DigitalInput(3);
    count = 0;

    comp = new Compressor();
    inup = new DoubleSolenoid(0, 1);
    inup2 = new DoubleSolenoid(2, 3);

    gyro = new AnalogGyro(0);
    gyro.setSensitivity(.007);
    gyro.calibrate();
    gyro.reset();

    enc = new Encoder(1, 2, true, Encoder.EncodingType.k4X);
    enc.setDistancePerPulse(.11977); // circumference of wheel/200 (PPR)
    enc.setPIDSourceType(PIDSourceType.kDisplacement);
    enc.reset();

    out =
        new PIDOutput() {
          @Override
          public void pidWrite(double out) {
            rd.setLeftRightMotorOutputs(out, out);
          }
        };

    controller = new PIDController(.007, 0, 0, enc, out);
    controller.setAbsoluteTolerance(.25);
    controller.disable();

    gcontroller = new PIDController(kp, ki, kd, gyro, out);
    gcontroller.setAbsoluteTolerance(ktol);
    gcontroller.disable();

    timerStart = -1;
    timeToCancel = -1;
  }
Ejemplo n.º 22
0
 // starts encoders
 public DriveTrain() {
   leftEncoder.start();
   rightEncoder.start();
   gyro.reset();
   Preferences p = Preferences.getInstance();
   if (!p.containsKey("DriveTrainShootLimitVoltage")) {
     p.putDouble("DriveTrainShootLimitVoltage", kDefaultShootLimitVoltage);
   }
   if (!p.containsKey("DriveTrainQuickTurnP")) {
     p.putDouble("DriveTrainQuickTurnP", kDefaultQuickTurnProportion);
   }
   if (!p.containsKey("DriveTrainQuickTurnDeadband")) {
     p.putDouble("DriveTrainQuickTurnDeadband", kDefaultQuickTurnDeadband);
   }
   if (!p.containsKey("DriveTrainReverseDirection")) {
     p.putBoolean("DriveTrainReverseDirection", kDefaultReverseDirection);
   }
 } // end constructor
Ejemplo n.º 23
0
 private double m_retract() {
   if (encoder.get() <= 0 || retractTime.get() >= 1.0) {
     shooting = false;
     retracting = false;
     return 0;
   } else { // encoder > 0 && retractTime < 1.0
     return .1;
   }
 }
Ejemplo n.º 24
0
 // moves elevator to starting position and sets encoder
 public void homingSequence() {
   if (homeUp) {
     while (!getUpperLimit()) {
       climberMotor.set(homingSpeed);
     }
     while (getUpperLimit()) {
       climberMotor.set(-homingReverseSpeed);
     }
     climberEncoder.reset();
   } else {
     while (!getLowerLimit()) {
       climberMotor.set(-homingSpeed);
     }
     while (getLowerLimit()) {
       climberMotor.set(homingReverseSpeed);
     }
     climberEncoder.reset();
   }
 }
Ejemplo n.º 25
0
  private OI() {
    us = new AnalogChannel(US_PORT_1);
    us2 = new AnalogChannel(US_PORT_2);
    us3 = new AnalogChannel(US_PORT_3);
    quad = new Encoder(QUAD_PORT_1, QUAD_PORT_2);
    quadL = new Encoder(QUAD_PORT_3, QUAD_PORT_4);
    quadR = new Encoder(QUAD_PORT_5, QUAD_PORT_6);
    quad.start();
    quadL.start();
    quadR.start();
    pot = new AnalogChannel(POT_PORT);

    stick = new Joystick(JOYSTICK_PORT);
    stick2 = new Joystick(JOYSTICK_PORT_2);

    j2b1 = new JoystickButton(stick2, 1);
    j2b2 = new JoystickButton(stick2, 2);
    j2b3 = new JoystickButton(stick2, 3);
    j2b4 = new JoystickButton(stick2, 4);
  }
Ejemplo n.º 26
0
 public boolean driveStraight(double dist) {
   SmartDashboard.putNumber("RightEncoder", rightEncoder.get());
   SmartDashboard.putNumber("LeftEncoder", leftEncoder.get());
   Preferences p = Preferences.getInstance();
   L_encoderVal =
       leftEncoder.get()
           / p.getDouble(
               "LeftEncoderRatio",
               0.0); // converts encoder value to inches; encoder ratio should be about 58.76
   R_encoderVal =
       rightEncoder.get()
           / p.getDouble("RightEncoderRatio", 0.0); // converts encoder value to inches
   encoderVal = (R_encoderVal); // only one encoder
   encoderErr = dist - encoderVal;
   distOut =
       encoderErr * p.getDouble("Encoder_kP", 0.0)
           + (Math.abs(encoderErr) / encoderErr)
               * p.getDouble(
                   "OutputMin",
                   0.0); // Encoder kP = 0.5; abs(x)/x returns sign of x; 0.2 is the min. magnitude
   gyroErr = gyro.getAngle(); // Setpoint is always 0
   double angleOut = gyroErr * p.getDouble("Gyro_kP", 0.0) + .3; // * distOut / Math.abs(distOut);
   SmartDashboard.putNumber("encoderErr", encoderErr);
   SmartDashboard.putNumber("distOut", distOut);
   SmartDashboard.putNumber("gyroErr", gyro.getAngle());
   SmartDashboard.putNumber("angleOut", angleOut);
   if (Math.abs(encoderErr) < p.getDouble("DistBuffer", 0.0)
       && Math.abs(gyroErr) < p.getDouble("AngleBuffer", 0.0)) {
     //            leftEncoder.reset();
     //            rightEncoder.reset();
     // gyro.reset();
     // arcadeDrive(0, 0);
     return true; // returns true when robot gets to its goal
   } else {
     arcadeDrive(
         -distOut,
         angleOut); // Gyro kP = 1/18.0; Arcade Drive uses reversed rotate values (neg. goes Left /
     // pos. goes Right)
     return false; // returns false if robot still hasn't reached its goal yet
   }
 } // end driveStraight
Ejemplo n.º 27
0
 public void tankDrive(double leftSpeed, double rightSpeed) {
   Preferences p = Preferences.getInstance();
   final boolean kReverseDirection = p.getBoolean("DriveTrainReverseDirection", false);
   robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, kReverseDirection);
   robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearRight, kReverseDirection);
   //        if (kReverseDirection) {
   //            robotDrive.tankDrive(rightSpeed, leftSpeed);
   //            return;
   //        }
   robotDrive.tankDrive(leftSpeed, rightSpeed);
   SmartDashboard.putNumber("gyroErr", gyro.getAngle());
   SmartDashboard.putNumber("encoder", rightEncoder.get());
 } // end tankDrive
Ejemplo n.º 28
0
  /**
   * Constructor for leadscrew class.
   *
   * @param dsModule digital sidecar module number
   * @param motor1Chan first motor PWM channel
   * @param motor2Chan second motor PWM channel
   * @param encAChan encoder 'A' channel
   * @param encBChan encoder 'B' channel
   * @param homeSwitchChan homing switch input channel
   */
  public Leadscrew(
      int dsModule,
      int motor1Chan,
      int motor2Chan,
      int encAChan,
      int encBChan,
      int homeSwitchChan) {
    motor1 = new Jaguar(dsModule, motor1Chan);
    motor2 = new Jaguar(dsModule, motor2Chan);

    // controller = new BangBangController();
    final int switchDebounceCycles = 3;
    // homeSwitch = new Button(dsModule, homeSwitchChan, switchDebounceCycles);

    encoder = new Encoder(dsModule, encAChan, dsModule, encBChan);
    encoder.setDistancePerPulse(RobotConfiguration.leadscrewRatio);
    encoder.setReverseDirection(false);

    controller = new BangBangController(motor1, tolerance, false);

    Reset();
  }
Ejemplo n.º 29
0
 // raises the elevator, tries again if hooks don't catch
 public void raiseElevator() {
   if (!getUpperLimit()) {
     if (!getError(true) || climberEncoder.get() < errorLowerLimit) {
       climberMotor.set(elevatorSpeed);
     } else {
       if (!getLowerLimit()) {
         climberMotor.set(-elevatorSpeed);
       }
     }
   } else {
     climberMotor.set(0);
   }
   putData();
 }
Ejemplo n.º 30
0
  public DriveTrain() {
    super();
    front_left_motor = new Talon(1);
    back_left_motor = new Talon(2);
    front_right_motor = new Talon(3);
    back_right_motor = new Talon(4);
    drive = new RobotDrive(front_left_motor, back_left_motor, front_right_motor, back_right_motor);
    left_encoder = new Encoder(1, 2);
    right_encoder = new Encoder(3, 4);

    // Encoders may measure differently in the real world and in
    // simulation. In this example the robot moves 0.042 barleycorns
    // per tick in the real world, but the simulated encoders
    // simulate 360 tick encoders. This if statement allows for the
    // real robot to handle this difference in devices.
    if (Robot.isReal()) {
      left_encoder.setDistancePerPulse(0.042);
      right_encoder.setDistancePerPulse(0.042);
    } else {
      // Circumference in ft = 4in/12(in/ft)*PI
      left_encoder.setDistancePerPulse((4.0 / 12.0 * Math.PI) / 360.0);
      right_encoder.setDistancePerPulse((4.0 / 12.0 * Math.PI) / 360.0);
    }

    rangefinder = new AnalogInput(6);
    gyro = new AnalogGyro(1);

    // Let's show everything on the LiveWindow
    LiveWindow.addActuator("Drive Train", "Front_Left Motor", (Talon) front_left_motor);
    LiveWindow.addActuator("Drive Train", "Back Left Motor", (Talon) back_left_motor);
    LiveWindow.addActuator("Drive Train", "Front Right Motor", (Talon) front_right_motor);
    LiveWindow.addActuator("Drive Train", "Back Right Motor", (Talon) back_right_motor);
    LiveWindow.addSensor("Drive Train", "Left Encoder", left_encoder);
    LiveWindow.addSensor("Drive Train", "Right Encoder", right_encoder);
    LiveWindow.addSensor("Drive Train", "Rangefinder", rangefinder);
    LiveWindow.addSensor("Drive Train", "Gyro", gyro);
  }