/** * {@inheritDoc} */ @Override public void calibrate() { if (m_spi == null) return; Timer.delay(0.1); synchronized (this) { m_accum_count = 0; m_accum_gyro_x = 0.0; m_accum_gyro_y = 0.0; m_accum_gyro_z = 0.0; } Timer.delay(kCalibrationSampleTime); synchronized (this) { m_gyro_offset_x = m_accum_gyro_x / m_accum_count; m_gyro_offset_y = m_accum_gyro_y / m_accum_count; m_gyro_offset_z = m_accum_gyro_z / m_accum_count; } }
protected boolean isFinished() { double wheelSpeedR; double wheelSpeedL; double elapsedTime; wheelSpeedL = RobotMap.driveTrainLeftFront.getSpeed(); wheelSpeedR = RobotMap.driveTrainRightFront.getSpeed(); elapsedTime = Timer.getFPGATimestamp() - startTime; if (elapsedTime > 1 && (wheelSpeedL <= .001 && wheelSpeedR <= .001)) { //System.out.println("VISION stop by Speed"); return true; } else if (elapsedTime > endTime) { //System.out.println("VISION Stop by timeout"); return true; } return false; //return (elapsedTime > 1 && (wheelSpeedL <= .001 && wheelSpeedR <= .001)) || (elapsedTime > endTime); //|| (NavX.ahrs.getWorldLinearAccelY() < -1); //-0.8); }
protected void initialize() { RobotMap.driveTrainRightFront.setPosition(0); RobotMap.driveTrainLeftFront.setPosition(0); RobotMap.driveTrainLeftFront.setEncPosition(0); RobotMap.driveTrainRightFront.setEncPosition(0); startTime = Timer.getFPGATimestamp(); maxAccel = 0; minAccel = 0; dispCount = 0; //initRightEnc = DriveEncoders.getRawRightValue(); //initLeftEnc = DriveEncoders.getRawLeftValue(); //prevTime = System.currentTimeMillis(); //prevRightError = 0; //prevLeftError = 0; //SmartDashboard.putString("Auton Debugging", "DriveForwardInit"); }
@Override public void perform() { terminal.writer().println(); terminal.writer().println(Messages.bold(NAME)); terminal.writer().println(); DigitalOutput digitalOutput = dioSet.getDigitalOutput(); if (digitalOutput == null) { terminal.writer().println(Messages.boldRed("no digital output selected")); return; } digitalOutput.disablePWM(); digitalOutput.enablePWM(0.25); Timer.delay(SLEEP_SEC); digitalOutput.updateDutyCycle(0.5); Timer.delay(SLEEP_SEC); digitalOutput.updateDutyCycle(0.75); Timer.delay(SLEEP_SEC); digitalOutput.updateDutyCycle(1.0); }
public void run() { double lastWriteTime = Timer.getFPGATimestamp(); while (true) { try { String msg = logMessages.take(); writer.write(msg); if (Timer.getFPGATimestamp() >= lastWriteTime + WRITE_TIME) { writer.flush(); lastWriteTime = Timer.getFPGATimestamp(); } } catch (InterruptedException | IOException e) { ((Throwable) e).printStackTrace(); } } }
@Override public void runCrashTracked() { synchronized (mTaskRunningLock) { if (mRunning) { double now = Timer.getFPGATimestamp(); Commands commands = Robot.getCommands(); RobotState robotState = Robot.getRobotState(); for (SubsystemLoop loop : mLoops) { loop.update(commands, robotState); Logger.getInstance().logSubsystemThread(loop.getStatus()); } mDt = now - mTimeStamp; mTimeStamp = now; } } }
public synchronized void start() { if (!mRunning) { System.out.println("Starting loops"); synchronized (mTaskRunningLock) { mTimeStamp = Timer.getFPGATimestamp(); for (SubsystemLoop loop : mLoops) { System.out.println("Starting " + loop.toString()); loop.start(); } mRunning = true; } mNotifier.startPeriodic(kPeriod); } else { System.out.println("SubsystemLooper already running"); } if (!mPrinting) { System.out.println("Starting subsystem printer"); mPrintTimeStamp = Timer.getFPGATimestamp(); mPrinting = true; mPrintNotifier.startPeriodic(kPrintRate); } }
public static void updateValues() { if (!initialized) return; // Default data if network table data pull fails double defaultDoubleVal = 0.0; // Pull data from grip numTargets = table.getNumber("targets", defaultDoubleVal); targetX = table.getNumber("targetX", defaultDoubleVal); targetY = table.getNumber("targetY", defaultDoubleVal); targetArea = table.getNumber("targetArea",defaultDoubleVal); targetDistance = table.getNumber("targetDistance",defaultDoubleVal); Timer.delay(0.02); }
@Override public void run() { double current = 0; final double filter = 0.1666667; final double max = 30; // 30 Amps should not be violated in most cases boolean isDisabled = false; while (!Thread.currentThread().isInterrupted()) { // Rolling Avg. current = filter * getOutputCurrent() + current * (1 - filter); if (current > max * 0.9 && !isDisabled) { disableControl(); isDisabled = true; }else if (current < max * 0.80 && isDisabled){ enableControl(); isDisabled = false; } Timer.delay(0.1); } }
public void Init() { Logger.PrintLine("Init 1",Logger.LOGGER_MESSAGE_LEVEL_DEBUG); m_pidController = new PIDController(10,10,10,m_encoderAverager,m_robotDrivePid); m_pidController.setOutputRange(-0.8,0.8); //m_pidController.setOutputRange(-0.4,0.4); Logger.PrintLine("Init 4",Logger.LOGGER_MESSAGE_LEVEL_ERROR); m_currentStateIndex = 0; SetCurrentState(m_nextStateArray[m_currentStateIndex]); m_disabled = false; m_shootingBall = false; m_driving = false; m_braking = false; m_detectingImage = false; m_currentStateIndex = 0; m_loadingBall = false; m_shooterPullingBack = false; m_autonomousStartTime = Timer.getFPGATimestamp(); m_robotDrivePid.resetGyro(); }
private void HandleStateRelease() { if (!m_gearReleased) { // set the gear in neutral m_gearControl.set(DoubleSolenoid.Value.kForward); m_gearReleased = true; } if(!m_latchReleased) { //release the latch m_latchReleaseServo.set(1); Timer.delay(0.5); m_latchReleased = true; } m_isPulledBack = false; m_currentState = SHOOTER_CONTROL_STATE_WAIT; }
/** * Initialize the gyro. Calibration is handled by calibrate(). */ public void initGyro() { result = new AccumulatorResult(); m_voltsPerDegreePerSecond = kDefaultVoltsPerDegreePerSecond; m_analog.setAverageBits(kAverageBits); m_analog.setOversampleBits(kOversampleBits); double sampleRate = kSamplesPerSecond * (1 << (kAverageBits + kOversampleBits)); AnalogInput.setGlobalSampleRate(sampleRate); Timer.delay(0.1); setDeadband(0.0); setPIDSourceType(PIDSourceType.kDisplacement); UsageReporting.report(tResourceType.kResourceType_Gyro, m_analog.getChannel()); LiveWindow.addSensor("AnalogGyro", m_analog.getChannel(), this); }
/** * {@inheritDoc} */ @Override public void calibrate() { if (m_spi == null) return; Timer.delay(0.1); synchronized (this) { m_accum_count = 0; m_accum_gyro_x = 0.0; m_accum_gyro_y = 0.0; m_accum_gyro_z = 0.0; } Timer.delay(kCalibrationSampleTime); synchronized (this) { m_gyro_center_x = m_accum_gyro_x / m_accum_count; m_gyro_center_y = m_accum_gyro_y / m_accum_count; m_gyro_center_z = m_accum_gyro_z / m_accum_count; } }
public static void reset() { System.out.println("NavXSensor::reset called!"); if (ahrs != null) { ahrs.reset(); ahrs.resetDisplacement(); ahrs.zeroYaw(); // allow zeroing to take effect Timer.delay(0.1); // get the absolute angle after reset - Not sure why it is non-zero, but we need to record it to zero it out yawOffset = ahrs.getAngle(); System.out.println("yawOffset read = " + yawOffset); } }
protected void execute() { //check to see limit switch // if(direction.equals("in") && Robot.intakeRollerSubsystem.getOverride()) // Robot.intakeRollerSubsystem.intake(); // else if(direction.equals("in") && RobotMap.shooterEncoder.getRate() > 0) // Robot.intakeRollerSubsystem.intake(); // else if (direction.equals("in") && buttonsNotPressed()) // Robot.intakeRollerSubsystem.intake(); if(direction.equals("in")) Robot.intakeRollerSubsystem.intake(); else if(direction.equalsIgnoreCase("out")) Robot.intakeRollerSubsystem.outake(); else if(direction.equalsIgnoreCase("auton")){ Robot.intakeRollerSubsystem.outake(); Timer.delay(1); } else Robot.intakeRollerSubsystem.neutral(); }
/** * This autonomous (along with the chooser code above) shows how to select * between different autonomous modes using the dashboard. The sendable * chooser code works with the Java SmartDashboard. If you prefer the * LabVIEW Dashboard, remove all of the chooser code and uncomment the * getString line to get the auto name from the text box below the Gyro * * You can add additional auto modes by adding additional comparisons to the * if-else structure below with additional strings. If using the * SendableChooser make sure to add them to the chooser code above as well. */ @Override public void autonomous() { String autoSelected = chooser.getSelected(); // String autoSelected = SmartDashboard.getString("Auto Selector", // defaultAuto); System.out.println("Auto selected: " + autoSelected); switch (autoSelected) { case customAuto: myRobot.setSafetyEnabled(false); myRobot.drive(-0.5, 1.0); // spin at half speed Timer.delay(2.0); // for 2 seconds myRobot.drive(0.0, 0.0); // stop robot break; case defaultAuto: default: myRobot.setSafetyEnabled(false); myRobot.drive(-0.5, 0.0); // drive forwards half speed Timer.delay(2.0); // for 2 seconds myRobot.drive(0.0, 0.0); // stop robot break; } }
public SimulatorFrame() { initComponenents(); setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE); RobotStateSingleton.get().addLoopListener(new RobotStateSingleton.LoopListener() { @Override public void looped() { mBasicPanel.update(); mEnablePanel.setTime(Timer.getMatchTime()); } }); }
@Override protected void initialize() { //// CHANGE FOR PRACTICE TO COMPETITION DONE DONE DONE DOOOONNNEE //////////////////////// CHANGE DURING COMP: NEED TO FIND RIGHT ENCODERS if (RobotMap.armMotor.getEncPosition() < 12){ // latchCylinder.cylinder(true); //Timer.delay(2); //latchCylinder.cylinder(false); latchCylinder.cylinder(true); Timer.delay(.5); latchCylinder.cylinder(false); } //latchCylinder.cylinder(DoubleSolenoid.Value.kReverse); //Timer.delay(1); //latchCylinder.cylinder(DoubleSolenoid.Value.kForward); //// CHANGE FOR PRACTICE TO COMPETITION END }
public synchronized void run() { Ultrasonic u = null; while (m_automaticEnabled) { if (u == null) { u = m_firstSensor; } if (u == null) { return; } if (u.isEnabled()) { u.m_pingChannel.pulse(m_pingChannel.m_channel, (float) kPingTime); // do // the // ping } u = u.m_nextSensor; Timer.delay(.1); // wait for ping to return } }
@Override public void init() { RobotMap.sL.SystemLoggerWriteTimeline("Distance_MovePID_Init"); brakeTimer = new Timer(); done = false; P = .35; // System.out.println("in Init"); // // I = 0; D = 0; sumL = 0; sumR = 0; prevTime = 0; prevSpeed = 0; // // // distanceR += (RobotMap.right1.getEncPosition() / Constants.encoderTickPerFoot); // P = P / distanceL; // timer.reset(); // timer.start(); }
/** * This autonomous (along with the chooser code above) shows how to select * between different autonomous modes using the dashboard. The sendable * chooser code works with the Java SmartDashboard. If you prefer the * LabVIEW Dashboard, remove all of the chooser code and uncomment the * getString line to get the auto name from the text box below the Gyro * * You can add additional auto modes by adding additional comparisons to the * if-else structure below with additional strings. If using the * SendableChooser make sure to add them to the chooser code above as well. */ @Override public void autonomous() { String autoSelected = chooser.getSelected(); // String autoSelected = SmartDashboard.getString("Auto Selector", // defaultAuto); System.out.println("Auto selected: " + autoSelected); gyro.reset(); switch (autoSelected) { case customAuto: myRobot.setSafetyEnabled(false); break; case defaultAuto: default: myRobot.setSafetyEnabled(false); myRobot.drive(-0.5, 0.0); // drive forwards half speed Timer.delay(2.0); // for 2 seconds myRobot.drive(0.0, 0.0); // stop robot break; } }
@Override protected void initialize() { Robot.drive.resetGyro(); timer = new Timer(); timer.reset(); timer.start(); }
protected void execute() { double runTime = Timer.getFPGATimestamp() - startTime; if(runTime < 2) { RobotMap.driveTrainLeftFront.set(0.5); RobotMap.driveTrainRightFront.set(0.0); RobotMap.driveTrainLeftRear.set(0.0); RobotMap.driveTrainRightRear.set(0.0); } else if(runTime < 4) { RobotMap.driveTrainLeftFront.set(0.0); RobotMap.driveTrainRightFront.set(0.5); RobotMap.driveTrainLeftRear.set(0.0); RobotMap.driveTrainRightRear.set(0.0); } else if(runTime < 6) { RobotMap.driveTrainLeftFront.set(0.0); RobotMap.driveTrainRightFront.set(0.0); RobotMap.driveTrainLeftRear.set(0.5); RobotMap.driveTrainRightRear.set(0.0); } else if(runTime < 8) { RobotMap.driveTrainLeftFront.set(0.0); RobotMap.driveTrainRightFront.set(0.0); RobotMap.driveTrainLeftRear.set(0.0); RobotMap.driveTrainRightRear.set(0.5); } else { RobotMap.driveTrainLeftFront.set(0.0); RobotMap.driveTrainRightFront.set(0.0); RobotMap.driveTrainLeftRear.set(0.0); RobotMap.driveTrainRightRear.set(0.0); } }
protected boolean isFinished() { double runTime = Timer.getFPGATimestamp() - startTime; if(runTime < 8) { return false; } else { return true; } }
protected void initialize() { setInitEncoderVal(DriveEncoders.getAbsoluteValue()); SmartDashboard.putString("Auton Debugging", "DriveForwardInit"); System.out.println("DriveFowardInit"); startTime = Timer.getFPGATimestamp(); }
protected boolean isFinished() { if ( 124 < Robot.pixyValue || 130 > Robot.pixyValue) { return true; } else if ((Timer.getFPGATimestamp() - startTime) > 3) return true; return false; /*if (Robot.pixyInput.getAverageVoltage() > .95 && Robot.pixyInput.getAverageVoltage() < 1.05) return true; return false; */ }
protected boolean isFinished() { if (Timer.getFPGATimestamp() - startTime > timeout) { return true; } else { return false; } }
protected void initialize() { setInitEncoderVal(DriveEncoders.getAbsoluteValue()); buffer = new byte[1]; startTime = Timer.getFPGATimestamp(); Robot.pixyValue = (byte) 255; dispCount = 0; }
protected void initialize() { RobotMap.driveTrainRightFront.setPosition(0); RobotMap.driveTrainLeftFront.setPosition(0); RobotMap.driveTrainLeftFront.setEncPosition(0); RobotMap.driveTrainRightFront.setEncPosition(0); startTime = Timer.getFPGATimestamp(); maxAccel = 0; minAccel = 0; //initRightEnc = DriveEncoders.getRawRightValue(); //initLeftEnc = DriveEncoders.getRawLeftValue(); //prevTime = System.currentTimeMillis(); //prevRightError = 0; //prevLeftError = 0; //SmartDashboard.putString("Auton Debugging", "DriveForwardInit"); }
protected boolean isFinished() { if (Timer.getFPGATimestamp() - startTime > 5) { System.out.println("End for time"); return true; } else if (NavX.ahrs.getWorldLinearAccelY() < stopLevel && Math.abs(RobotMap.driveTrainRightFront.getEncPosition()) > distance) { System.out.println("end for Accel and Dist: Cur: " + minAccel + " " + RobotMap.driveTrainRightFront.getEncPosition() + " Dist: " + distance); return true; } else return false; }
protected boolean isFinished() { if (Timer.getFPGATimestamp() - startTime > timeout) { System.out.println("End for time"); return true; } else if (NavX.ahrs.getWorldLinearAccelY() < stopLevel && Math.abs(RobotMap.driveTrainRightFront.getEncPosition()) > distance) { System.out.println("end for Accel and Dist: Cur:" + RobotMap.driveTrainRightFront.getEncPosition() + " Dist: " + distance + " Accel " + maxAccel); return true; } else return false; }
public static void intializeEncoders() { RobotMap.driveTrainLeftFront.setEncPosition(0); RobotMap.driveTrainRightFront.setEncPosition(0); RobotMap.driveTrainRightFront.setPosition(0); RobotMap.driveTrainLeftFront.setPosition(0); System.out.println("OLD - RF: " + RobotMap.driveTrainRightFront.getEncPosition() + " LF:" + RobotMap.driveTrainLeftFront.getEncPosition()); Timer.delay(0.04); System.out.println("NEW: RF: " + RobotMap.driveTrainRightFront.getEncPosition() + " LF:" + RobotMap.driveTrainLeftFront.getEncPosition()); }
public DriveForward(double power, double time) { requires(Robot.driveTrain); _timer = new Timer(); _power = power; _time = time; }
protected void execute() { double runTime = Timer.getFPGATimestamp() - startTime; if(runTime < 2) { WheelMotors.lFrontDrive.set(0.5); WheelMotors.rFrontDrive.set(0.0); WheelMotors.lRearDrive.set(0.0); WheelMotors.rRearDrive.set(0.0); } else if(runTime < 4) { WheelMotors.lFrontDrive.set(0.0); WheelMotors.rFrontDrive.set(0.5); WheelMotors.lRearDrive.set(0.0); WheelMotors.rRearDrive.set(0.0); } else if(runTime < 6) { WheelMotors.lFrontDrive.set(0.0); WheelMotors.rFrontDrive.set(0.0); WheelMotors.lRearDrive.set(0.5); WheelMotors.rRearDrive.set(0.0); } else if(runTime < 8) { WheelMotors.lFrontDrive.set(0.0); WheelMotors.rFrontDrive.set(0.0); WheelMotors.lRearDrive.set(0.0); WheelMotors.rRearDrive.set(0.5); } else { WheelMotors.lFrontDrive.set(0.0); WheelMotors.rFrontDrive.set(0.0); WheelMotors.lRearDrive.set(0.0); WheelMotors.rRearDrive.set(0.0); } }
