示例#1
0
  public void disabledPeriodic() {
    // gyroSubAvg += gyro.getAngle();
    // gyroSubAvg /= 2;

    SmartDashboard.putNumber("Gyro", gyro.getAngle());
    // SmartDashboard.putNumber("Gyro Scaling Value", gyroSubAvg);
  }
示例#2
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());
 }
示例#3
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;
  }
示例#4
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();
  }
示例#5
0
  public void teleopPeriodic() {
    /*SmartDashboard.getNumber("Accel x: ", accel.getX());
    SmartDashboard.getNumber("Accel Y: ", accel.getY());
    SmartDashboard.getNumber("Accel Z: ", accel.getZ());*/
    if (controller.isEnabled()) {
      if (((Timer.getFPGATimestamp() - timerStart) > timeToCancel) || controller.onTarget()) {
        controller.disable();
        timerStart = -1;
        timeToCancel = -1;
      }
    }

    if (j1.getRawButton(2)) {
      enc.reset();
      controller.setSetpoint(120); // destination 24 inches -> NO!! Trying to figure out this value

      timerStart = Timer.getFPGATimestamp();
      timeToCancel = 10; // timeout after 10 seconds
      controller.enable();
    } else if (j1.getRawButton(
        1)) { // this button stops the robot if the button 2 command goes crazy
      controller.disable();
      timerStart = -1;
      timeToCancel = -1;
    } else { // if time out or distance, end
      controller.disable();
      timerStart = -1;
      timeToCancel = -1;
    }

    if (!controller.isEnabled()) {
      rd.arcadeDrive(-j1.getY(), -j1.getX()); // normal arcade drive
    }

    if (j2.getRawAxis(2) != 0) { // set shooter values to the left trigger value
      shooter.set(-j2.getRawAxis(2));
      shooter2.set(-j2.getRawAxis(2));
      SmartDashboard.putNumber("Shooter: ", shooter.get());
    } else { // stop shooter
      shooter.set(0);
      shooter2.set(0);
    }
    /*if(j2.getRawButton(8)){ //runs intake, waits, runs shooter
    	shooter.set(-1);
    	intake.set(.5);
    	Timer.delay(.50);
    	intake.set(0);
    	Timer.delay(1.5);
    	intake.set(-1);
    	Timer.delay(1);
    	shooter.set(0);
    	intake.set(0);
    }*/

    // Need to mess around with sensitivity of light sensor
    if (j2.getRawAxis(3) != 0 && !light.get()) { // run intake at speed of right trigger
      intake.set(-1 * j2.getRawAxis(3));
    } else if (j2.getRawButton(5) && !light.get()) { // run intake into robot
      intake.set(-1);
    } else if (j2.getRawButton(6)) { // run intake out of robot
      intake.set(1);
    } else { // stop intake
      intake.set(0);
    }

    if (j2.getRawButton(2)) { // lift intake
      inup.set(DoubleSolenoid.Value.kForward);
      inup2.set(DoubleSolenoid.Value.kForward);
    } else if (j2.getRawButton(3)) { // drop intake
      inup.set(DoubleSolenoid.Value.kReverse);
      inup2.set(DoubleSolenoid.Value.kReverse);
    } else { // solenoids off
      inup.set(DoubleSolenoid.Value.kOff);
      inup2.set(DoubleSolenoid.Value.kOff);
    }

    // reading values
    SmartDashboard.putNumber("Encoder Dist: ", enc.getDistance()); // Distance is in inches
    SmartDashboard.putNumber("Encoder: ", enc.get());
    SmartDashboard.putNumber("Encoder Rate: ", enc.getRate());
    SmartDashboard.putNumber("Gyro: ", gyro.getAngle());
    SmartDashboard.putNumber("Encoder Raw", enc.getRaw());
    SmartDashboard.putNumber("Controller: ", controller.getSetpoint());
    SmartDashboard.putBoolean("Light Sensor: ", light.get());
  }
示例#6
0
 public void teleopInit() {
   gyro.reset();
   enc.reset();
 }
示例#7
0
 public void disabledInit() {
   gyroSubAvg = 0;
   gyro.reset();
 }
示例#8
0
 /** @return The robots heading in degrees. */
 public double getHeading() {
   return gyro.getAngle();
 }
示例#9
0
 /** Reset the robots sensors to the zero states. */
 public void reset() {
   gyro.reset();
   left_encoder.reset();
   right_encoder.reset();
 }