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

ShooterControl(Encoder encoder, SpeedController pullBackSpeedController, double speedControllerMaxRpm,
               DigitalInput limitSwitch, DoubleSolenoid gearControl, Servo latchServo,
               Relay angleControl)
{
    m_latchReleaseServo = latchServo;
    m_currentState = SHOOTER_CONTROL_STATE_WAIT;
    m_encoder = encoder;
    m_pullBackSpeedController = pullBackSpeedController;
    m_angleControl = angleControl;
    m_speedControllerMaxRpm = speedControllerMaxRpm;
    m_limitSwitch = limitSwitch;
    m_pullBackEncoderRpm = new EncoderRPM();
    m_pullBackEncoderRpm.Init(m_pullBackSpeedController, m_encoder, (-1)*m_speedControllerMaxRpm, m_speedControllerMaxRpm, 0.05, 100, m_limitSwitch);
    m_releaseFromMidptEncoderRpm = new EncoderRPM();
    m_releaseFromMidptEncoderRpm.Init(m_pullBackSpeedController, m_encoder,(-1)*m_speedControllerMaxRpm/4,m_speedControllerMaxRpm,0.05, 3);
    m_gearControl = gearControl;
    m_latchReleased = false;
    m_gearReleased = false;
    m_isPulledBack = false;
}

/**
 * Set the angular position of a servo.
 * 
 * @param angle in degrees (0 - 180) to set the servo to.
 * @param invert clockwise or counterclockwise rotation. Clockwise if false.
 * @param servo the Servo object to set the angle of.
 * @param minAngle the minimum allowable angle for the servo to travel to,
 *   not to be lower than 0.0.
 * @param maxAngle the maximum allowable angle for the servo to travel to,
 *   not to exceed 170.0.
 */
private void setAngle(double angle, boolean invert, Servo servo,
        double minAngle, double maxAngle) {

    //The Servo class has hard coded ranges of travel.
    if(minAngle < 0) {
        minAngle = 0.0;
    }

    if(maxAngle > 170.0){
        maxAngle = 170.0;
    }

    //Get angle in range
    if(angle > maxAngle) {
        angle = maxAngle;
    } else if(angle < minAngle) {
        angle = minAngle;
    }

    if(invert) {
        angle = maxAngle - angle;
    }

    servo.setAngle(angle);
}

/**
 * Creates the Stirrer subsystem.
 */
public Stirrer() {
  logger.info("Initialize");

  // HSR-1425CR
  stirrerServo = new Servo(RobotMap.STIRRER_SERVO_CHANNEL);
  // values for the servo we have. http://hitecrcd.com/faqs/servos/general-servos
  stirrerServo.setBounds(2.4, 0, 0, 0, 0.9);
  stirrerServo.setPeriodMultiplier(PeriodMultiplier.k4X);
  setStirrer(RobotMap.STIRRER_SERVO_VALUE_STOP);
}

/**
 * Creates the GearManipulator, and sets servo to closed position.
 */
public GearManipulator() {
  logger.info("Initialize");

  gearServo = new Servo(RobotMap.GEAR_SERVO_CHANNEL);
  gearServo.setBounds(2.4, 0, 0, 0, 0.8);
  gearServo.setPeriodMultiplier(PeriodMultiplier.k4X);

  setPosition(Position.CLOSED);

  lsSpokeDown = new AnalogInput(RobotMap.AI_LS_SPOKE_DOWN);
  lsWedgeDown = new AnalogInput(RobotMap.AI_LS_WEDGE_DOWN);

  pressurePlate = new AnalogInput(RobotMap.AI_PRESSURE_PLATE);
}

public static void initialize(){
    climbingWinch = new CANTalon(8); //this value is guessed as of 3/21
    release = new Servo(2); //this value is guessed as of 3/21
    motorLatch = new Servo(3); //this value is guessed as of 3/21
    climbingWinch.changeControlMode(CANTalon.TalonControlMode.PercentVbus);
    climbingWinch.enableBrakeMode(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);
}

/**
     * Instantiate latch and sensor
     */
    public Climber() {
        armLatch = new Servo(RobotMap.CLIMBER_LATCH);
        dart = new Talon(4);
//      atBottom = new DigitalInput(RobotMap.ClimberBottomSensor);
        dartLimit = new DigitalInput(RobotMap.CLIMBER_DART_LIMIT);
        winchBanner = new DigitalInput(RobotMap.CLIMBER_BANNER);
    }

/**
 * Code for driving robot
 */
public Drive() {
    leftDrive = new Talon(RobotMap.LEFT_DRIVE_TALON);
    rightDrive = new Talon(RobotMap.RIGHT_DRIVE_TALON);
    PTOMotor = new Servo(RobotMap.DRIVE_PTO);
    gyro = new AnalogGyro(0);
}

public CameraSubsystem(Servo _horizontal, Servo _vertical, Relay _lights) {
    this._horizontal = _horizontal;
    this._vertical = _vertical;
    this._lights = _lights;

    this.setDataKey(CameraSubsystemDataKey.HORIZONTAL_ROTATION_DEGREES, 90.0d);
    this.setDataKey(CameraSubsystemDataKey.VERTICAL_ROTATION_DEGREES, 0.0d);
    this.setDataKey(CameraSubsystemDataKey.LIGHTS, false);
}

public TurtlePWMServo(int channel, double startingAngle, double degreesPerSec, double maxMovement) {
    servo = new Servo(channel);
    currentAngle = startingAngle;
    degreesPerSecond = degreesPerSec;
    max = maxMovement;
    timer = new Timer();
    servo.setAngle(currentAngle);
    timer.start();
}

public void initialize() 
{
    rearLeftMotor = new Jaguar(0);
    frontLeftMotor = new Jaguar(1);
    liftMotor = new Jaguar(2);
    liftMotor2 = new Jaguar(3);
    liftEncoder = new Encoder(6, 7, false, EncodingType.k4X);

    drivetrain = new RobotDrive(rearLeftMotor, frontLeftMotor); 

    drivetrain.setInvertedMotor(MotorType.kFrontLeft, true);
    drivetrain.setInvertedMotor(MotorType.kFrontRight, true);

    halsensor = new DigitalInput(0);

    horizontalCamera = new Servo(8);
    verticalCamera = new Servo(9);

    solenoid = new DoubleSolenoid(0,1);

    gyro = new Gyro(1);
    accelerometer = new BuiltInAccelerometer();

    liftEncoder.reset();

    RobotHardwareWoodbot hardware = (RobotHardwareWoodbot)Robot.bot;

    LiveWindow.addActuator("Drive Train", "Front Left Motor", (Jaguar)hardware.frontLeftMotor);
    LiveWindow.addActuator("Drive Train", "Back Left Motor", (Jaguar)hardware.rearLeftMotor);
    //LiveWindow.addActuator("Drive Train", "Front Right Motor", (Jaguar)hardware.frontRightMotor);
    //LiveWindow.addActuator("Drive Train", "Back Right Motor", (Jaguar)hardware.rearRightMotor);
}

@Override
public void initialize()

{
    //PWM
    liftMotor = new Victor(0); //2);
    leftMotors = new Victor(1);
    rightMotors = new Victor(2); //0);
    armMotors = new Victor(3);
    transmission = new Servo(7);

    //CAN
    armSolenoid = new DoubleSolenoid(4,5);

    //DIO
    liftEncoder = new Encoder(0, 1, false, EncodingType.k4X);
    liftBottomLimit = new DigitalInput(2);
    liftTopLimit = new DigitalInput(3);
    backupLiftBottomLimit = new DigitalInput(5);

    switch1 = new DigitalInput(9);
    switch2 = new DigitalInput(8);

    //ANALOG
    gyro = new Gyro(0);

    //roboRio
    accelerometer = new BuiltInAccelerometer();

    //Stuff
    drivetrain = new RobotDrive(leftMotors, rightMotors);

    liftSpeedRatio = 1; //Half power default
    liftGear = 1;
    driverSpeedRatio = 1;
    debounceComp = 0;

    drivetrain.setInvertedMotor(MotorType.kRearLeft, true);
    drivetrain.setInvertedMotor(MotorType.kRearRight, true);
}

@Override
public void initialize()
{
    //PWM
    liftMotor = new Talon(0); //2);
    leftMotors = new Talon(1);
    rightMotors = new Talon(2); //0);
    armMotors = new Talon(3);
    transmission = new Servo(7);

    //CAN
    armSolenoid = new DoubleSolenoid(4,5);

    //DIO
    /*liftEncoder = new Encoder(0, 1, false, EncodingType.k4X);
    liftBottomLimit = new DigitalInput(2);
    liftTopLimit = new DigitalInput(3);
    backupLiftBottomLimit = new DigitalInput(4);

    switch1 = new DigitalInput(9);
    switch2 = new DigitalInput(8);*/

    //ANALOG
    gyro = new Gyro(0);

    //roboRio
    accelerometer = new BuiltInAccelerometer();

    //Stuff
    drivetrain = new RobotDrive(leftMotors, rightMotors);

    liftSpeedRatio = 1; //Half power default
    liftGear = 1;
    driverSpeedRatio = 1;
    debounceComp = 0;

    drivetrain.setInvertedMotor(MotorType.kRearLeft, true);
    drivetrain.setInvertedMotor(MotorType.kRearRight, true);
}

/**
 * Default constructor for vision target. TCP Listens on Port 1111;
 */
private Vision()
{
    cam = new TCPCameraSensor(1111, 200);
    vision_servo = new Servo(RobotMap.visionServo.getInt());
    //initialize data variable
    dataReceived = new String[cam.getMessageLength()];

    for(int i = 0; i < cam.getMessageLength(); i++)
        dataReceived[i] = "0";

    LeftOrRightHot = 0;

    cam.start();
}

/**
 * A private constructor to prevent multiple instances from being created.
 */
private ServoBallTapper() {
    leftTapper = new Servo(RobotMap.leftTapperServo.getInt());
    rightTapper = new Servo(RobotMap.rightTapperServo.getInt());

    setLeftAngle(0);
    setRightAngle(0);
}

/**
 * Get the angle of a servo.
 * @param inverted specifies if positive angles should be
 *   clockwise (false) or counterclockwise (true).
 * @param servo The Servo object to get the angle of. 
 * @param maxAngle The maximum angle the servo can travel.
 * @return the angle of the zero (degrees).
 */
private double getAngle(boolean inverted, Servo servo, double maxAngle) {
    double angle = servo.getAngle();

    if(inverted) {
        angle = -angle + maxAngle;
    }

    return angle;
}

public CameraUnit()
{
    camera = AxisCamera.getInstance(ReboundRumble.CAMERA_IP);
    camera.writeResolution(AxisCamera.ResolutionT.k320x240);
    camera.writeExposurePriority(AxisCamera.ExposurePriorityT.imageQuality);
    camera.writeExposureControl(AxisCamera.ExposureT.hold);
    camera.writeWhiteBalance(AxisCamera.WhiteBalanceT.fixedIndoor);
    cc = new CriteriaCollection();
    cc.addCriteria(MeasurementType.IMAQ_MT_BOUNDING_RECT_WIDTH, 15, 400, false);
    cc.addCriteria(MeasurementType.IMAQ_MT_BOUNDING_RECT_HEIGHT, 15, 400, false);
    cameraPan = new Servo(1, 6);
    cameraTilt = new Servo(1, 7);
}

public Climber(int leftM, int rightM, int leftSecondaryM, int rightSecondaryM, int leftS, int rightS, int gyro)
{
    this.leftM = new Jaguar(leftM);
    this.rightM = new Jaguar(rightM);
    this.leftSecondaryM = new Victor(leftSecondaryM);
    this.rightSecondaryM = new Victor(rightSecondaryM);
    this.leftS = new Servo(leftS);
    this.rightS = new Servo(rightS);
    this.gyro = new Gyro(gyro);
    this.gyro.setSensitivity(.007);
    System.out.println("Sensitivity set");
    this.gyro.reset();

    Log.v(TAG, "Climber subsystem instantiated.");
}

public AimingSystem()
{
    camera = AxisCamera.getInstance(Parameters.cameraIP);
    camera.writeResolution(AxisCamera.ResolutionT.k640x480);
    camera.writeExposurePriority(AxisCamera.ExposurePriorityT.imageQuality);
    camera.writeExposureControl(AxisCamera.ExposureT.hold);
    camera.writeWhiteBalance(AxisCamera.WhiteBalanceT.fixedIndoor);
    cc = new CriteriaCollection();
    cc.addCriteria(NIVision.MeasurementType.IMAQ_MT_AREA, 500, 65535, false);
    ultrasonicSensor = new Ultrasonic(Parameters.UltrasonicAnalogChannel);
    servo = new Servo(Parameters.cameraServoPWMChannel);
    setShootPosition();
}

public Drive(JoystickControl controller)
{
    leftMotor = new Motor(JAGUAR_LEFT_ID, JAGUAR_LEFT_INVERTED);
    rightMotor = new Motor(JAGUAR_RIGHT_ID, JAGUAR_RIGHT_INVERTED);

    leftEncoderInput = new DigitalInput(ENCODER_DIGITAL_SIDECAR_CHANNEL, ENCODER_LEFT_CHANNEL);
    rightEncoderInput = new DigitalInput(ENCODER_DIGITAL_SIDECAR_CHANNEL, ENCODER_RIGHT_CHANNEL);

    leftEncoder = new Encoder(leftEncoderInput, leftEncoderInput, false, EncodingType.k1X);
    rightEncoder = new Encoder(rightEncoderInput, rightEncoderInput, false, EncodingType.k1X);

    raiseServo = new Servo(5);

    this.controller = controller;
}

public Gear() {
    left = new Servo(Constants.GEAR_LEFT);
    right = new Servo(Constants.GEAR_RIGHT);

    isGearOut = false;
}

@Override
protected String header() {
  Servo servo = servoSet.getServo();
  String id = servo != null ? String.valueOf(servo.getChannel()) : "";
  return Messages.boldGreen("\nServo: " + id + "\n");
}

public Servo getServo() {
  return servo;
}

public void setServo(Servo servo) {
  clearSelected();
  this.servo = servo;
}

public ServoItem(Servo servo, String description) {
  super(TYPE, description, MEASURES);
  this.servo = servo;
}

public ServoItem(Servo servo) {
  this(servo, "Servo " + servo.getChannel());
}

public static void initialize() {
    if (initialized)
        return;

    // reset trigger init time
    initTriggerTime = Utility.getFPGATime();        

    cameraLedRelay = new Relay(HardwareIDs.CAMERA_LED_RELAY_CHANNEL,Relay.Direction.kForward);
    cameraLedRelay.set(Relay.Value.kOff);

    positionServo = new Servo(HardwareIDs.CAMERA_SERVO_PWM_ID);

    gamepad = new Joystick(HardwareIDs.DRIVER_CONTROL_ID);

    ledState = false;

    initialized = true;
}

public static void initialize() {
    if (initialized)
        return;

    // reset trigger init time
    initTriggerTime = Utility.getFPGATime();        

    cameraLedRelay = new Relay(HardwareIDs.CAMERA_LED_RELAY_CHANNEL,Relay.Direction.kForward);
    cameraLedRelay.set(Relay.Value.kOff);

    positionServo = new Servo(HardwareIDs.CAMERA_SERVO_PWM_ID);

    gamepad = new Joystick(HardwareIDs.DRIVER_CONTROL_ID);

    ledState = false;

    initialized = true;
}

public WsServo(String name, int channel) {
    this.position = new DoubleSubject(name);
    this.servo = new Servo(channel);
    angleSet = false;
}

CameraMount() {
    tilt = new Servo(HardwareDefines.DIGITAL_MODULE_PORT, 
                     HardwareDefines.TILT_SERVO_CHANNEL);
    rotate = new Servo(HardwareDefines.DIGITAL_MODULE_PORT,
                       HardwareDefines.ROT_SERVO_CHANNEL);
}

public WsServo(String name, int channel) {
    this.position = new DoubleSubject(name);
    this.servo = new Servo(channel);
    angleSet = false;
}

/**
 * This function is run when the robot is first started up and should be
 * used for any initialization code.
 */
public void robotInit() {
    // Initializes the Talon motor controllers for driving
    frontLeftTalon = new Talon(1);
    backLeftTalon = new Talon(2);
    frontRightTalon = new Talon(3);
    backRightTalon = new Talon(4);

    // Initializes the controller for the four driving motors and reverses two of them
    driveControl = new RobotDrive(frontLeftTalon, backLeftTalon, frontRightTalon, backRightTalon);
    driveControl.setInvertedMotor(RobotDrive.MotorType.kFrontRight, true);
    driveControl.setInvertedMotor(RobotDrive.MotorType.kRearRight, true);

    // Initializes the rest of the jaguars and talons for various functions
    frisbeeAcceleratorTalon = new Talon(5);
    frisbeePusherJaguar = new Jaguar(6);
    leftHookTalon = new Talon(7);
    rightHookTalon = new Talon(8);
    shooterTiltTalon = new Talon(9);

    // Initialize joysticks
    driverJoystick = new Joystick(1);
    secondJoystick = new Joystick(2);

    // Initialize the limit switches for the hooks
    hookLimitLeft = new DigitalInput(1);
    hookLimitRight = new DigitalInput(2);

    // Initialize toggle switch for the hooks
    runningHooks = false;

    // Initialize the limit switches for the frisbee pusher
    pusherLimitFront = new DigitalInput(3);
    pusherLimitBack = new DigitalInput(4);

    // Initialize the limit switch on the shooter tilt
    tiltLimitBottom = new DigitalInput(5);
    tiltLimitTop = new DigitalInput(6);

    // Initialize autonomous timer
    frisbeeAutoTimer = new Timer();

    // Initialize encoder
    encoder = new AnalogChannel(3);

    gyro = new Gyro(1);

    camServoX = new Servo(2, 1);
    camServoY = new Servo(2, 2);

    camServoXAngle = 0;
    camServoYAngle = 0;


}

protected LeftLock() {
    lock = new Servo(Constants.P_LEFT_LOCK_CAR, Constants.P_LEFT_LOCK_CHAN);
}

private RightLock(){
    this.lock = new Servo(Constants.P_RIGHT_LOCK_CAR, Constants.P_RIGHT_LOCK_CHAN);       
}

public static Servo getServo(){
    return new SlowServo(1, 1);
}

public WsServo(String name, int channel) {
    this.position = new DoubleSubject(name);
    this.servo = new Servo(channel);
    angleSet = false;
}