import edu.wpi.first.wpilibj.DriverStationLCD.Line; //导入依赖的package包/类
public static void log(String[] text) {
    for (int i = 0; i < text.length; i++) {
        if (text[i] == null || text[i].trim().equals("")) {
            continue;
        }
        Line line = lines[i % 6];
        int pos = 1;
        String out = text[i].trim();
        /*if (text.length > 6) {
            pos = i < 6 ? 1 : DriverStationLCD.kLineLength / 2;
            out = out.substring(0, (DriverStationLCD.kLineLength / 2) - 1); // Constrain
        }*/

        ds.println(line, pos, out);
    }

    update();
}
 

import edu.wpi.first.wpilibj.DriverStationLCD.Line; //导入依赖的package包/类
/**
 * This function is called periodically during operator control
 */
public void teleopPeriodic() {
    display.println(Line.kUser1, 1, "Lauch: " + launchercontroller.launcherswitch() + "      ");
    display.updateLCD();
    Scheduler.getInstance().run();
    if (alliance == 0) {
        arduino.setPattern("1");
        pattern = 1;
    } else if (alliance == 1){
        arduino.setPattern("2");
        pattern = 2;
    } else {
        arduino.setPattern("0");
        pattern = 0;
    }
}
 

import edu.wpi.first.wpilibj.DriverStationLCD.Line; //导入依赖的package包/类
public void disabledPeriodic() {
    display.println(Line.kUser1, 1, "Lauch: " + launchercontroller.launcherswitch() + "      ");
    display.updateLCD();
    digital1 = driverStation.getDigitalIn(1);
    digital2 = driverStation.getDigitalIn(2);
    digital3 = driverStation.getDigitalIn(3);
    alliance = driverStation.getAlliance().value;
    if (digital1 == true && digital2 == false && digital3 == false) {
        arduino.setPattern("4");
        pattern = 1;
    } else if (digital2 == true && digital1 == false && digital3 == false) {
        arduino.setPattern("5");
        pattern = 5;
    } else if (digital3 == true && digital1 == false && digital2 == false) {
        arduino.setPattern("6");
        pattern = 6;
    } else {
        arduino.setPattern("0");
        pattern = 0;
    }
    
}
 

import edu.wpi.first.wpilibj.DriverStationLCD.Line; //导入依赖的package包/类
/**
 * This method is called periodically during operator control.
 * <p/>
 * This method is called approximately 200 times a second.
 */
public void teleopPeriodic() {
    
    System.out.println("DIO 6: " + !RobotMap.armsExtendedFore.get() + ", DIO 7: " + !RobotMap.armsExtendedAft.get() + ", DIO 9: " + !RobotMap.armsHome.get());
    
    display.updateLCD();
    
    RobotMap.debug = DriverStation.getInstance().getDigitalIn(1);
    RobotMap.debugTable = DriverStation.getInstance().getDigitalIn(2);
    
    Scheduler.getInstance().run();
        
    display.println(Line.kUser1, 1, "Aspect Ratio: " + RobotMap.Top.aspect);
    display.println(Line.kUser2, 1, "CenX: " + RobotMap.Top.cenX);
    display.println(Line.kUser3, 1, "CenY: " + RobotMap.Top.cenY);
    display.println(Line.kUser4, 1, "Distance (in): " + RobotMap.Top.getRange());
    //display.println(Line.kUser5, 1, "Distance (ft): " + RobotMap.Top.getRange()/12);
    display.println(Line.kUser5, 1, "shoot RPM: " + RobotMap.shootEncoder.getRate()*60);
    display.println(Line.kUser6, 1, "shoot encoder: " + (((double)RobotMap.shootEncoder.get())/360));
    this.debugToTable("Encoder getrate", RobotMap.shootEncoder.getRate());
    this.debugToTable("Encoder get", RobotMap.shootEncoder.get());
    
    display.updateLCD();
}
 

import edu.wpi.first.wpilibj.DriverStationLCD.Line; //导入依赖的package包/类
/**
 * This function is called once each time the robot enters autonomous mode.
 */
public void autonomous() 
{
    reset();
    getWatchdog().setEnabled(false);
    
    m_LCD.println(Line.kUser6, LCDCol, "Driving");
    autoLoop(-.5, -.6, 1.75f);
    m_LCD.println(Line.kUser6, LCDCol, "Firing 1");
    m_Shooter.Fire();
    autoLoop(0, 0, 3.0f);
    m_LCD.println(Line.kUser6, LCDCol, "Firing 2");
    m_Shooter.Fire();
    autoLoop(0, 0, 3.0f);
    m_LCD.println(Line.kUser6, LCDCol, "Firing 3");
    m_Shooter.Fire();
    autoLoop(0, 0, 3.0f);
    m_LCD.println(Line.kUser6, LCDCol, "Turning Off");
    m_Shooter.TurnOff();
    autoLoop(0, 0, 0.25f);
    m_LCD.println(Line.kUser6, LCDCol, "           ");

}
 

import edu.wpi.first.wpilibj.DriverStationLCD.Line; //导入依赖的package包/类
/**
 *
 */
public void autonomousInit() {
    // schedule the autonomous command (example)
    // 
    autonomousCommand.start();
    //System.out.println("Entering Autonomous....");
    display.println(Line.kUser1, 1, "Autonomous");
    display.updateLCD();

}
 

import edu.wpi.first.wpilibj.DriverStationLCD.Line; //导入依赖的package包/类
/**
 *
 */
public void shiftLowGear() {
    GShiftSolUp.set(!RobotMap.DriveTrainLowGearSolenoidValue);
    GShiftSolDown.set(RobotMap.DriveTrainLowGearSolenoidValue);
    GShiftSolUp.set(false);
    GShiftSolDown.set(true);
    display.println(Line.kUser1, 1, "Into Low Gear ");
    display.updateLCD();
}
 

import edu.wpi.first.wpilibj.DriverStationLCD.Line; //导入依赖的package包/类
/**
 *
 */
public void shiftHighGear() {
    GShiftSolUp.set(!RobotMap.DriveTrainLowGearSolenoidValue);
    GShiftSolDown.set(RobotMap.DriveTrainLowGearSolenoidValue);
    GShiftSolUp.set(true);
    GShiftSolDown.set(false);

    display.println(Line.kUser1, 1, "Into High Gear ");
    display.updateLCD();
}
 

import edu.wpi.first.wpilibj.DriverStationLCD.Line; //导入依赖的package包/类
/**
 * This function is run when the robot is first started up and should be
 * used for any initialization code.
 */
public void robotInit() {
    // instantiate the command used for the autonomous period
    //status = new String();
    nt = NetworkTable.getTable("ST");
    debugTable = NetworkTable.getTable("Debug");
    nt.putString("Status", "Initializing");
    nt.putBoolean("AutoAlign", false);
    pusher =  new Pusher();
    DriveTrain = new DriveTrain(); // must be before Arms constructor
    arms = new Arms();
    VerticalAxis = new VerticalTurretAxis();   
    shooterController = new Shooter();
    display = DriverStationLCD.getInstance();
    OI.initialize();
    display.updateLCD();
    display.println(Line.kUser1, 1, "Initializing...                      ");
    display.println(Line.kUser2, 1, "                                     ");
    display.println(Line.kUser3, 1, "                                     ");
    display.println(Line.kUser4, 1, "                                     ");
    display.println(Line.kUser5, 1, "                                     ");
    display.println(Line.kUser6, 1, "                                     ");
    display.updateLCD();
    compressor = new Compressor(RobotMap.pressureSwitch, RobotMap.compressorRelay);
    compressor.start();
    RobotMap.shootEncoder.start();
    nt.putString("Status", "Initialized");
    tp = new TargetParser();
    
    updateSolenoids = new UpdateSolenoidModule();
    updateSolenoids.start();
    
    //shooterElevationPID = new ShooterElevationPID(RobotMap.shooterElevationKp, RobotMap.shooterElevationKi, RobotMap.shooterElevationKd);
    //shooterElevationPID.start();
    
    RobotMap.shootEncoder.setDistancePerPulse((1.0/360.0));
    
}
 

import edu.wpi.first.wpilibj.DriverStationLCD.Line; //导入依赖的package包/类
/**
 *  Call this to reset the robot.
 */
public void reset()
{
    //Pistons
    //if (m_ArmPistonOut) //If Arm is out..
    //{
    //    m_ArmSolIn.set(true); //pull it back in
    //    m_ArmSolOut.set(false);
    //}
    
    //Variables
    m_TrigRightWasDown  = false;
    //m_ArmPistonOut      = false;
    //m_XButWasDown       = false;
    //m_ArmPistonBringIn  = false;
    //m_BackButWasDown    = false;
    //m_XBackButWasDown   = false;
    
    m_YButWasDown       = false;

    m_FrisbeeMotorSpin  = false;
    //m_SetSpin           = 0;
    m_BButWasDown       = false;
    m_FrisbeeFired      = false;
    m_ShotsFired        = 0;
    
    //Strings
    //m_LCD.println(Line.kUser1, LCDCol, ArmPosBegin);
    //m_LCD.println(Line.kUser2, LCDCol, ArmDirBegin);
    m_LCD.println(Line.kUser4, LCDCol, FrisbeeBegin);
    m_LCD.println(Line.kUser5, LCDCol, ShotsBegin);
    m_LCD.println(Line.kUser6, LCDCol, "                   ");
    
    m_LCD.updateLCD();
}
 

import edu.wpi.first.wpilibj.DriverStationLCD.Line; //导入依赖的package包/类
/**
     *  Call this to use the Frisbee shooter.
     */
    public void frisbee()
    {     
        if (m_BButPressed)
        {
            if (m_FrisbeeMotorSpin)
            {
                m_FrisbeeMotorSpin = false;
                m_Shooter.TurnOff();
                //m_SetSpin = 0; //Stopping spin
            }
            else
            {
                m_FrisbeeMotorSpin = true;
                //m_Shooter.TurnOn();
                //m_SetSpin = 1; //Spinning full power
            }
        //m_FrisbeeMotor.set(m_SetSpin*-1); //Seting spin
        }
        
        if (m_Shooter.GetState() < Shooter.SHOOTER_ON)
        {
            m_LCD.println(Line.kUser4, LCDCol, NoSpinString);
        }
        else 
        {
            m_LCD.println(Line.kUser4, LCDCol, SpunUpString);
        }
        
        if (m_TrigRightPressed)
        {
            m_Shooter.Fire();
            m_ShotsFired++;
            m_LCD.println(Line.kUser5, LCDCol, m_ShotsFired + ShotsString);
//            if (!m_FrisbeeFired)
//            {
//                m_FrisbeeFired = true;
//                m_FrisbeeSolIn.set(true);
//                m_FrisbeeSolOut.set(false); //Pushing frisbee into firing motor
//                m_ShotsFired++;
//                m_LCD.println(Line.kUser5, LCDCol, m_ShotsFired + ShotsString);
//            }
//            else
//            {
//                m_FrisbeeFired = false;
//                m_FrisbeeSolIn.set(false);
//                m_FrisbeeSolOut.set(true); //Grabbing another frisbee
//            }
        }
    }