Java 类edu.wpi.first.wpilibj.Ultrasonic 实例源码

/**
 * Creates the ProximitySensor.
 */
public ProximitySensor() {
  logger.trace("Initializing proximity sensor");

  ultrasonicLeft =
      new Ultrasonic(RobotMap.ULTRASONIC_LEFT_TRIGGER, RobotMap.ULTRASONIC_LEFT_ECHO);

  ultrasonicRight =
      new Ultrasonic(RobotMap.ULTRASONIC_RIGHT_TRIGGER, RobotMap.ULTRASONIC_RIGHT_ECHO);
  ultrasonicRight.setAutomaticMode(true);

  bufferLeft = new double[12];
  bufferCopyLeft = new double[12];
  bufferIndexLeft = 0;

  bufferRight = new double[12];
  bufferCopyRight = new double[12];
  bufferIndexRight = 0;

  Thread averagingThread = new Thread(this::averagingThread);
  averagingThread.setName("Ultrasonic Averaging Thread");
  averagingThread.setDaemon(true);
  averagingThread.start();
}

public Robot() {
    stick = new Joystick(0);
    driveLeftFront = new Victor(LEFT_FRONT_DRIVE);
    driveLeftRear = new Victor(LEFT_REAR_DRIVE);
    driveRightFront = new Victor(RIGHT_FRONT_DRIVE);
    driveRightRear = new Victor(RIGHT_REAR_DRIVE);
    enhancedDriveLeft = new Victor(ENHANCED_LEFT_DRIVE);
    enhancedDriveRight = new Victor(ENHANCED_RIGHT_DRIVE);
    gearBox = new DoubleSolenoid(GEAR_BOX_PART1, GEAR_BOX_PART2);
    climber1 = new Victor(CLIMBER_PART1);
    climber2 = new Victor(CLIMBER_PART2);
    vexSensorBackLeft = new Ultrasonic(0, 1);
    vexSensorBackRight = new Ultrasonic(2, 3);
    vexSensorFrontLeft = new Ultrasonic(4, 5);
    vexSensorFrontRight = new Ultrasonic(6, 7);

    Skylar = new RobotDrive(driveLeftFront, driveLeftRear, driveRightFront, driveRightRear);
    OptimusPrime = new RobotDrive(enhancedDriveLeft, enhancedDriveRight);
}

private Drive()
{
    drive = new RobotDrive(new Talon(1),new Talon(2),new Talon(3),new Talon(4));
    drive.setSafetyEnabled(false);


    e1 = new Encoder(RobotMap.ENCODER_1_A, RobotMap.ENCODER_1_B, false, CounterBase.EncodingType.k4X);
    e2 = new Encoder(RobotMap.ENCODER_2_A, RobotMap.ENCODER_2_B, false, CounterBase.EncodingType.k4X);

    //A calculated constant to convert from encoder ticks to feet on 4 inch diameter wheels
    e1.setDistancePerPulse(0.0349065850388889/12);
    e2.setDistancePerPulse(0.0349065850388889/12);
    startEncoders();

    drive.setInvertedMotor(RobotDrive.MotorType.kFrontLeft, true);      
    drive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, true);

    sonic = new Ultrasonic(RobotMap.ULTRASONIC_A, RobotMap.ULTRASONIC_B);
    sonic.setAutomaticMode(true);
    sonic.setEnabled(true);


    shifter = new DoubleSolenoid(RobotMap.SHIFTER_ENGAGE, RobotMap.SHIFTER_DISENGAGE);
}

/**
 * Constructor
 * 
 * @param goalHeight
 * @param ultra
 * @param targetX
 */
public AngleFinder(double goalHeight, Ultrasonic ultra, int targetX) {
    this.goalHeight = goalHeight;
    this.targetX = targetX;
    this.targetY = 0;
    this.ultra = ultra;
    ultra.setAutomaticMode(true);
}

/**
 * This function is run when the robot is first started up and should be
 * used for any initialization code.
 */
public void robotInit()
{
    // PWM's
    mTestMotor1 = new Talon(0);
    mTestMotor2 = new Jaguar(1);
    mServo = new Servo(2);

    // Digital IO
    mDigitalOutput = new DigitalOutput(0);
    mRightEncoder = new Encoder(4, 5);
    mLeftEncoder = new Encoder(1, 2);
    mUltrasonic = new Ultrasonic(7, 6);

    // Analog IO
    mAnalogGryo = new AnalogGyro(0);
    mPotentiometer = new AnalogPotentiometer(1);

    // Solenoid
    mSolenoid = new Solenoid(0);

    // Relay
    mRelay = new Relay(0);

    // Joysticks
    mJoystick1 = new Joystick(0);
    mJoystick2 = new XboxController(1);

    // SPI
    mSpiGryo = new ADXRS450_Gyro();

    // Utilities
    mTimer = new Timer();
    mPDP = new PowerDistributionPanel();

    Preferences.getInstance().putDouble("Motor One Speed", .5);
}

public static void initialize()
{
    if (!initialized) {
        //ultrasonicDevice = new AnalogInput(ANALOG_CHANNEL_ID);
        ultrasonicDevice = new Ultrasonic(TRIGGER_CHANNEL_ID,ECHO_CHANNEL_ID);
        ultrasonicDevice.setAutomaticMode(true);

        initialized = true;
    }
}

public static void initialize()
{
    if (!initialized) {
        ultrasonicDevice = new Ultrasonic(HardwareIDs.TRIGGER_CHANNEL_ID,HardwareIDs.ECHO_CHANNEL_ID);
        ultrasonicDevice.setAutomaticMode(true);

        initialized = true;
    }
}

public static void initialize()
{
    if (!initialized) {
        ultrasonicDevice = new Ultrasonic(HardwareIDs.TRIGGER_CHANNEL_ID,HardwareIDs.ECHO_CHANNEL_ID);
        ultrasonicDevice.setAutomaticMode(true);

        initialized = true;
    }
}

public static void initialize()
{
    if (!initialized) {
        ultrasonicDevice = new Ultrasonic(HardwareIDs.TRIGGER_CHANNEL_ID,HardwareIDs.ECHO_CHANNEL_ID);
        ultrasonicDevice.setAutomaticMode(true);

        initialized = true;
    }
}

public void robotInit(){
    driveStick= new JoyStickCustom(1, DEADZONE);
    secondStick=new JoyStickCustom(2, DEADZONE);

    frontLeft= new Talon(1);
    rearLeft= new Talon(2);
    frontRight= new Talon(3);
    rearRight= new Talon(4);

    timer=new Timer();
    timer.start();

    armM = new Victor(7);
    rollers =new Victor(6);

    mainDrive=new RobotDrive(frontLeft,rearLeft,frontRight,rearRight);

    mainDrive.setInvertedMotor(RobotDrive.MotorType.kFrontRight,true);
    mainDrive.setInvertedMotor(RobotDrive.MotorType.kRearRight, true);

    armP = new AnalogPotentiometer(1);
    distance= new AnalogChannel(2);
    ultra=new Ultrasonic(6,7);
    ultra.setAutomaticMode(true);

    compress=new Compressor(5,1);

    handJoint=new Pneumatics(3,4);

    //ultra = new Ultrasonic(6,5);
    //ultra.setAutomaticMode(true);

    winch = new Winch(secondStick);

}

public DistanceReader(){
    sensor = new Ultrasonic(RobotMap.PING_CHANNEL, RobotMap.ECHO_CHANNEL);
    sensor.setEnabled(true);
    sensor.setAutomaticMode(true);
}

public void init() {

    motorLeft = new Talon(1);
    motorRight = new Talon(2);
    System.out.println("[INFO] TALON[1&2]: Created!");
    elToro1 = new Talon(3);
    elToro2 = new Talon(4);
    System.out.println("[INFO] TALON[3&4]: Created!");
    robotdrive = new RobotDriveSteering(motorLeft, motorRight);
    robotdrive.setInvertedMotor(RobotDriveSteering.MotorType.kRearLeft, true);
    robotdrive.setInvertedMotor(RobotDriveSteering.MotorType.kRearRight, true);

    compressor = new Compressor(1, 1); // presureSwitchDigitalInput, RelayOut
    compressor.start();

    wormgear = new Relay(2);
    spreader = new Relay(3);
    System.out.println("[INFO] RELAY[1&2&3]: Created!");

    joystick = new Joystick(1);
    //joystick2 = new Joystick(2);
    System.out.println("[INFO] JOYSTICK[1&2]: Created!");

    cock = new Solenoid(1);
    uncock = new Solenoid(2);
    lock = new Solenoid(3);
    fire = new Solenoid(4);

    System.out.println("[INFO] Digital I/O: Enabled.");

    sonic = new Ultrasonic(4, 2);
    sonic.setEnabled(true);
    sonic2 = new AnalogChannel(3);
    pot = new AnalogPotentiometer(2, 10);

    autonomousTimer = new Timer();
    t = new Timer();

    LCD = DriverStationLCD.getInstance();
    ds = DriverStation.getInstance();

    System.out.println("[INFO] Robot Initialized");
}

public void robotInit(){
        Subsystems.robot = this;
        Subsystems.imu = new IMU(GYRO_CHANNEL);
        Subsystems.imu.reset();

        Subsystems.driveTrain = new DriveTrain(DRIVE_MOTOR_FRONTLEFT, DRIVE_MOTOR_BACKLEFT
                , DRIVE_MOTOR_FRONTRIGHT, DRIVE_MOTOR_BACKRIGHT, ContinuousMotor.SpeedControllerType.TALON);
//      Subsystems.driveTrain = new DriveTrain(1,2,ContinuousMotor.SpeedControllerType.JAGUAR);

        Subsystems.compressor = new Compressor(COMPRESSOR_RELAY, COMPRESSOR_PRESSURESWITCH);

        Subsystems.nets = new Nets
                ( new LimitedMotor(NET_MOTOR_LEFT, LimitedMotor.SpeedControllerType.VICTOR, NET_SWITCH_LEFT, NET_PROX_LEFT)
                , new LimitedMotor(NET_MOTOR_RIGHT, LimitedMotor.SpeedControllerType.VICTOR, NET_SWITCH_RIGHT, NET_PROX_RIGHT));

        Subsystems.arm = new RollerArm(SOLENOID_LEFT_EXTEND,SOLENOID_LEFT_RETRACT,SOLENOID_RIGHT_EXTEND,SOLENOID_RIGHT_RETRACT);
        Subsystems.roller = new Roller(ROLLER_MOTOR, ContinuousMotor.SpeedControllerType.VICTOR);
        Subsystems.ranger = new Ranger(RANGER_CHANNEL);
        Subsystems.pid = new PIDController(0.5,0,0,Subsystems.ranger,new PIDDriveInterface());
        Subsystems.turnPID = new PIDController(0.003,0,0.007,Subsystems.imu, new PIDTurnInterface());

        Subsystems.leftUltrasonicSensor = new Ultrasonic(ULTRASONIC_LEFT_PING, ULTRASONIC_LEFT_ECHO, Ultrasonic.Unit.kInches);
        Subsystems.rightUltrasonicSensor = new Ultrasonic(ULTRASONIC_RIGHT_PING, ULTRASONIC_RIGHT_EHCO, Ultrasonic.Unit.kInches);

        Subsystems.leftUltrasonicSensor.setAutomaticMode(true);
        Subsystems.rightUltrasonicSensor.setAutomaticMode(true);

        //Subsystems.compressor.init();
        OperatorInterface.init();

        smartDashboardInit();

//      Subsystems.compressor.activate();
//      long then = System.currentTimeMillis();
//      while(System.currentTimeMillis()-then<100*1000){
//          Subsystems.compressor.activate();
//      }
//      Subsystems.compressor.deactive();
        Subsystems.compressor.init();

        //TODO Have drivers refine these values and make them constants in DriveTrain.
        SmartDashboard.putNumber("Range 1", 0.4);
        SmartDashboard.putNumber("Range 2", 0.8);
        SmartDashboard.putNumber("Range 3", 1.0);

        SmartDashboard.putNumber("Max Delta 1", 1.0);
        SmartDashboard.putNumber("Max Delta 2", 0.1);
        SmartDashboard.putNumber("Max Delta 3", 0.01);
//      Scheduler.getInstance().add(new SetState(Subsystems.nets.leftNet, State.CLOSED, Nets.CLOSE_SPEED));

//      Scheduler.getInstance().add(new AddCommandAfterDelay
//              (new SetState(Subsystems.nets.rightNet, State.CLOSED, Nets.OPEN_SPEED),0.5));
    }

/**
 * Constructor
 * 
 * @param goalHeight
 * @param ultra
 * @param targetX
 * @param targetY
 */
public AngleFinder(double goalHeight, Ultrasonic ultra, int targetX, int targetY) {
    this.goalHeight = goalHeight;
    this.targetX = targetX;
    this.targetY = targetY;
    this.ultra = ultra;
    ultra.setAutomaticMode(true);
}

/**
 * Used to get ultra
 * 
 * @return ultra
 */
public Ultrasonic getUltra() {
    return ultra;
}

/**
 * Used to set ultra
 * 
 * @param ultra
 */    
public void setUltra(Ultrasonic ultra) {
    this.ultra = ultra;
}

/**
 * Create a digital ultrasonic {@link DistanceSensor} for an {@link Ultrasonic} sensor that uses the specified channels.
 *
 * @param pingChannel the digital output channel to use for sending pings
 * @param echoChannel the digital input channel to use for receiving echo responses
 * @return a {@link DistanceSensor} linked to the specified channels
 */
public static DistanceSensor digitalUltrasonic(int pingChannel, int echoChannel) {
    Ultrasonic ultrasonic = new Ultrasonic(pingChannel, echoChannel);
    ultrasonic.setAutomaticMode(true);
    return DistanceSensor.create(ultrasonic::getRangeInches);
}

/**
 * Constructs the module with the ultrasonic object underneath this class to
 * call methods from.
 *
 * @param ultrasonic the composing instance which will return values
 */
protected UltrasonicRangeFinderModule(Ultrasonic ultrasonic) {
    this.ultrasonic = ultrasonic;
}

/**
 * Constructs the range finder using the ping and echo channel. Ping is used
 * to send signals and echo is used to receive the reflected signal.
 *
 * @param pingChannel channel on digital sidecar for pinging
 * @param echoChannel channel on digital sidecar for receiving
 */
public UltrasonicRangeFinderModule(int pingChannel, int echoChannel) {
    this(new Ultrasonic(pingChannel, echoChannel));
}