public void autonomousPeriodic() {
   relay.set(Relay.Value.kOn);
   smart.putNumber("distance", ultrasonic.getVoltage());
   drive.setRun(false);
   loadAndShoot.setRun(false);
   //    relay.set(Relay.Value.kOn);
   if (autoTimer.get() < autoMoveForwardTime) {
     System.out.println(
         "Moving forward, Timer at "
             + autoTimer.get()
             + ", Ultrasonic at "
             + ultrasonic.getAverageVoltage());
   }
   if (autoTimer.get() >= autoMoveForwardTime && ultrasonic.getAverageVoltage() <= AUTO_DISTANCE) {
     stop();
     if (!autonomousStopped) {
       autonomousStopped = true;
       autoTimeAtStop = autoTimer.get();
     }
     System.out.println("Stopped, Ultrasonic at " + ultrasonic.getAverageVoltage());
   }
   if (autoTimer.get() >= autoTimeAtStop + 1 && ultrasonic.getAverageVoltage() <= AUTO_DISTANCE) {
     autoStop = true;
     shoot();
     System.out.println("Shooting, Ultrasonic at " + ultrasonic.getAverageVoltage());
   }
 }
  public TargetFinder() {
    System.out.println("TargetFinder() begin " + Timer.getFPGATimestamp());

    cam = AxisCamera.getInstance();
    cam.writeResolution(AxisCamera.ResolutionT.k320x240);
    cam.writeBrightness(camBrightness);
    cam.writeColorLevel(camColor);
    cam.writeWhiteBalance(camWhiteBalance);
    cam.writeExposureControl(camExposure);
    cam.writeMaxFPS(15);
    cam.writeExposurePriority(AxisCamera.ExposurePriorityT.none);
    cam.writeCompression(50);
    // System.out.println("TargetFinder() * " + cam.toString() + "[" + Timer.getFPGATimestamp());
    boxCriteria = new CriteriaCollection();
    inertiaCriteria = new CriteriaCollection();
    boxCriteria.addCriteria(
        NIVision.MeasurementType.IMAQ_MT_BOUNDING_RECT_WIDTH, 30, bboxWidthMin, false);
    boxCriteria.addCriteria(
        NIVision.MeasurementType.IMAQ_MT_BOUNDING_RECT_HEIGHT, 40, bboxHeightMin, false);
    inertiaCriteria.addCriteria(
        NIVision.MeasurementType.IMAQ_MT_NORM_MOMENT_OF_INERTIA_XX, 0, inertiaXMin, true);
    inertiaCriteria.addCriteria(
        NIVision.MeasurementType.IMAQ_MT_NORM_MOMENT_OF_INERTIA_YY, 0, inertiaYMin, true);
    Timer.delay(7);
  }
Ejemplo n.º 3
0
  /*
   * reads the value of the digital input of the specified channel
   * requires that the readArudiono() method has already been called
   * param 1: the channel to be read
   * return: value of the specified digital input
   */
  public boolean getInput(int channelID) {
    byte temp;

    if (timer.get() > 2.0) {
      DriverStationLCD.getInstance()
          .println(DriverStationLCD.Line.kUser3, 1, "                     ");
      DriverStationLCD.getInstance().updateLCD();
      timer.stop();
      timer.reset();
    }

    if (Math.abs(channelID - (13 / 2)) - (13 / 2)
        > 0) // returns true if less than zero or greater than thirteen
    {
      timer.start();
      DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser3, 1, "Invalid ID");
      DriverStationLCD.getInstance().updateLCD();
      return false;
    } else if (channelID < 8) {
      temp = (byte) (loByte & (1 << channelID));
      return temp != 0;
    } else {
      temp = (byte) (hiByte & (1 << channelID - 8));
      return temp != 0;
    }
  }
  public void autonomousInit() {
    try {
      robot.arm.moveTo(PegPosition.RESET);
    } catch (CANTimeoutException e1) {
      // TODO Auto-generated catch block
      e1.printStackTrace();
    }
    robot.compressor.start();
    Timer.delay(3.5);
    robot.grabber.tiltDown();

    Timer.delay(2);
    robot.grabber.grab();

    robot.lineSensor.setLinePreference(LinePreference.LEFT);
    controller.enable();

    Timer.delay(2.5);
    robot.grabber.tiltUp();
    Timer.delay(2.5);
    try {
      robot.arm.moveTo(PegPosition.MIDDLE_OFFSET);
    } catch (CANTimeoutException e) {
      // TODO Auto-generated catch block
      e.printStackTrace();
    }
  }
Ejemplo n.º 5
0
  protected void initialize() {

    time = new Timer();

    time.reset();
    time.start();
  }
Ejemplo n.º 6
0
  public void driveToBridge() {
    System.out.println("Val: " + potentiometer.getVoltage());

    if (Autonomousflag) {
      if (potentiometer.getVoltage() < 5) { // line
        shoot();
        Timer.delay(7);
        shoot.set(0);
        collect.set(0);

        driveTrain.tankDrive(-speed, speed);
        Timer.delay(1);
        driveTrain.tankDrive(speed, speed);
        Timer.delay(1.2);

        driveTrain.tankDrive(0, 0);
        Timer.delay(0.9);
        bridge.set(-1);

      } else if (potentiometer.getVoltage() > 5) { // left
        shoot();
      }
      Autonomousflag = false;
    } else {
      driveTrain.tankDrive(0, 0);
    }
  }
Ejemplo n.º 7
0
 public boolean waitForComplete(double milli) {
   System.out.println("time currently: " + timer.get() + " - waiting for: " + milli);
   if (timer.get() <= milli) return false;
   else {
     return true;
   }
 }
Ejemplo n.º 8
0
 @Override
 protected void initialize() {
   beginTime = Integer.MAX_VALUE;
   triggered = false;
   timer1.reset();
   timer1.start();
   System.out.println("Shoot Init");
 }
Ejemplo n.º 9
0
 // Called just before this Command runs the first time
 protected void initialize() {
   Robot.logger.log("Initialize", 1);
   finished = false;
   timer.reset();
   timer.start();
   lastOffset = 0;
   stillCycles = 0;
 }
Ejemplo n.º 10
0
 public void shoot(double timeToShoot) {
   m_timeToShoot = timeToShoot;
   if (false == shooting && false == retracting) {
     shooting = true;
     retracting = false;
     shootTime.start();
     shootTime.reset();
   }
 }
Ejemplo n.º 11
0
 // Called just before this Command runs the first time
 protected void initialize() {
   Robot.drivetrain.resetGyro();
   Robot.drivetrain.drivePID.setSetpoint(STRAIGHT_SET_POINT);
   Robot.drivetrain.throttleInt.useJoystick(false);
   Robot.drivetrain.throttleInt.setThrottle(thisDriveSpeed);
   timer2.start();
   startTime2 = timer2.get();
   state = State.START_DRIVE; // initializes the state
 }
Ejemplo n.º 12
0
  protected void execute() {
    double time = _timer.get();

    if (time >= _period) {
      Robot.ledStrip.setColor(
          _colors[index].getRed(), _colors[index].getGreen(), _colors[index].getBlue());
      index = (index + 1) % _colors.length;
      _timer.reset();
    }
  }
Ejemplo n.º 13
0
 public double getRPM() {
   if (Timer.getFPGATimestamp() > RPMUpdatePeriod + encoderMarkTime) {
     double encoderCPS =
         (enc.get() - encoderMarkCounts) / (Timer.getFPGATimestamp() - encoderMarkTime);
     encoderRPM = (encoderCPS * 60) / 16;
     encoderMarkCounts = enc.get();
     encoderMarkTime = Timer.getFPGATimestamp();
   }
   return encoderRPM;
 }
Ejemplo n.º 14
0
 protected void initialize() {
   if (!inProgress) {
     inProgress = true;
     Pickup.checkArms();
     Catapult.latch.set(Latch.UNLOCKED);
     Timer.delay(0.5);
     Catapult.setLauncher(RETRACTED);
     Timer.delay(2.0);
     Catapult.latch.set(Latch.LOCKED);
   }
 }
Ejemplo n.º 15
0
 // Called just before this Command runs the first time
 protected void initialize() {
   Robot.intake.setIntakePosition(true);
   Timer.delay(1);
   Robot.shooter.setShooterPosition(true);
   Robot.intake.setIntakePosition(false);
   Timer.delay(4);
   // Robot.shooter.switchShooterPosition();
   // runTime.reset();
   // runTime.start();
   System.out.println("rock wall");
 }
Ejemplo n.º 16
0
 /** Resets the recorder so we can record properly next time */
 public void reset() {
   if (m_bRecStarted) {
     writeDataToFile();
     m_List.removeAllElements();
     m_Index = 0;
     m_tmRecorder.stop();
     m_tmRecorder.reset();
     m_bRecDone = false;
     m_bRecStarted = false;
   }
 }
 // Called just before this Command runs the first time
 protected void initialize() {
   Robot.driveTrain.setLeftMotors(0.5);
   Robot.driveTrain.setRightMotors(-0.5);
   Timer.delay(1.5);
   Robot.driveTrain.setLeftMotors(0);
   Robot.driveTrain.setRightMotors(-0.8);
   Timer.delay(0.5);
   Robot.driveTrain.setLeftMotors(0.6);
   Robot.driveTrain.setRightMotors(-0.5);
   Timer.delay(1);
 }
Ejemplo n.º 18
0
  public void autonomousInit() {
    // Timer section
    timerMain = new Timer();
    timerMain.reset();
    timerMain.start();

    timer1Left = new Timer();
    timer2Left = new Timer();

    timer1Right = new Timer();
    timer2Right = new Timer();

    timer1Center = new Timer();
    timer2Center = new Timer();

    controlChuteLeft = new boolean[] {false, false};
    controlChuteRight = new boolean[] {false, false};
    controlChuteCenter = new boolean[] {false, false};
    controlChuteLeft[0] = true;
    controlChuteRight[0] = true;
    controlChuteCenter[0] = true;

    /* Boolean setting section
     * Robot Aligns Left
     */
    if (DigitalInput7.get() == false && DigitalInput8.get() == true) {
      b_leftAutonomous = true;
      b_rightAutonomous = false;
      b_centerAutonomous = false;
      b_upAutonomous = false;
      b_downAutonomous = true;
      b_leftDriveAutonomous = false;
    }
    // Robot Aligns Right
    if (DigitalInput7.get() == true && DigitalInput8.get() == false) {
      b_leftAutonomous = false;
      b_rightAutonomous = true;
      b_centerAutonomous = false;
      b_upAutonomous = false;
      b_downAutonomous = true;
      b_rightDriveAutonomous = false;
    }
    // Robot Aligns Center
    if (DigitalInput7.get() == true && DigitalInput8.get() == true) {
      b_leftAutonomous = false;
      b_rightAutonomous = false;
      b_centerAutonomous = true;
      b_upAutonomous = false;
      b_downAutonomous = true;
      b_centerDriveAutonomous = false;
    }
  } // End autonomousInit
Ejemplo n.º 19
0
 private double m_shoot() {
   if (encoder.get() >= desired || shootTime.get() > m_timeToShoot) {
     shooting = false;
     retracting = true;
     shootTime.stop();
     retractTime.start();
     retractTime.reset();
     return .1;
   } else { // encoder < desired && shootTime < 0.2
     error = desired - encoder.get();
     return -1;
   }
 }
Ejemplo n.º 20
0
  protected void execute() {
    double delta = timer.get();
    distance += (drivetrain.getAverageForwardVelocity() * delta);
    timer.reset();

    // PID tuning code
    pidControllerDistance.setPID(
        pidConfigDistance.getP(P),
        pidConfigDistance.getI(I),
        pidConfigDistance.getD(D),
        pidConfigDistance.getF(F));
    pidConfigDistance.setSetpoint(pidControllerDistance.getSetpoint());
    pidConfigDistance.setValue(drivetrainRotation.pidGet());
  }
Ejemplo n.º 21
0
  protected void execute() {

    if (time.get() < 3.0) {
      talonLeft.set(1);
      talonRight.set(1);
    } else if (time.get() < 5.0) {
      talonLeft.set(0);
      talonRight.set(0);
      soleLiftDown.set(true);
    } else if (time.get() < 6.0) {
      talonLeft.set(-1);
      talonRight.set(-1);
    } else talonLeft.set(1);
    talonRight.set(1);
  }
Ejemplo n.º 22
0
 /** Runs the motors with tank steering. */
 public void operatorControl() {
   myRobot.setSafetyEnabled(true);
   while (isOperatorControl() && isEnabled()) {
     myRobot.tankDrive(leftStick, rightStick);
     Timer.delay(0.005); // wait for a motor update time
   }
 }
Ejemplo n.º 23
0
 @Override
 protected void end() {
   timer1.stop();
   shooterWheel.rawShoot(0);
   shooterBar.rawBallPush(0);
   System.out.println("Shoot End");
 }
Ejemplo n.º 24
0
 protected void end() {
   Timer.delay(1.5);
   if (Catapult.latch.get() == Latch.UNLOCKED) {
     Catapult.latch.set(Latch.LOCKED); // set latch in
   }
   inProgress = false;
 }
  public AutoCommandGroup() {

    // STARTING AT YELLOW BIN

    addSequential(new LiftCommand(0.75, 0.5, true));
    Timer.delay(1);
    addSequential(new AutoMove(1, 2.5, 13.5));
    addParallel(new LiftCommand(1, 0.5, true));
    addSequential(new LiftCommand(0.75, 0.5, false));

    // STARTING AT LANDFILL

    // addSequential(new AutoMove (0.6, 2.5, 0.4));
    // Timer.delay(1.5);
    // addSequential(new LiftCommand (0.75,0.5, true));
    // addSequential(new AutoMove (-1, 2.5, 7.8));
    // addSequential (new LiftCommand (0.75, 0.5, false));

    // Add Commands here:
    // e.g. addSequential(new Command1());
    //      addSequential(new Command2());
    // these will run in order.

    // To run multiple commands at the same time,
    // use addParallel()
    // e.g. addParallel(new Command1());
    //      addSequential(new Command2());
    // Command1 and Command2 will run in parallel.

    // A command group will require all of the subsystems that each member
    // would require.
    // e.g. if Command1 requires chassis, and Command2 requires arm,
    // a CommandGroup containing them would require both the chassis and the
    // arm.
  }
Ejemplo n.º 26
0
  public Shooter(int encoderA, int encoderB, int victor1, int victor2, int victor3) {

    encoder = new Encoder(encoderA, encoderB);
    encoder.start();
    encoder.reset();
    motor1 = new Victor(victor1); // 2
    motor2 = new Victor(victor2); // 5
    motor3 = new Victor(victor3); // 4

    time = new Timer();
    shootTime = new Timer();
    retractTime = new Timer();

    time.start();
    time.reset();
  }
Ejemplo n.º 27
0
  /**
   * start up automatic capture you should see the video stream from the webcam in your FRC PC
   * Dashboard.
   */
  public void operatorControl() {

    while (isOperatorControl() && isEnabled()) {
      /** robot code here! * */
      Timer.delay(0.005); // wait for a motor update time
    }
  }
Ejemplo n.º 28
0
  @Override
  protected void initialize() {
    backLeft = new Talon(ControlsManager.BACK_LEFT_SPEED_CONTROLLER);
    frontLeft = new Talon(ControlsManager.FRONT_LEFT_SPEED_CONTROLLER);
    backRight = new Talon(ControlsManager.BACK_RIGHT_SPEED_CONTROLLER);
    frontRight = new Talon(ControlsManager.FRONT_RIGHT_SPEED_CONTROLLER);

    backLeft.setInverted(true);
    frontLeft.setInverted(true);
    backRight.setInverted(true);
    frontRight.setInverted(true);

    lidar = new LIDAR(Port.kMXP);

    // ultrasonic = new Ultrasonic(RobotMap.ultraPing, RobotMap.ultraEcho);
    // ultrasonic.setEnabled(true); ultrasonic.setAutomaticMode(true);

    maxbotix = new BadUltrasonic(ControlsManager.MAXBOTIX_ULTRASONIC);

    // mxp stuff
    serialPort = new SerialPort(57600, SerialPort.Port.kMXP);

    byte update_rate_hz = 127;
    mxp = new IMU(serialPort, update_rate_hz);
    Timer.delay(0.3);
    mxp.zeroYaw();

    train = new RobotDrive(backLeft, frontLeft, backRight, frontRight);
  }
Ejemplo n.º 29
0
  public void calibrateAcel() {
    double avgX = 0;
    double avgY = 0;

    for (int i = 0; i < calibrationSamples; i++) {
      avgX += acel.getX();
      avgY += acel.getY();
      Timer.delay(0.005); // calibrationSampleRate / 1000.0);
    }
    avgX /= calibrationSamples;
    avgY /= calibrationSamples;
    acelBias = new Vector2(avgX, avgY);
    lastTimestamp = Timer.getFPGATimestamp();
    // SmartDashboard.putNumber("X Bias", acelBias.x);
    // SmartDashboard.putNumber("Y Bias", acelBias.y);
  }
 public void init() {
   try {
     TKOHardware.changeTalonMode(
         TKOHardware.getLeftDrive(), CANTalon.ControlMode.Position, p, i, d);
     TKOHardware.changeTalonMode(
         TKOHardware.getRightDrive(), CANTalon.ControlMode.Position, p, i, d);
     TKOHardware.getLeftDrive().reverseOutput(false);
     TKOHardware.getRightDrive().reverseOutput(true);
     TKOHardware.getLeftDrive().reverseSensor(true);
     TKOHardware.getRightDrive().reverseSensor(false);
     TKOHardware.getLeftDrive().enableBrakeMode(true);
     TKOHardware.getRightDrive().enableBrakeMode(true);
     TKOHardware.getLeftDrive().setPosition(0); // resets encoder
     TKOHardware.getRightDrive().setPosition(0);
     TKOHardware.getLeftDrive().ClearIaccum(); // stops bounce
     TKOHardware.getRightDrive().ClearIaccum();
     Timer.delay(0.1);
     TKOHardware.getLeftDrive().set(TKOHardware.getLeftDrive().getPosition());
     TKOHardware.getRightDrive().set(TKOHardware.getRightDrive().getPosition());
     TKOHardware.getPiston(0).set(Definitions.SHIFTER_LOW);
     TKOHardware.getPiston(2).set(Definitions.WHEELIE_RETRACT);
   } catch (TKOException e) {
     e.printStackTrace();
     System.out.println("Err.... Talons kinda died ");
   }
   System.out.println("Drive atom initialized");
 }