/** This function is called periodically during autonomous */
  public void autonomousPeriodic() {
    wedge.moveDown();
    lights.set(Relay.Value.kForward);
    // tracking.run();
    try {
      driveMainBelt(true, true);
      csc.setSetpoint(2300); // 15' = ~2400rpm
      if (launcher.getSpeed() < -2250) {
        try {
          belt2.setX(-1);
        } catch (Exception e) {
        }
      } else {
        belt2.setX(0);
      }
      station.println(DriverStationLCD.Line.kMain6, 1, "RPM: " + String.valueOf(csc.tmpResult));
    } catch (CANTimeoutException ex) {
    }

    /*try{
        if(tracking.foundTarget()){
            station.println(DriverStationLCD.Line.kUser2, 1, "Distance: " + String.valueOf(tracking.getTarget("HIGH").distance));
            station.println(DriverStationLCD.Line.kUser3, 1, "Suggested RPM: " + String.valueOf(tracking.getTarget("HIGH").distance*62.8));
            if(x>0)
                station.println(DriverStationLCD.Line.kUser4, 1, "Rotation: <--");
            else if(x<0)
                station.println(DriverStationLCD.Line.kUser4, 1, "Roation: -->");
            else
                station.println(DriverStationLCD.Line.kUser4, 1, "I don't need to rotate");
        }
        else{
            station.println(DriverStationLCD.Line.kUser2, 1, "Help, I can't find target by myself!");
            station.println(DriverStationLCD.Line.kUser3, 1, "");
            station.println(DriverStationLCD.Line.kUser4, 1, "");
        }
    }catch(Exception e){}*/
    station.updateLCD();
  }
  /** This function is called periodically during operator control */
  public void teleopPeriodic() {
    lights.set(Relay.Value.kForward);
    // tracking.run();
    /*try {
        readString = serial.readString();
        readString = readString.substring(readString.indexOf("data{"), readString.indexOf("}"));
        xTurn = readString.substring(readString.indexOf("x("), readString.indexOf(")"));
        xTurnVal = Integer.parseInt(xTurn);
    } catch (VisaException ex) {
        System.out.println("Error Reading string - " + ex);
    }*/
    y = drive_control.getRawAxis(1);
    x = drive_control.getRawAxis(2);
    t = drive_control.getRawAxis(3);

    if (y > 1) y = 1;
    else if (y < -1) y = -1;

    if (x > 1) x = 1;
    else if (x < -1) x = -1;

    drive.setDriveJ(x, y, t);

    /*if(drive_control.getRawButton(12))
        //wheelsOn = true;
    else if(drive_control.getRawButton(11));
        wheelsOn = false;*/

    /*try{
        launcher.setX((drive_control.getRawAxis(4)-1)/2);
    }catch(Exception e){}*/

    // System.out.println((drive_control.getRawAxis(4)-1)*-1350);
    if (!autoWheels) {
      csc.setSetpoint((drive_control.getRawAxis(4) - 1) * -1500);
      try {
        // launcher.setX((drive_control.getRawAxis(4)+1)*2000);
        System.out.println(
            -launcher.getSpeed()
                + " -- "
                + csc.tmpResult
                + " -- "
                + (drive_control.getRawAxis(4) - 1) * -1500);
      } catch (Exception e) {
        System.out.println(e);
      }
      if (drive_control.getRawButton(11)) autoWheels = true;
    } else {
      csc.setSetpoint(2300);
      if (drive_control.getRawButton(11)) autoWheels = false;
    }

    /*if(wheelsOn)
        shoot(1);
    else
        shoot(0);*/

    if (drive_control.getRawButton(5)) {
      wedge.moveDown();
      // servo1.set(1);
      // servo2.set(0);
      // wedge.set(Relay.Value.kForward);
    } else if (drive_control.getRawButton(6)) {
      wedge.moveUp();
      // servo1.set(0);
      // servo2.set(1);
      // wedge.set(Relay.Value.kReverse);
    } else {
      // servo1.set(.5);
      // servo2.set(.5);
      // wedge.set(Relay.Value.kOff);
    }

    if (drive_control.getRawButton(3)) turret.set(.5);
    else if (drive_control.getRawButton(4)) turret.set(-.5);
    else turret.set(0);

    if (drive_control.getRawButton(2)) driveMainBelt(true, true);
    else if (drive_control.getRawButton(7)) driveMainBelt(false, true);
    else driveMainBelt(false, false);

    if (drive_control.getTrigger()) {
      try {
        belt2.setX(-1);
      } catch (Exception e) {
      }
    } else if (drive_control.getRawButton(8)) {
      try {
        belt2.setX(1);
      } catch (Exception e) {
      }
    } else {
      try {
        belt2.setX(0);
      } catch (Exception e) {
      }
    }

    if (control.getRawButton(3)) {
      turret.set(.25);
    } else if (control.getRawButton(4)) {
      turret.set(-.25);
    } else {
      turret.set(0);
    }

    try {
      station.println(
          DriverStationLCD.Line.kMain6, 1, "RPM: " + String.valueOf(-launcher.getSpeed()));
    } catch (CANTimeoutException ex) {
    }
    /*try{
        if(tracking.foundTarget()){
            station.println(DriverStationLCD.Line.kUser2, 1, "Distance: " + String.valueOf(tracking.getTarget("HIGH").distance));
            station.println(DriverStationLCD.Line.kUser3, 1, "Suggested RPM: " + String.valueOf(tracking.getTarget("HIGH").distance*62.8));
            if(x>0)
                station.println(DriverStationLCD.Line.kUser4, 1, "Rotation: <--");
            else if(x<0)
                station.println(DriverStationLCD.Line.kUser4, 1, "Roation: -->");
            else
                station.println(DriverStationLCD.Line.kUser4, 1, "I don't need to rotate");
        }
        else{
            station.println(DriverStationLCD.Line.kUser2, 1, "Help, I can't find target by myself!");
            station.println(DriverStationLCD.Line.kUser3, 1, "");
            station.println(DriverStationLCD.Line.kUser4, 1, "");
        }
    }catch(Exception e){}*/
    station.updateLCD();
  }