예제 #1
0
  /** Operator-controlled drive for Teleop mode. */
  public void operatorControl() {
    boolean retry = true;

    while (retry) {
      try {
        retry = false;

        DriverStation.getInstance().setDigitalOut(2, false);
        DriverStation.getInstance().setDigitalOut(5, false);

        Robot.compressorPump.start();
        teleopMode.init();

        try {
          while (isOperatorControl() && isEnabled() && teleopMode.step()) {
            WorkerManager.work();
            Dashboard.render();
          }
        } catch (Exception ex) {
          logException(ex);
          retry = true;
        }

        teleopMode.disable();
        Robot.compressorPump.stop();
      } catch (Exception ex) {
        logException(ex);
        retry = true;
      }
    }
  }
예제 #2
0
 public String modeText() {
   System.out.println("thing: " + m_ds);
   return m_ds == null
       ? "Not connected"
       : m_ds.isDisabled()
           ? "Disabled"
           : m_ds.isAutonomous() ? "Autonomous" : m_ds.isTest() ? "Test" : "Teleop";
 }
예제 #3
0
  private void sendActions() {
    DriverStation.reportError(recording, false);

    try {
      // BufferedWriter bw = new BufferedWriter(new OutputStreamWriter((new Socket("LSCHS-ROBOTICS",
      // 5800).getOutputStream())));
      BufferedWriter bw =
          new BufferedWriter(new FileWriter(new File("/var/rcrdng/autonRecording3.rcrdng")));
      bw.write(recording);

    } catch (Exception e) {
      DriverStation.reportError(e.getMessage(), false);
    }
  }
예제 #4
0
  private byte[] output_voltage() {
    String voltage = Double.toString(_ds.getBatteryVoltage());
    if (voltage.length() != 4) {
      voltage = voltage.substring(0, 4);
    }

    byte[] output = new byte[10];

    byte second_digit_two = CHARS[voltage.charAt(1) - 48][1];
    second_digit_two |= (byte) 0b01000000;

    output[0] = (byte) (0b0000111100001111);

    output[2] = CHARS[31][0]; // V
    output[3] = CHARS[31][1]; // V
    output[4] = CHARS[voltage.charAt(3) - 48][0];
    ; // third digit
    output[5] = CHARS[voltage.charAt(3) - 48][1];
    ; // third digit
    output[6] = CHARS[voltage.charAt(1) - 48][0]; // second digit of voltage
    output[7] = second_digit_two; // second digit of voltage, with decimal point.
    output[8] = CHARS[voltage.charAt(0) - 48][0]; // first digit of voltage
    output[9] = CHARS[voltage.charAt(0) - 48][1]; // first digit of voltage

    return output;
  }
예제 #5
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();
  }
  /**
   * This function is run when the robot is first started up and should be used for any
   * initialization code.
   */
  public void robotInit() {
    autoTimer = new Timer();
    jag1 = new Jaguar(1, 1);
    jag2 = new Jaguar(1, 2);
    jag3 = new Jaguar(1, 3);
    jag4 = new Jaguar(1, 4);
    victor = new Victor(5);

    sol1 = new Solenoid(1);
    sol2 = new Solenoid(2);

    sol4 = new Solenoid(4);
    sol5 = new Solenoid(5);

    sol7 = new Solenoid(7);
    sol8 = new Solenoid(8);

    relay = new Relay(1, 1);

    digi14 = new DigitalInput(1, 14);
    digi13 = new DigitalInput(1, 12);
    digi3 = new DigitalInput(1, 3);

    driverstation = DriverStation.getInstance();
    teamColor = new DigitalOutput(1, 7);
    speedColor = new DigitalOutput(1, 8);
    suckingLED = new DigitalOutput(1, 9);
    readyShoot = new DigitalOutput(1, 6);

    encoder = new AnalogChannel(2);
    ultrasonic = new AnalogChannel(3);

    gyro = new Gyro(1);
    gyro.setSensitivity(0.007);
    gyro.reset();

    xBox = new Joystick(1);

    drive = new Drive(jag1, jag2, jag3, jag4, sol1, sol2, xBox, speedColor);
    loadAndShoot =
        new loadAndShoot(
            encoder,
            victor,
            sol4,
            sol5,
            sol7,
            sol8,
            xBox,
            digi14,
            digi13,
            digi3,
            smart,
            readyShoot);

    drive.start();
    loadAndShoot.start();
    compressor = new Compressor(1, 13, 1, 8);
    compressor.start();
  }
 public void setLEDTeamColour() {
   // System.out.println(driverstation.getAlliance().name);
   if (driverstation.getAlliance().name.startsWith("B")) {
     System.out.println("Blue");
     teamColor.set(true);
   } else {
     teamColor.set(false);
   }
 }
  @Override
  protected void initialize() {
    String defense = Robot.robot.getSelectedDefense();

    DriverStation.reportError("Building autotraverse for defense " + defense, false);

    // Traverse the selected defense
    Scheduler.getInstance().add(new AutoTraverseOnly(defense));
  }
예제 #9
0
 public void logData(
     DriverStation ds,
     Joystick left,
     Joystick right,
     Joystick co,
     DriveTrain dt,
     SuperStructure ss) {
   System.out.println(
       "Data: B:" + ds.getBatteryVoltage() + " L:" + left.getY() + " R:" + right.getY());
 }
예제 #10
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);
    }
  }
예제 #11
0
 public void run() {
   try {
     while (true) {
       if (isRecording) {
         record();
         Thread.sleep(timeStep);
       }
     }
   } catch (Exception e) {
     DriverStation.reportError(e.getStackTrace() + "", false);
   }
 }
예제 #12
0
  /** Automated drive for autonomous mode. */
  public void autonomous() {
    try {
      DriverStation.getInstance().setDigitalOut(2, false);
      DriverStation.getInstance().setDigitalOut(5, false);

      Robot.compressorPump.start();
      hybridMode.init();

      try {
        while (isAutonomous() && isEnabled() && hybridMode.step()) {
          WorkerManager.work();
          Dashboard.render();
        }
      } catch (Exception ex) {
        ex.printStackTrace();
      }

      hybridMode.disable();
      Robot.compressorPump.stop();
    } catch (Exception ex) {
      logException(ex);
    }
  }
예제 #13
0
  public void updateSmartDashboard() {
    if (joystick_left.getRawButton(2) && dev_ < 100) {
      dev_++;

    } else if (joystick_left.getRawButton(3) && dev_ > 0) {
      dev_--;
    }

    SmartDashboard.putDouble("X", joystick_left.getX());
    SmartDashboard.putDouble("Y", joystick_left.getY());
    // SmartDashboard.putDouble("Optical Sensor", Optical_Sensor.getVoltage());
    SmartDashboard.putDouble("Battery Voltage", DriverStation.getInstance().getBatteryVoltage());
    SmartDashboard.putInt("dev_", dev_);

    SmartDashboard.putDouble(
        "Elevator Sensor Value (.getAverageVoltage())",
        this.elevator.ana_chana.getAverageVoltage());

    String state = "ROBOT STATE INFORMATION:";
    if (this.elevator.state == 0) {
      state = state + "\nElevator Stopped.";
    }
    if (this.elevator.state == 1) {
      state = state + "\nElevator Moving.";
    }
    if (this.elevator.state == 2) {
      state = state + "\nElevator E-Stopped.";
    }

    //            state = state+"\nShooter Power Level (Raw):
    // "+this.shooter.calculateShooterSpeed();
    //            state = state+"\nShooter Power Level (Setting): "+this.shooter.shooterSpeedState;

    // if (this.integ.globalState==1){
    //    state = state+"\nMoving Shooter Up To Shoot.";
    // }if (this.integ.globalState==2){
    //    state = state+"\nShooting!";
    // }if (this.integ.globalState2LOCK){
    //     state = state+"\nREALLY SHOOTING!.";
    // }

    state = state + "\nNumber Of Discs: " + this.elevator.numDiscs;

    state =
        state
            + "\n Louis + Bobni Are Amazing (Hiding This Message Is ILLEGAL! IT WILL STOP THE ROBOT!)";

    SmartDashboard.putString("State", state);
    SmartDashboard.putBoolean("Intake", this.intake.toggle.get());
  }
예제 #14
0
 private static void openLog(String type) {
   long time = System.currentTimeMillis();
   if (log != null) log.close();
   try {
     log =
         new PrintWriter(
             new FileWriter(
                 "/" // + time + "_"
                     + type
                     + "_log.txt"));
     log.println(("" + time) + " New log created.");
   } catch (IOException e) {
     DriverStation.reportError("creating log failed", true);
   }
 }
예제 #15
0
 /**
  * This function is run when the robot is first started up and should be used for any
  * initialization code.
  */
 public void robotInit() {
   oi = new OI();
   chooser = new SendableChooser();
   chooser.addDefault("Default Auto", new ExampleCommand());
   // chooser.addObject("My Auto", new MyAutoCommand());
   SmartDashboard.putData("Auto mode", chooser);
   try {
     /* Communicate w/navX-MXP via the MXP SPI Bus.                                     */
     /* Alternatively:  I2C.Port.kMXP, SerialPort.Port.kMXP or SerialPort.Port.kUSB     */
     /* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details. */
     ahrs = new AHRS(SPI.Port.kMXP);
   } catch (RuntimeException ex) {
     DriverStation.reportError("Error instantiating navX-MXP:  " + ex.getMessage(), true);
   }
 }
 /**
  * Gets value from a button
  *
  * @param button number of the button
  * @return State of the button
  */
 public boolean getRawButton(int button) {
   if (button == ButtonType.kRTrigger.value) { // Abstracted Buttons from Analog Axis
     if (getThrottle() <= -.6) {
       return true;
     } else {
       return false;
     }
   }
   if (button == ButtonType.kLTrigger.value) { // Abstracted Buttons from Analog Axis
     if (getThrottle() >= .6) {
       return true;
     } else {
       return false;
     }
   }
   return ((0x1 << (button - 1)) & m_ds.getStickButtons(m_port)) != 0;
 }
예제 #17
0
  RobotIO() {
    // -------Motors-------
    frontLeftMotor = new Talon(RobotMap.frontLeftMotor);
    rearLeftMotor = new Talon(RobotMap.rearLeftMotor);
    frontRightMotor = new Talon(RobotMap.frontRightMotor);
    rearRightMotor = new Talon(RobotMap.rearRightMotor);

    shooterArmMotor = new Talon(RobotMap.shooterArmMotor);
    shooterLeftWheelMotor = new Talon(RobotMap.shooterLeftWheelMotor);
    shooterRightWheelMotor = new Talon(RobotMap.shooterRightWheelMotor);

    portcullisArmMotors = new Relay(RobotMap.portcullisArmMotors);

    climberLeftMotor = new Victor(RobotMap.climberLeftMotor);
    climberRightMotor = new Victor(RobotMap.climberRightMotor);

    // ------Solenoids-----
    shooterPistonExtend = new Solenoid(RobotMap.shooterPistonExtend);
    shooterPistonRetract = new Solenoid(RobotMap.shooterPistonRetract);

    portcullisLifterPistonsExtend = new Solenoid(RobotMap.portcullisLifterPistonsExtend);
    portcullisLifterPistonsRetract = new Solenoid(RobotMap.portcullisLifterPistonsRetract);

    // -------Sensors------
    topGyro = new AnalogGyro(RobotMap.topGyro);
    bottomGyro = new AnalogGyro(RobotMap.bottomGyro);

    leftBallIR = new AnalogInput(RobotMap.leftBallIR);
    rightBallIR = new AnalogInput(RobotMap.rightBallIR);

    driveEncoderLeft = new Encoder(RobotMap.driveEncoderLeftA, RobotMap.driveEncoderLeftB);
    driveEncoderRight = new Encoder(RobotMap.driveEncoderRightA, RobotMap.driveEncoderRightB);
    shooterArmEncoder = new Encoder(RobotMap.shooterArmEncoderA, RobotMap.shooterArmEncoderB);

    // -----Joysticks-----
    driverStick = new Joystick(RobotMap.driverStick);
    operatorStick = new Joystick(RobotMap.operatorStick);

    // -------Misc--------
    driveRobot = new RobotDrive(frontLeftMotor, rearLeftMotor, frontRightMotor, rearRightMotor);
    pdp = new PowerDistributionPanel();
    ds = DriverStation.getInstance();
  }
예제 #18
0
 public double doRobotdriveScaling(double value) {
   if (driverstation.getDigitalIn(Constants.driverstation_scale_toggle_channel)) {
     if (driverstation.getDigitalIn(Constants.driverstation_scale_toggle_channel2)) {
       driverstation.setDigitalOut(2, true);
       driverstation.setDigitalOut(1, false);
       driverstation.setDigitalOut(3, false);
       if (driverstation.getAnalogIn(Constants.driverstation_scale_channel) < 1) {
         return value * driverstation.getAnalogIn(Constants.driverstation_scale_channel);
       }
       driverstation.setDigitalOut(3, true);
       return 0;
     }
     driverstation.setDigitalOut(3, false);
     driverstation.setDigitalOut(1, true);
     driverstation.setDigitalOut(2, false);
     return value * driverstation.getAnalogIn(Constants.driverstation_scale_channel) * 0.2;
   }
   driverstation.setDigitalOut(3, false);
   driverstation.setDigitalOut(1, false);
   driverstation.setDigitalOut(2, false);
   return value * Constants.robotdrive_scale_hi;
 }
예제 #19
0
public class Team3699Robot extends SimpleRobot {

  public Joystick joystick_left = new Joystick(Constants.joystick_left_USB);
  public Joystick joystick_right = new Joystick(Constants.joystick_right_USB);
  public Joystick joystick_elevator = new Joystick(3);
  public DriverStationLCD userMessages = DriverStationLCD.getInstance();
  public DriverStation driverstation = DriverStation.getInstance();
  public NetworkTable server;

  /*<<<<<<< HEAD
  //    public ShooterControl shooter = new ShooterControl();
        public Jaguar shooterMotor = new Jaguar(Constants.shooterPWMChannel);
  =======*/
  public ShooterControl shooter = new ShooterControl();
  public Jaguar shooterMotor = new Jaguar(Constants.shooterPWMChannel);
  // >>>>>>> pULL dOWN sHOOTER dURING aUTONOMUS, wITH jUST a tIMED sCRIPT

  public ArmControl3 arm = new ArmControl3(this);
  public Jaguar armMotor = new Jaguar(Constants.armMotorPWM);

  public IntegrationControl2 integ = new IntegrationControl2(this);

  // public AnalogChannel Optical_Sensor = new AnalogChannel(Constants.OpticalSensorChannel);

  //    public Jaguar test_CIM_1 = new Jaguar(7);
  //    public Jaguar test_CIM_2 = new Jaguar(8);

  /*
  public DigitalInput Disc_Check = new DigitalInput(Constants.Disc_Check_Channel);
  public DigitalInput Disc_Top_Check = new DigitalInput(Constants.Disc_Top_Check_Channel);
  public DigitalInput Upside_Down_Check = new DigitalInput(Constants.Upside_Down_Check_Channel);
  */

  public RobotDrive robotdrive =
      new RobotDrive(Constants.robotdrive_left_PWM, Constants.robotdrive_right_PWM);
  public Jaguar Intake_motor = new Jaguar(Constants.intake_PWM);
  public Jaguar Elevator_motor = new Jaguar(Constants.elevator_PWM);
  public Jaguar Elevator_intake_motor = new Jaguar(Constants.elevator_intake_PWM);
  public Jaguar pullDownJaguar = new Jaguar(Constants.pulldownPWM);
  // public Jaguar Elevator_intake = new Jaguar(Constants.elevator_intake_PWM);
  // public Jaguar Elevator_outtake = new Jaguar(Constants.elevator_outtake_PWM);

  public ElevatorControl elevator = new ElevatorControl(this);
  public IntakeControl intake = new IntakeControl(this);
  // public Auto_Integration2 integ2 = new Auto_Integration2(this);
  public Shoot3Auto auto_shoot = new Shoot3Auto(this);
  // SendableChooser autoChooser;

  int dev_ = 0;

  public boolean[] errors = new boolean[10];

  public AutoPullDown pullDown = new AutoPullDown();

  // ^ Yeah, yeah. Way overinitizized
  // ERRNO 0: Init error
  // ERRNO 1: SmartDashboard error
  // ERRNO 2: Teleop error

  public boolean allowUnlimitedErrors = true;

  // Set to true to allow unlimeted errors

  public void error(String message, int errno) {
    if (!(this.errors[errno]) || this.allowUnlimitedErrors) {
      log(message);
      errors[errno] = true;
    }
  }

  public void resetError(int errno) {
    errors[errno] = false;
  }

  public void log(String message) {
    System.out.println(message);
  }

  public void robotInit() {
    log("RobotInit called. Initializing NetworkTabes...");
    try {
      NetworkTable.setIPAddress("10.36.99.2");
      NetworkTable.setServerMode();
      NetworkTable.setTeam(3699);
      NetworkTable.initialize();

    } catch (Exception be) {
      error("Error in robotInit: \n " + be.getMessage(), 0);
      error("Reocourrence Test! You shouldnt see this!", 0);
    }
    log("Init SmartDashboard...");
    this.server = NetworkTable.getTable("SmartDashboard");

    log("" + this.server.containsKey("/SmartDashboard/"));
    log("" + this.server.containsKey("/SmartDashboard/FPS"));
    log("" + NetworkTable.DEFAULT_PORT);
    this.server.putString("_PREINIT0", "_PREINIT::0");
    this.server.putString("_PREINIT1", "_PREINIT::1");
    this.server.putString("_PREINIT2", "_PREINIT::2");

    /*LiveWindow.addActuator("Elevator", "Elevator Intake", Elevator_intake);
    LiveWindow.addActuator("Elevator", "Elevator Gearbox", Elevator_motor);
    LiveWindow.addActuator("Elevator", "Elevator Outtake", Elevator_outtake);
    LiveWindow.addActuator("Intake", "Intake", Elevator_motor);*/

    log("Disabling RobotDrive Saftey... :D");
    robotdrive.setSafetyEnabled(false);

    // autoChooser = new SendableChooser();
    // autoChooser.addDefault("Default 3 Discs", this.auto_shoot);
    // SmartDashboard.putData("Autonomous mode chooser",autoChooser);

  }

  public void operatorControl() {
    log("Teleop Called Once!");
    resetError(1);
    resetError(2);
    this.elevator.resetState();
    //        try{
    showUserMessages("TeleOp");
    this.arm.state = 0;

    while (isOperatorControl() && isEnabled()) {
      // robotdrive.setSafetyEnabled(true); //In case the program were to stop, this stops the
      // motors from continuing to run.
      if (!Util.checkButton(this, Constants.robotdrive_break_button)) {
        //                robotdrive.tankDrive(-doRobotdriveScaling(joystick_left.getY()),
        //                        -doRobotdriveScaling(joystick_right.getY()));
        robotdrive.arcadeDrive(
            doRobotdriveScaling(joystick_left.getY()), doRobotdriveScaling(joystick_left.getX()));
      }
      // x, y int := 0, 1;
      //            if (driverstation.getDigitalIn(3)){
      //                this.test_CIM_1.set(driverstation.getAnalogIn(2));
      //            }else{
      //                this.test_CIM_1.set(0-driverstation.getAnalogIn(2));
      //            }
      //
      //            if (driverstation.getDigitalIn(4)){
      //                this.test_CIM_2.set(driverstation.getAnalogIn(3));
      //            }else{
      //                this.test_CIM_2.set(0-driverstation.getAnalogIn(3));
      //            }

      updateSmartDashboard();

      this.shooter.updateStates(this);
      this.shooterMotor.set(this.shooter.calculateShooterSpeed());

      if (this.driverstation.getDigitalIn(4)) {
        this.armMotor.set(this.driverstation.getAnalogIn(3));
      } else {
        this.armMotor.set(-this.driverstation.getAnalogIn(3));
      }

      this.arm.update();
      if (this.arm.getArmSpeed() != 0D) {
        this.armMotor.set(this.arm.getArmSpeed());
      }

      if (this.integ.doElevatorUpdate()) {
        this.elevator.update();
        this.Elevator_motor.set(this.elevator.getElevatorSpeed());
      }

      this.intake.update();
      this.Intake_motor.set(this.intake.calculateIntakeSpeed());
      if (this.intake.state == 1) {
        this.Elevator_intake_motor.set(0.6D);
      } else {
        this.Elevator_intake_motor.set(0D);
      }

      if (this.driverstation.getDigitalIn(3)) {
        this.pullDownJaguar.set(this.driverstation.getAnalogIn(2));
      } else {
        this.pullDownJaguar.set(-this.driverstation.getAnalogIn(2));
      }

      this.integ.update();

      SmartDashboardUpdater.updateSmartDashboard(this);

      if (Util.checkButton(this, 9)) {

        this.elevator.numDiscs = 0;
      }
      if (Util.checkButton(this, 8)) {
        this.elevator.state = 0;
        this.integ.state = 0;
        this.intake.state = 0;
        this.integ.elevator_out_roller.set(0d);
        this.arm.state = 0;
      }

      {
        try {
          SmartDashboard.putString("DEBUG*isConnected", "" + this.server.isConnected());
          SmartDashboard.putString("DEBUG*isServer", "" + this.server.isServer());
          SmartDashboard.putDouble("FPS~!", this.server.getNumber("FPS", -1d));
          SmartDashboard.putDouble("NewDistance~!", this.server.getNumber("NewDistance", -1d));
          // SmartDashboard.putString("NetworkTable Keys", this.server.);

          log("W0RK1NG!");
        } catch (Exception be) {
          error("ERROR UPDATING SmartDashboard dtt STACK TRACE: " + be.getMessage(), 1);
        }
      }

      Timer.delay(0.005); // and make sure we dont overload the cRIO
    }
    //        }catch (Exception be){
    //            error("ERROR IN TELEOP! STACKTRACE: \n "+be.getMessage(), 2);
    //        }
  }

  public void disabled() {
    log("Disabled.");
    showUserMessages("Disabled");
  }

  public void autonomous() {
    log("Autonomus! (XD I am bad at spelling)");
    showUserMessages("Autonomous"); /*
<<<<<<< HEAD
        //autoChooser.getSelected();
        auto_shoot.update();


//        robotdrive.setSafetyEnabled(false);
//        robotdrive.tankDrive(doRobotdriveScaling(-0.45),doRobotdriveScaling(-0.45));
//        Timer.delay(2.0);
//        robotdrive.tankDrive(doRobotdriveScaling(0.0),doRobotdriveScaling(0.0)); // Use to determine ft/s at this power.

=======
        this.pullDown.reset();
        while (this.isAutonomous()&&isEnabled()){
        this.pullDown.pullDown(this);

        }
        //robotdrive.tankDrive(doRobotdriveScaling(-0.45),doRobotdriveScaling(-0.45));
        //Timer.delay(2.0);
        //robotdrive.tankDrive(doRobotdriveScaling(0.0),doRobotdriveScaling(0.0)); // Use to determine ft/s at this power.
>>>>>>> pULL dOWN sHOOTER dURING aUTONOMUS, wITH jUST a tIMED sCRIPT
*/
  }

  public double doRobotdriveScaling(double value) {
    if (driverstation.getDigitalIn(Constants.driverstation_scale_toggle_channel)) {
      if (driverstation.getDigitalIn(Constants.driverstation_scale_toggle_channel2)) {
        driverstation.setDigitalOut(2, true);
        driverstation.setDigitalOut(1, false);
        driverstation.setDigitalOut(3, false);
        if (driverstation.getAnalogIn(Constants.driverstation_scale_channel) < 1) {
          return value * driverstation.getAnalogIn(Constants.driverstation_scale_channel);
        }
        driverstation.setDigitalOut(3, true);
        return 0;
      }
      driverstation.setDigitalOut(3, false);
      driverstation.setDigitalOut(1, true);
      driverstation.setDigitalOut(2, false);
      return value * driverstation.getAnalogIn(Constants.driverstation_scale_channel) * 0.2;
    }
    driverstation.setDigitalOut(3, false);
    driverstation.setDigitalOut(1, false);
    driverstation.setDigitalOut(2, false);
    return value * Constants.robotdrive_scale_hi;
  }

  public void showUserMessages(String status) {
    this.userMessages.println(DriverStationLCD.Line.kUser2, 1, "FRC Robotics Team              ");
    this.userMessages.println(
        DriverStationLCD.Line.kUser3, 1, "3699 Robot " + status + "              ");
    this.userMessages.println(DriverStationLCD.Line.kUser4, 1, Constants.version);
    this.userMessages.println(DriverStationLCD.Line.kUser5, 1, "===VENDETTA LIST:===");
    this.userMessages.println(DriverStationLCD.Line.kUser6, 1, "(Names Forthcoming) ");
    this.userMessages.updateLCD();
  }

  public void updateSmartDashboard() {
    if (joystick_left.getRawButton(2) && dev_ < 100) {
      dev_++;

    } else if (joystick_left.getRawButton(3) && dev_ > 0) {
      dev_--;
    }

    SmartDashboard.putDouble("X", joystick_left.getX());
    SmartDashboard.putDouble("Y", joystick_left.getY());
    // SmartDashboard.putDouble("Optical Sensor", Optical_Sensor.getVoltage());
    SmartDashboard.putDouble("Battery Voltage", DriverStation.getInstance().getBatteryVoltage());
    SmartDashboard.putInt("dev_", dev_);

    SmartDashboard.putDouble(
        "Elevator Sensor Value (.getAverageVoltage())",
        this.elevator.ana_chana.getAverageVoltage());

    String state = "ROBOT STATE INFORMATION:";
    if (this.elevator.state == 0) {
      state = state + "\nElevator Stopped.";
    }
    if (this.elevator.state == 1) {
      state = state + "\nElevator Moving.";
    }
    if (this.elevator.state == 2) {
      state = state + "\nElevator E-Stopped.";
    }

    //            state = state+"\nShooter Power Level (Raw):
    // "+this.shooter.calculateShooterSpeed();
    //            state = state+"\nShooter Power Level (Setting): "+this.shooter.shooterSpeedState;

    // if (this.integ.globalState==1){
    //    state = state+"\nMoving Shooter Up To Shoot.";
    // }if (this.integ.globalState==2){
    //    state = state+"\nShooting!";
    // }if (this.integ.globalState2LOCK){
    //     state = state+"\nREALLY SHOOTING!.";
    // }

    state = state + "\nNumber Of Discs: " + this.elevator.numDiscs;

    state =
        state
            + "\n Louis + Bobni Are Amazing (Hiding This Message Is ILLEGAL! IT WILL STOP THE ROBOT!)";

    SmartDashboard.putString("State", state);
    SmartDashboard.putBoolean("Intake", this.intake.toggle.get());
  }
}
예제 #20
0
public class Digit_Board {

  private static class Board_Task implements Runnable {
    private Digit_Board _b;

    Board_Task(Digit_Board b) {
      _b = b;
    }

    public void run() {
      _b.board_task();
    }
  }

  private static Digit_Board instance = new Digit_Board();

  private Thread _task_thread;
  private boolean _do_things = false;

  public static Digit_Board getInstance() {
    return Digit_Board.instance;
  }

  protected Digit_Board() {
    _task_thread = new Thread(new Board_Task(this), "1504_Display_Board");
    _task_thread.setPriority((Thread.NORM_PRIORITY + Thread.MAX_PRIORITY) / 2);

    DisplayInit();

    start();

    System.out.println("MXP Leader, standing by.");
  }

  public void start() {
    if (_do_things) return;
    _do_things = true;
    _task_thread = new Thread(new Board_Task(this));
    _task_thread.start();
  }

  public void stop() {
    _do_things = false;
  }

  private DriverStation _ds = DriverStation.getInstance();

  private I2C _display_board;
  private DigitalInput _a;
  private DigitalInput _b;
  // taken from mike
  private static final int A_MASK = 0b0000000000000001;
  private static final int B_MASK = 0b0000000000000010;
  private volatile int _input_mask, _input_mask_rising, _input_mask_rising_last;

  private static long _timeout = 2500;

  private AnalogInput _pot;

  private static enum STATE {
    Voltage,
    Position,
    Obstacle,
    Wait
  }

  private static String[] _positions = {"P  1", "P  2", "P  3", "P  4", "P  5"};
  private static String[] _obstacles = {
    "LBAR", "PORT", "DRAW", "MOAT", "FRIS", "RAMP", "SALP", "ROCK", "TERR"
  };

  int pos = 0;
  int obs = 0;

  private void DisplayInit() {
    _display_board = new I2C(I2C.Port.kMXP, 0x70);

    _a = new DigitalInput(19);
    _b = new DigitalInput(20);
    _pot = new AnalogInput(7);
  }

  public void update() {
    int current_mask = get_input_mask();

    _input_mask |= current_mask;

    _input_mask_rising |= (~_input_mask_rising_last & current_mask);
    _input_mask_rising_last = current_mask;
  }

  private int get_input_mask() {
    int mask = 0;
    mask |= (getA() ? 1 : 0) << A_MASK;
    mask |= (getB() ? 1 : 0) << B_MASK;
    return mask;
  }

  private boolean getRawButtonOnRisingEdge(int button_mask) {
    button_mask = button_mask << 1;
    // Compute a clearing mask for the button.
    int clear_mask = 0b1111111111111111 - button_mask;
    // Get the value of the button - 1 or 0
    boolean value = (_input_mask_rising & button_mask) != 0;
    // Mask this and only this button back to 0
    _input_mask_rising &= clear_mask;
    return value;
  }

  private boolean getRawButtonLatch(int button_mask) {
    // Compute a clearing mask for the button.
    int clear_mask = 0b1111111111111111 - button_mask;
    // Get the value of the button - 1 or 0
    boolean value = (_input_mask & button_mask) != 0;
    // Mask this and only this button back to 0
    _input_mask &= clear_mask;
    return value;
  }

  public boolean getA() {
    return (!_a.get());
  }

  public boolean getALatch() {
    return getRawButtonLatch(A_MASK);
  }

  public boolean getAOnRisingEdge() {
    return getRawButtonOnRisingEdge(A_MASK);
  }

  public boolean getB() {
    return (!_b.get());
  }

  public boolean getBLatch() {
    return getRawButtonLatch(B_MASK);
  }

  public boolean getBOnRisingEdge() {
    return getRawButtonOnRisingEdge(B_MASK);
  }

  public double getPot() {
    double val = (double) _pot.getAverageValue(); // integer between 3 - 400
    double delay = Math.min((val / 400), 10.0); // number between 0 and 10
    return delay;
  }

  private byte[] output_voltage() {
    String voltage = Double.toString(_ds.getBatteryVoltage());
    if (voltage.length() != 4) {
      voltage = voltage.substring(0, 4);
    }

    byte[] output = new byte[10];

    byte second_digit_two = CHARS[voltage.charAt(1) - 48][1];
    second_digit_two |= (byte) 0b01000000;

    output[0] = (byte) (0b0000111100001111);

    output[2] = CHARS[31][0]; // V
    output[3] = CHARS[31][1]; // V
    output[4] = CHARS[voltage.charAt(3) - 48][0];
    ; // third digit
    output[5] = CHARS[voltage.charAt(3) - 48][1];
    ; // third digit
    output[6] = CHARS[voltage.charAt(1) - 48][0]; // second digit of voltage
    output[7] = second_digit_two; // second digit of voltage, with decimal point.
    output[8] = CHARS[voltage.charAt(0) - 48][0]; // first digit of voltage
    output[9] = CHARS[voltage.charAt(0) - 48][1]; // first digit of voltage

    return output;
  }

  private byte[] output_pos(String input) {
    byte[] output = new byte[10];

    output[0] = (byte) (0b0000111100001111);

    output[2] = CHARS[input.charAt(3) - 48][0];
    output[3] = CHARS[input.charAt(3) - 48][1];

    output[4] = output[5] = output[6] = output[7] = (byte) 0b00000000;

    output[8] = CHARS[input.charAt(0) - 55][0];
    output[9] = CHARS[input.charAt(0) - 55][1];

    return output;
  }

  private byte[] output_obs(String input) {
    byte[] output = new byte[10];

    output[0] = (byte) (0b0000111100001111);

    for (int i = 0; i < input.length(); i++) {
      output[(2 * i) + 2] = CHARS[input.charAt(3 - i) - 55][0];
      output[(2 * i) + 3] = CHARS[input.charAt(3 - i) - 55][1];
    }
    //			output[2] = CHARS[input.charAt(3) - 55][0];
    //			output[3] = CHARS[input.charAt(3) - 55][1];
    //			output[4] = CHARS[input.charAt(2) - 55][0];
    //			output[5] = CHARS[input.charAt(2) - 55][1];
    //			output[6] = CHARS[input.charAt(1) - 55][0];
    //			output[7] = CHARS[input.charAt(1) - 55][1];
    //			output[8] = CHARS[input.charAt(0) - 55][0];
    //			output[9] = CHARS[input.charAt(0) - 55][1];

    return output;
  }

  private void board_task() {
    byte[] osc = new byte[1];
    byte[] blink = new byte[1];
    byte[] bright = new byte[1];
    osc[0] = (byte) 0x21;
    blink[0] = (byte) 0x81;
    bright[0] = (byte) 0xEF;

    _display_board.writeBulk(osc);
    _display_board.writeBulk(bright);
    _display_board.writeBulk(blink);

    STATE mode = STATE.Voltage;

    long refresh = 0;

    while (_do_things) {
      update();

      if ((System.currentTimeMillis() - refresh) > _timeout) {
        mode = STATE.Voltage;
      }

      boolean update_refresh = true;

      if (getAOnRisingEdge()) {
        if (mode == STATE.Position) {
          pos = (pos + 1) % _positions.length; // just display current position on first press
        }
        mode = STATE.Position;

        //				System.out.println("pos");
      } else if (getBOnRisingEdge()) {
        if (mode == STATE.Obstacle) {
          obs = (obs + 1) % _obstacles.length;
        }
        mode = STATE.Obstacle; // display current obstacle on first press

        //				System.out.println("obs");
      } else {
        update_refresh = false;
      }

      if (pos == 0) {
        obs = 0;
      }
      if (pos > 0 && obs == 0) {
        obs++;
      }

      if (update_refresh) {
        refresh = System.currentTimeMillis();
      }

      if (mode == STATE.Position) {
        _display_board.writeBulk(output_pos(_positions[pos]));
      } else if (mode == STATE.Obstacle) {
        _display_board.writeBulk(output_obs(_obstacles[obs]));
      } else {
        _display_board.writeBulk(output_voltage());
      }

      try {
        Thread.sleep(40); // wait a while because people can't read that
        // fast
      } catch (InterruptedException e) {
        e.printStackTrace();
      }
    }
  }

  // Thanks @Team 1493
  private static final byte[][] CHARS = {
    {(byte) 0b00111111, (byte) 0b00000000}, // 0; 0
    {(byte) 0b00000110, (byte) 0b00000000}, // 1; 1
    {(byte) 0b11011011, (byte) 0b00000000}, // 2; 2
    {(byte) 0b11001111, (byte) 0b00000000}, // 3; 3
    {(byte) 0b11100110, (byte) 0b00000000}, // 4; 4
    {(byte) 0b11101101, (byte) 0b00000000}, // 5; 5
    {(byte) 0b11111101, (byte) 0b00000000}, // 6; 6
    {(byte) 0b00000111, (byte) 0b00000000}, // 7; 7
    {(byte) 0b11111111, (byte) 0b00000000}, // 8; 8
    {(byte) 0b11101111, (byte) 0b00000000}, // 9; 9
    {(byte) 0b11110111, (byte) 0b00000000}, // A; 10
    {(byte) 0b10001111, (byte) 0b00010010}, // B; 11
    {(byte) 0b00111001, (byte) 0b00000000}, // C; 12
    {(byte) 0b00001111, (byte) 0b00010010}, // D; 13
    {(byte) 0b11111001, (byte) 0b00000000}, // E; 14
    {(byte) 0b11110001, (byte) 0b00000000}, // F; 15
    {(byte) 0b10111101, (byte) 0b00000000}, // G; 16
    {(byte) 0b11110110, (byte) 0b00000000}, // H; 17
    {(byte) 0b00001001, (byte) 0b00010010}, // I; 18
    {(byte) 0b00011110, (byte) 0b00000000}, // J; 19
    {(byte) 0b01110000, (byte) 0b00001100}, // K; 20
    {(byte) 0b00111000, (byte) 0b00000000}, // L; 21
    {(byte) 0b00110110, (byte) 0b00000101}, // M; 22
    {(byte) 0b00110110, (byte) 0b00001001}, // N; 23
    {(byte) 0b00111111, (byte) 0b00000000}, // O; 24
    {(byte) 0b11110011, (byte) 0b00000000}, // P; 25
    {(byte) 0b00111111, (byte) 0b00001000}, // Q; 26
    {(byte) 0b11110011, (byte) 0b00001000}, // R; 27
    {(byte) 0b10001101, (byte) 0b00000001}, // S; 28
    {(byte) 0b00000001, (byte) 0b00010010}, // T; 29
    {(byte) 0b00111110, (byte) 0b00000000}, // U; 30
    {(byte) 0b00110000, (byte) 0b00100100}, // V; 31
    {(byte) 0b00110110, (byte) 0b00101000}, // W; 32
    {(byte) 0b00000000, (byte) 0b00101101}, // X; 33
    {(byte) 0b00000000, (byte) 0b00010101}, // Y; 34
    {(byte) 0b00001001, (byte) 0b00100100}, // Z; 35
  };
}
예제 #21
0
 public void run() {
   m_ds.task();
 }
예제 #22
0
파일: OI.java 프로젝트: Team1065/FRC-2013
 public OI() {
   ds = DriverStation.getInstance().getEnhancedIO();
   shooterFeedbackEnableButton.whileHeld(new ShootingWithFeedback());
 }
예제 #23
0
  public OI() {
    enhancedIO = DriverStation.getInstance().getEnhancedIO();
    leftStick = new Joystick(RobotMap.LEFT_JOYSTICK_PORT);
    rightStick = new Joystick(RobotMap.RIGHT_JOYSTICK_PORT);

    shooterStick = new Joystick(RobotMap.SHOOTER_JOYSTICK_PORT);
    debugBox = new Joystick(RobotMap.DEBUG_BOX_PORT);

    distanceButton = DISTANCE_BUTTON_STOP;
    distanceInches = Flywheel.distances[Flywheel.STOP_INDEX];

    try {
      if (!Devmode.DEV_MODE) {
        enhancedIO.setDigitalConfig(
            BIT_1_CHANNEL, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
        enhancedIO.setDigitalConfig(
            BIT_2_CHANNEL, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
        enhancedIO.setDigitalConfig(
            BIT_3_CHANNEL, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
        enhancedIO.setDigitalConfig(
            ACQUIRER_IN_SWITCH_CHANNEL, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
        enhancedIO.setDigitalConfig(
            ACQUIRER_OUT_SWITCH_CHANNEL, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
        enhancedIO.setDigitalConfig(
            CONVEYOR_UP_SWITCH_CHANNEL, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
        enhancedIO.setDigitalConfig(
            CONVEYOR_DOWN_SWITCH_CHANNEL, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
        enhancedIO.setDigitalConfig(
            SHOOTER_BUTTON_CHANNEL, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
        enhancedIO.setDigitalConfig(
            STINGER_SWITCH_CHANNEL, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);

        enhancedIO.setDigitalConfig(
            DISTANCE_BUTTON_KEY_LIGHT_CHANNEL, DriverStationEnhancedIO.tDigitalConfig.kOutput);
        enhancedIO.setDigitalConfig(
            DISTANCE_BUTTON_FAR_LIGHT_CHANNEL, DriverStationEnhancedIO.tDigitalConfig.kOutput);
        enhancedIO.setDigitalConfig(
            DISTANCE_BUTTON_FENDER_2PT_LIGHT_CHANNEL,
            DriverStationEnhancedIO.tDigitalConfig.kOutput);
        enhancedIO.setDigitalConfig(
            DISTANCE_BUTTON_FENDER_NARROW_LIGHT_CHANNEL,
            DriverStationEnhancedIO.tDigitalConfig.kOutput);
        enhancedIO.setDigitalConfig(
            DISTANCE_BUTTON_REVERSE_LIGHT_CHANNEL, DriverStationEnhancedIO.tDigitalConfig.kOutput);
        enhancedIO.setDigitalConfig(
            DISTANCE_BUTTON_FENDER_LIGHT_CHANNEL, DriverStationEnhancedIO.tDigitalConfig.kOutput);
        enhancedIO.setDigitalConfig(
            DISTANCE_BUTTON_STOP_LIGHT_CHANNEL, DriverStationEnhancedIO.tDigitalConfig.kOutput);
      }
    } catch (EnhancedIOException e) {
    }

    if (!Devmode.DEV_MODE) {
      new JoystickButton(leftStick, 1).whenPressed(new DrivetrainSetGear(false));
      new JoystickButton(leftStick, 2).whenPressed(new DrivetrainSetGear(true));
      new JoystickButton(rightStick, 1).whenPressed(new TusksExtend());
      new JoystickButton(rightStick, 2).whenPressed(new TusksRetract());

      // OI box switches
      new InverseDigitalIOButton(ACQUIRER_IN_SWITCH_CHANNEL).whileHeld(new AcquirerAcquire());
      new InverseDigitalIOButton(ACQUIRER_OUT_SWITCH_CHANNEL).whileHeld(new AcquirerReverse());
      new InverseDigitalIOButton(CONVEYOR_UP_SWITCH_CHANNEL).whileHeld(new ConveyManual());
      new InverseDigitalIOButton(CONVEYOR_DOWN_SWITCH_CHANNEL).whileHeld(new ConveyReverseManual());
      new InverseDigitalIOButton(SHOOTER_BUTTON_CHANNEL).whileHeld(new ConveyAutomatic());
      new InverseDigitalIOButton(STINGER_SWITCH_CHANNEL).whileHeld(new StingerExtend());

      new JoystickButton(shooterStick, 1).whileHeld(new ConveyManual());
      new JoystickButton(shooterStick, 4).whenPressed(new FlywheelStop());
      new JoystickButton(shooterStick, 5).whileHeld(new AcquirerReverse());
      new JoystickButton(shooterStick, 6).whileHeld(new ConveyReverseManual());
      new JoystickButton(shooterStick, 7).whileHeld(new AcquirerAcquire());
      new JoystickButton(shooterStick, 8).whileHeld(new ConveyAutomatic());

      // see getDistanceButton()

      // Debug box switches
      new JoystickButton(debugBox, 1)
          .whileHeld(
              new FlywheelRun(Flywheel.distances[Flywheel.FENDER_INDEX], Flywheel.speedsTopHoop));
      new JoystickButton(debugBox, 2).whileHeld(new AcquirerAcquire());
      new JoystickButton(debugBox, 3).whileHeld(new ConveyAutomatic());
      new JoystickButton(debugBox, 4).whileHeld(new ConveyManual());
      // Debug box buttons
      new JoystickButton(debugBox, 5).whileHeld(new DrivetrainSetGear(false)); // low gear
      new JoystickButton(debugBox, 6).whileHeld(new DrivetrainSetGear(true)); // high gear
      new JoystickButton(debugBox, 9).whileHeld(new TusksExtend());
      new JoystickButton(debugBox, 10).whileHeld(new TusksRetract());
    }
  }
 /**
  * Constructor
  *
  * @param port USB Port on DriverStation
  */
 public XboxController(int port) {
   super();
   m_port = port;
   m_ds = DriverStation.getInstance();
 }
 /**
  * Get Value from an Axis
  *
  * @param axis Axis Number
  * @return Value from Axis (-1 to 1)
  */
 public double getRawAxis(int axis) {
   return m_ds.getStickAxis(m_port, axis);
 }
  @Override
  public void execute() {
    System.out.println("Starting execution");
    try {
      if (distance > 0) {
        while (DriverStation.getInstance().isEnabled()
            && TKOHardware.getDriveTalon(0).getSetpoint() < distance) {
          TKOHardware.getDriveTalon(0)
              .set(TKOHardware.getDriveTalon(0).getSetpoint() + incrementer);
          TKOHardware.getDriveTalon(2)
              .set(TKOHardware.getDriveTalon(2).getSetpoint() + incrementer);
          System.out.println(
              "Ncoder Left: "
                  + TKOHardware.getDriveTalon(0).getPosition()
                  + "\t Ncoder Rgith: "
                  + TKOHardware.getDriveTalon(2).getPosition()
                  + "\t Left Setpoint: "
                  + TKOHardware.getDriveTalon(0).getSetpoint());
          TKOLogger.getInstance()
              .addMessage(
                  "Ncoder Left: "
                      + TKOHardware.getDriveTalon(0).getPosition()
                      + "\t Ncoder Rgith: "
                      + TKOHardware.getDriveTalon(2).getPosition()
                      + "\t Left Setpoint: "
                      + TKOHardware.getDriveTalon(0).getSetpoint());
          Timer.delay(0.001);
        }
      } else {
        while (DriverStation.getInstance().isEnabled()
            && TKOHardware.getDriveTalon(0).getSetpoint() > distance) {
          TKOHardware.getDriveTalon(0)
              .set(TKOHardware.getDriveTalon(0).getSetpoint() - incrementer);
          TKOHardware.getDriveTalon(2)
              .set(TKOHardware.getDriveTalon(2).getSetpoint() - incrementer);
          System.out.println(
              "Ncoder Left: "
                  + TKOHardware.getDriveTalon(0).getPosition()
                  + "\t Ncoder Rgith: "
                  + TKOHardware.getDriveTalon(2).getPosition()
                  + "\t Left Setpoint: "
                  + TKOHardware.getDriveTalon(0).getSetpoint());
          TKOLogger.getInstance()
              .addMessage(
                  "Ncoder Left: "
                      + TKOHardware.getDriveTalon(0).getPosition()
                      + "\t Ncoder Rgith: "
                      + TKOHardware.getDriveTalon(2).getPosition()
                      + "\t Left Setpoint: "
                      + TKOHardware.getDriveTalon(0).getSetpoint());
          Timer.delay(0.001);
        }
      }

      TKOHardware.getDriveTalon(0).set(distance);
      TKOHardware.getDriveTalon(2).set(distance);

      while (Math.abs(TKOHardware.getLeftDrive().getPosition() - distance) > threshold
          && DriverStation.getInstance().isEnabled()) {
        // not close enough doe; actually gets stuck here
        TKOLogger.getInstance()
            .addMessage(
                "NOT CLOSE ENOUGH TO TARGET DIST: "
                    + (TKOHardware.getLeftDrive().getPosition() - distance));
        Timer.delay(0.001);
      }

    } catch (TKOException e1) {
      e1.printStackTrace();
      System.out.println("Error at another expected spot, I would assume....");
    }
    System.out.println("Done executing");
  }
예제 #27
0
/**
 * Sends data back to LabVIEW dashboard on computer. For camera targeting data, sensor feedback,
 * etc.
 *
 * @author Ziv
 */
public class Dashboards {

  private DriverStation ds = DriverStation.getInstance();
  private Dashboard lowDash = ds.getDashboardPackerLow();
  private static Dashboards instance = null;

  private Dashboards() {}

  public static Dashboards getInstance() {
    if (instance == null) {
      instance = new Dashboards();
    }
    return instance;
  }

  public void sendContinuousData() {
    // LabVIEW stuff.
    lowDash.addCluster(); // top
    lowDash.addCluster(); // target1 tracking
    lowDash.addInt((int) CommandBase.cam.tracker.getTarget1().rawBboxCornerX);
    lowDash.addInt((int) CommandBase.cam.tracker.getTarget1().rawBboxCornerY);
    lowDash.addInt((int) CommandBase.cam.tracker.getTarget1().rawBboxWidth);
    lowDash.addInt((int) CommandBase.cam.tracker.getTarget1().rawBboxHeight);
    lowDash.finalizeCluster();
    lowDash.addCluster(); // target2 tracking
    lowDash.addInt((int) CommandBase.cam.tracker.getTarget2().rawBboxCornerX);
    lowDash.addInt((int) CommandBase.cam.tracker.getTarget2().rawBboxCornerY);
    lowDash.addInt((int) CommandBase.cam.tracker.getTarget2().rawBboxWidth);
    lowDash.addInt((int) CommandBase.cam.tracker.getTarget2().rawBboxHeight);
    lowDash.finalizeCluster();
    lowDash.addCluster(); // target3 tracking
    lowDash.addInt((int) CommandBase.cam.tracker.getTarget3().rawBboxCornerX);
    lowDash.addInt((int) CommandBase.cam.tracker.getTarget3().rawBboxCornerY);
    lowDash.addInt((int) CommandBase.cam.tracker.getTarget3().rawBboxWidth);
    lowDash.addInt((int) CommandBase.cam.tracker.getTarget3().rawBboxHeight);
    lowDash.finalizeCluster();
    lowDash.addCluster(); // target4 tracking
    lowDash.addInt((int) CommandBase.cam.tracker.getTarget4().rawBboxCornerX);
    lowDash.addInt((int) CommandBase.cam.tracker.getTarget4().rawBboxCornerY);
    lowDash.addInt((int) CommandBase.cam.tracker.getTarget4().rawBboxWidth);
    lowDash.addInt((int) CommandBase.cam.tracker.getTarget4().rawBboxHeight);
    lowDash.finalizeCluster();
    lowDash.addDouble(CommandBase.cam.tracker.getBestTarget().centerX);
    // Shooter.
    lowDash.addDouble(CommandBase.shooter.getSetpoint());
    lowDash.addDouble(CommandBase.shooter.getRate());
    // Elevator.
    lowDash.addDouble(CommandBase.shooter.getPower());
    lowDash.finalizeCluster();
    lowDash.commit();
  }

  public void sendPeriodicData() {
    // SmartDashboard output stuff.
    SmartDashboard.putDouble("Force Sensor Value", CommandBase.elev.getPressure());
    SmartDashboard.putDouble("Yaw position", CommandBase.dt.yawGyro.getAngle());
    SmartDashboard.putDouble("Target1 X", CommandBase.cam.tracker.pidGet());
    SmartDashboard.putDouble("Cam servo", CommandBase.cam.getPos());
    SmartDashboard.putDouble("Shooter rate", CommandBase.shooter.getRate());
    SmartDashboard.putDouble("DT left", CommandBase.dt.getLeftPos());
    SmartDashboard.putDouble("DT right", CommandBase.dt.getRightPos());
    SmartDashboard.putDouble("Cam pos", CommandBase.cam.getPos());
    SmartDashboard.putDouble("Shooter power", CommandBase.shooter.getPower());
    SmartDashboard.putDouble("Throttle", CommandBase.oi.getDriveThrottle());
    SmartDashboard.putDouble("Wheel", CommandBase.oi.getDriveWheel());
    SmartDashboard.putDouble("Cam delta", CommandBase.oi.getCamDelta());
  }
}