public static void teleopPeriodic() { double currentTime = Utility.getFPGATime(); // if not enough time has passed, no polling allowed! if ((currentTime - initTriggerTime) < TRIGGER_CYCLE_WAIT_US) return; if (gamepad.getRawButton(FIRE_BUTTON)) { fire(); // reset trigger init time initTriggerTime = Utility.getFPGATime(); } if (gamepad.getRawButton(HOLD_BUTTON)) { hold(); // reset trigger init time initTriggerTime = Utility.getFPGATime(); } }
private static void checkShooterControls() { // fire controls - using a timer to debounce double currentTime = Utility.getFPGATime(); // if not enough time has passed, no polling allowed! if ((currentTime - initTriggerTime) < TRIGGER_CYCLE_WAIT_US) return; // shooter commands if (gamepad.getRawButton(HardwareIDs.FIRE_HIGH_BUTTON)) setShooterStrength(MOTOR_HIGH); if (gamepad.getRawButton(HardwareIDs.FIRE_MEDIUM_BUTTON)) setShooterStrength(MOTOR_MEDIUM); if (gamepad.getRawButton(HardwareIDs.FIRE_LOW_BUTTON)) setShooterStrength(MOTOR_LOW); if (gamepad.getRawButton(HardwareIDs.HOLD_BUTTON)) setShooterStrength(MOTOR_OFF); }
public static void teleopInit() { gearTrayOn(); //collectorOff(); // keep collector off until gamepad control pressed resetMotors(); // spawn a wait thread to turn relays back off after a number of seconds /* new Thread() { public void run() { try { Thread.sleep(3000); // wait a number of sec before starting to feed gearTrayOff(); // turn relays off } catch (Exception e) { e.printStackTrace(); } } }.start(); */ initTriggerTime = Utility.getFPGATime(); }
public boolean isTriggered() { double gyroAngle = getGyroAngle(); if (Math.abs(gyroAngle - targetAngleDeg) > errorDeg) { // outside error range... // reset timer and return false startTimeUs = Utility.getFPGATime(); return false; } long currentTimeUs = Utility.getFPGATime(); double delta = (currentTimeUs - startTimeUs)/1e6; //System.out.println("delta = " + delta + " duration = " + durationSec); if (delta < durationSec) { // within error range, but not for enough time return false; } System.out.println("ClosedLoopAngleEvent triggered!"); return true; }
public void updateMotorRPM() { long currentTimeMicroSeconds = Utility.getFPGATime(); long deltaTimeMicroSeconds = currentTimeMicroSeconds - m_lastTimeMicroSeconds; m_lastTimeMicroSeconds = currentTimeMicroSeconds; // RPM = counts/microsec * 1000000 microsec/sec * 60 sec/min * 1/counts_per_rev double replacedMotorRPM = m_motorRPM[m_rpmIndex]; double currentRPM = (m_encoder.get() / (double)deltaTimeMicroSeconds) * 60000000.0 / m_pulsesPerRotation; if (currentRPM > MAX_RPM) { currentRPM = MAX_RPM; } m_motorRPM[m_rpmIndex] = currentRPM; m_encoder.reset(); m_averagedMotorRPM += (m_motorRPM[m_rpmIndex] - replacedMotorRPM) / m_numAveragedCycles; m_rpmIndex++; if (m_rpmIndex == m_numAveragedCycles) { m_rpmIndex = 0; } }
protected boolean isFinished() { log("Gyro Angle: "+driveTrain.getGyro().getAngle() + " bearing: "+bearing); //if by time if (driveTime > 0 && Utility.getFPGATime() >= startTime + driveTime) { driveTrain.tankDrive(0, 0); state = FINISHED; return true; } else if (distance > 0 && driveTrain.getDistanceToWall() <= distance && lockedOnIterations >= 3) { return true; } return false; }
protected boolean isFinished() { //if by time if (driveTime > 0 && Utility.getFPGATime() >= startTime + driveTime) { driveTrain.tankDrive(0, 0); state = FINISHED; return true; } else if (distance > 0 && driveTrain.getDistanceToWall() <= distance && lockedOnIterations >= 3) { return true; } return false; }
public void start(double period, boolean periodic) { synchronized (m_processLock) { m_periodic = periodic; m_period = period; m_expirationTime = Utility.getFPGATime() * 1e-6 + m_period; updateAlarm(); } }
public void initialize() { //System.out.println("TimeEvent initialized!"); startTimeUs = Utility.getFPGATime(); super.initialize(); }
public boolean isTriggered() { long currentTimeUs = Utility.getFPGATime(); double delta = (currentTimeUs - startTimeUs)/1e6; //System.out.println("delta = " + delta + " duration = " + durationSec); if (delta > durationSec) { System.out.println("TimeEvent triggered!"); return true; } return false; }
public static void autoInit() { gearTrayOff(); //collectorOff(); resetMotors(); initTriggerTime = Utility.getFPGATime(); }
public static void teleopPeriodic() { // fire controls - using a timer to debounce double currentTime = Utility.getFPGATime(); // if not enough time has passed, no polling allowed! if ((currentTime - initTriggerTime) < TRIGGER_CYCLE_WAIT_US) return; double currentPos = positionServo.get(); // switch to control camera servo if (gamepad.getRawButton(HardwareIDs.CAMERA_CONTROL_BUTTON) == true) { if (Math.abs(currentPos - BOILER_CAM_POS) > SERVO_POS_TOLERANCE) moveToPos(BOILER_CAM_POS); } else { if (Math.abs(currentPos - GEAR_CAM_POS) > SERVO_POS_TOLERANCE) moveToPos(GEAR_CAM_POS); } // button to strobe camera LED rings if ((gamepad.getRawButton(HardwareIDs.CAMERA_LED_STROBE_BUTTON) == true) && (!ledState)) { setCameraLed(true); } else if ((gamepad.getRawButton(HardwareIDs.CAMERA_LED_STROBE_BUTTON) == false) && (ledState)) { setCameraLed(false); } // reset trigger init time initTriggerTime = Utility.getFPGATime(); }
public void initialize() { //System.out.println("GyroAngleEvent initialized!"); startTimeUs = Utility.getFPGATime(); super.initialize(); }
public void initialize() { //System.out.println("ClosedLoopPositionEvent initialized!"); startTimeUs = Utility.getFPGATime(); super.initialize(); }
public boolean isTriggered() { // measure current position error double actualPosInches = AutoDriveAssembly.getDistanceInches(); double errorPosInches = Math.abs(targetPosInches - actualPosInches); if (errorPosInches > errorThresholdInches) { // outside error range... // reset timer startTimeUs = Utility.getFPGATime(); return false; } long currentTimeUs = Utility.getFPGATime(); double delta = (currentTimeUs - startTimeUs)/1e6; //System.out.println("delta = " + delta + " duration = " + durationSec); if (delta < durationSec) { // within error range, but not for enough time return false; } // within error range for enough time System.out.println("ClosedLoopPositionEvent triggered!"); return true; }
/** * Create a new time system that uses the FPGA clock. At this time, the precision of the resulting clock has not been * verified or tested. * * @return the FPGA-based clock; never null * @throws StrongbackRequirementException if the FPGA hardware is not available */ public static Clock fpga() { try { Utility.getFPGATime(); // If we're here, then the method did not throw an exception and there is FPGA hardware on this platform ... return Utility::getFPGATime; } catch (UnsatisfiedLinkError | NoClassDefFoundError e) { throw new StrongbackRequirementException("Missing FPGA hardware or software", e); } }
public void resetMotorRPM() { for (int i = 0; i < m_numAveragedCycles; i++) { m_motorRPM[i] = 0.0; } m_lastTimeMicroSeconds = Utility.getFPGATime(); m_averagedMotorRPM = 0; m_encoder.start(); }
public void setMoveMotorSpeed(double percentVbus) { double deltaTimeMicroSeconds = Utility.getFPGATime() - m_startTimeMicroSeconds; double deltaTimeSeconds = deltaTimeMicroSeconds / 1000000; percentVbus = limitAccel(percentVbus, m_maxSpeed, deltaTimeSeconds); double steeringError = m_rightEncoder.getDistance() - m_leftEncoder.getDistance(); // double steeringError = m_yawGyro.getAngle(); double steerOffset = STEERING_ZERO_OFFSET; // if (percentVbus > 0) { // neg = forward // steerOffset = -steerOffset; // } m_drive.arcadeDrive(percentVbus, steerOffset + steeringError * KP_MOVE_STEERING); // System.out.println("Time = " + System.currentTimeMillis() + ", PercentVbus = " + percentVbus + ", Distance = " + returnPIDInput() + ", Steer error = " + steeringError + ", Left Distance = " + m_leftEncoder.getDistance() + ", Right Distance = " + m_rightEncoder.getDistance()); }
protected void initialize() { setSpeed = .8; scaleFactor = 1; //driveTrain.getGyro().reset(); bearing = driveTrain.getGyro().getAngle(); startTime = Utility.getFPGATime(); //returns fpga time in MICROseconds. DRIVE_SPEED = Double.parseDouble(BadPreferences.getValue(driveSpeedKey, ".6")); delayTime = Double.parseDouble(BadPreferences.getValue("DRIVE_STRAIGHT_FORWARD_WITH_DISTANCE_DELAY", "2.4")); }
protected void execute() { //with time if (distance <= 0) { driveTrain.getTrain().drive(-DRIVE_SPEED, -(driveTrain.getGyro().getAngle() - bearing) * Kp); } //with distance else { if (driveTrain.getDistanceToWall() < distance && Utility.getFPGATime() - startTime > delayTime*1000000) { lockedOnIterations++; } else { if (lockedOnIterations > 0) lockedOnIterations = 0; driveTrain.getTrain().drive(DRIVE_SPEED, -(driveTrain.getGyro().getAngle() - bearing) * Kp); } } }
protected void initialize() { setSpeed = .8; scaleFactor = 1; //driveTrain.getGyro().reset(); bearing = driveTrain.getGyro().getAngle(); startTime = Utility.getFPGATime(); //returns fpga time in MICROseconds. DRIVE_SPEED = Double.parseDouble(BadPreferences.getValue(driveSpeedKey, ".8")); delayTime = Double.parseDouble(BadPreferences.getValue("DRIVE_STRAIGHT_FORWARD_WITH_DISTANCE_DELAY", "2.4")); }
protected void execute() { //with time if (distance <= 0) { driveTrain.getTrain().drive(DRIVE_SPEED, -(driveTrain.getGyro().getAngle() - bearing) * Kp); } //with distance else { if (driveTrain.getDistanceToWall() < distance && Utility.getFPGATime() - startTime > delayTime*1000000) { lockedOnIterations++; } else { if (lockedOnIterations > 0) lockedOnIterations = 0; driveTrain.getTrain().drive(DRIVE_SPEED, -(driveTrain.getGyro().getAngle() - bearing) * Kp); } } }
private double getMsClock() { return Utility.getFPGATime() / 1000.0; }
private long getMsClock() { return Utility.getFPGATime() / 1000; }
public static void teleopInit() { setMotorStrength(MOTOR_MEDIUM); initTriggerTime = Utility.getFPGATime(); }
public static void initialize() { if (initialized) return; // reset trigger init time initTriggerTime = Utility.getFPGATime(); // create and reset collector relay collectorSolenoid = new Spark(HardwareIDs.COLLECTOR_SOLENOID_PWM_ID); // create and reset gear tray spark & relay gearTrayRelay = new Relay(HardwareIDs.GEAR_TRAY_RELAY_CHANNEL_1,Relay.Direction.kForward); gearTrayRelay.set(Relay.Value.kOff); gearTrayRelay2 = new Relay(HardwareIDs.GEAR_TRAY_RELAY_CHANNEL_2,Relay.Direction.kForward); gearTrayRelay2.set(Relay.Value.kOff); // create motors & servos transportMotor = new Spark(HardwareIDs.TRANSPORT_PWM_ID); collectorMotor = new CANTalon(HardwareIDs.COLLECTOR_TALON_ID); agitatorServo = new Spark(HardwareIDs.AGITATOR_PWM_ID); // continuous servo control modeled as Spark PWM feederMotor = new CANTalon(HardwareIDs.FEEDER_TALON_ID); shooterMotor = new CANTalon(HardwareIDs.SHOOTER_TALON_ID); // set up shooter motor sensor shooterMotor.reverseSensor(false); shooterMotor.setFeedbackDevice(FeedbackDevice.QuadEncoder); // FOR REFERENCE ONLY: //shooterMotor.configEncoderCodesPerRev(12); // use this ONLY if you are NOT reading Native units // USE FOR DEBUG ONLY: configure shooter motor for open loop speed control //shooterMotor.changeControlMode(TalonControlMode.PercentVbus); // configure shooter motor for closed loop speed control shooterMotor.changeControlMode(TalonControlMode.Speed); shooterMotor.configNominalOutputVoltage(+0.0f, -0.0f); shooterMotor.configPeakOutputVoltage(+12.0f, -12.0f); // set PID(F) for shooter motor (one profile only) shooterMotor.setProfile(0); shooterMotor.setP(3.45); shooterMotor.setI(0); shooterMotor.setD(0.5); shooterMotor.setF(9.175); // make sure all motors are off resetMotors(); gamepad = new Joystick(HardwareIDs.GAMEPAD_ID); initialized = true; }
public static void setShooterStrength(int newIndex) { if (!initialized) initialize(); // if out of range, just return... if ((newIndex > MOTOR_HIGH) || (newIndex < MOTOR_OFF)) return; // report on what strength we're setting //System.out.println("Motor Strength = " + motorSettings[newIndex]); double shooter_rpm = motorSettings[newIndex] * NATIVE_TO_RPM_FACTOR; InputOutputComm.putDouble(InputOutputComm.LogTable.kMainLog,"BallMgmt/ShooterRpm_Target", shooter_rpm); // set motor to the specified value shooterMotor.set(motorSettings[newIndex]); // if turning off motors... if (newIndex == MOTOR_OFF) { stopFeeding(); // turn off feeder & agitator motors } // if turning on motors... else { // if shooter motor is on and we're not yet feeding (i.e. motor is spinning up from being off) if (!feeding) { // spawn a wait thread to ensure agitator and feeder are turned on only AFTER a certain period new Thread() { public void run() { try { Thread.sleep(1000); // wait one second before starting to feed startFeeding(); // start feeder motors } catch (Exception e) { e.printStackTrace(); } } }.start(); } } // reset trigger init time initTriggerTime = Utility.getFPGATime(); }
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; }
