import edu.wpi.first.wpilibj.SPI; //导入依赖的package包/类
public NavX() {
     try {
/***********************************************************************
 * navX-MXP:
 * - Communication via RoboRIO MXP (SPI, I2C, TTL UART) and USB.            
 * - See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface.
 * 
 * navX-Micro:
 * - Communication via I2C (RoboRIO MXP or Onboard) and USB.
 * - See http://navx-micro.kauailabs.com/guidance/selecting-an-interface.
 * 
 * Multiple navX-model devices on a single robot are supported.
 ************************************************************************/
         ahrs = new AHRS(SPI.Port.kMXP); // Use SPI!!!
         //ahrs = new AHRS(I2C.Port.kMXP);
     } catch (RuntimeException ex ) {
         DriverStation.reportError("Error instantiating navX MXP:  " + ex.getMessage(), true);
     }
 }
 

import edu.wpi.first.wpilibj.SPI; //导入依赖的package包/类
public IMU() {
 	try {
/***********************************************************************
 * navX-MXP:
 * - Communication via RoboRIO MXP (SPI, I2C, TTL UART) and USB.            
 * - See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface.
 * 
 * navX-Micro:
 * - Communication via I2C (RoboRIO MXP or Onboard) and USB.
 * - See http://navx-micro.kauailabs.com/guidance/selecting-an-interface.
 * 
 * Multiple navX-model devices on a single robot are supported.
 ************************************************************************/
         ahrs = new AHRS(SPI.Port.kMXP);
     } catch (RuntimeException ex ) {
         DriverStation.reportError("Error instantiating navX MXP:  " + ex.getMessage(), true);
     }
 }
 

import edu.wpi.first.wpilibj.SPI; //导入依赖的package包/类
/**
 * Constructor.
 *
 * @param port
 *            (the SPI port that the gyro is connected to)
 */
public ADXRS453_Gyro(SPI.Port port) {
    m_spi = new SPI(port);
    m_spi.setClockRate(3000000);
    m_spi.setMSBFirst();
    m_spi.setSampleDataOnRising();
    m_spi.setClockActiveHigh();
    m_spi.setChipSelectActiveLow();

    /** Validate the part ID */
    if ((readRegister(kPIDRegister) & 0xff00) != 0x5200) {
        m_spi.free();
        m_spi = null;
        DriverStation.reportError("Could not find ADXRS453 gyro on SPI port " + port.value, false);
        return;
    }

    m_spi.initAccumulator(kSamplePeriod, 0x20000000, 4, 0x0c00000E, 0x04000000, 10, 16, true, true);

    calibrate();

    LiveWindow.addSensor("ADXRS453_Gyro", port.value, this);
}
 

import edu.wpi.first.wpilibj.SPI; //导入依赖的package包/类
/**
 * Constructor.
 *
 * @param port
 *            (the SPI port that the gyro is connected to)
 */
public ADXRS453_Gyro(SPI.Port port) {
    m_spi = new SPI(port);
    m_spi.setClockRate(3000000);
    m_spi.setMSBFirst();
    m_spi.setSampleDataOnRising();
    m_spi.setClockActiveHigh();
    m_spi.setChipSelectActiveLow();

    /** Validate the part ID */
    if ((readRegister(kPIDRegister) & 0xff00) != 0x5200) {
        m_spi.free();
        m_spi = null;
        DriverStation.reportError("Could not find ADXRS453 gyro on SPI port " + port.name(), false);
        return;
    }

    m_spi.initAccumulator(kSamplePeriod, 0x20000000, 4, 0x0c00000E, 0x04000000, 10, 16, true, true);

    calibrate();


}
 

import edu.wpi.first.wpilibj.SPI; //导入依赖的package包/类
public SpatialAwarenessSubsystem() {
  super("SpatialAwarenessSubsystem");

  cameraServer = CameraServer.getInstance();
  gearCamera = cameraServer.startAutomaticCapture(0);
  gearCamera.setResolution(IMG_WIDTH, IMG_HEIGHT);
  gearCamera.setFPS(30);
  gearCamera.setBrightness(7);
  gearCamera.setExposureManual(30);
  gearVideo = cameraServer.getVideo(gearCamera);

  visionProcessing = new VisionProcessing();

  leftUltrasonic = new AnalogInput(RobotMap.ANALOG_ULTRASONIC_LEFT);
  rightUltrasonic = new AnalogInput(RobotMap.ANALOG_ULTRASONIC_RIGHT);

  leftUltrasonicKalman = new SimpleKalman(1.4d, 64d, leftUltrasonic.getAverageVoltage() * ULTRASONIC_VOLTS_TO_METERS);
  rightUltrasonicKalman = new SimpleKalman(1.4d, 64d, rightUltrasonic.getAverageVoltage() * ULTRASONIC_VOLTS_TO_METERS);

  navX = new AHRS(SPI.Port.kMXP);

  System.out.println("SpatialAwarenessSubsystem constructor finished");
}
 

import edu.wpi.first.wpilibj.SPI; //导入依赖的package包/类
public static void initialize()
{
	if (!initialized) {
			
		System.out.println("NavXSensor::initialize() called...");
		
		try {
			ahrs = new AHRS(SPI.Port.kMXP);     
		} catch (RuntimeException ex ) {
            DriverStation.reportError("Error instantiating navX MXP:  " + ex.getMessage(), true);
        }

		reset();
		
		initialized = true;
	}
}
 

import edu.wpi.first.wpilibj.SPI; //导入依赖的package包/类
public Robot() {
     myRobot = new RobotDrive(0, 1);
     myRobot.setExpiration(0.1);
     stick = new Joystick(0);
     try {
/***********************************************************************
 * navX-MXP:
 * - Communication via RoboRIO MXP (SPI, I2C, TTL UART) and USB.            
 * - See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface.
 * 
 * navX-Micro:
 * - Communication via I2C (RoboRIO MXP or Onboard) and USB.
 * - See http://navx-micro.kauailabs.com/guidance/selecting-an-interface.
 * 
 * Multiple navX-model devices on a single robot are supported.
 ************************************************************************/
         ahrs = new AHRS(SPI.Port.kMXP); 
     } catch (RuntimeException ex ) {
         DriverStation.reportError("Error instantiating navX MXP:  " + ex.getMessage(), true);
     }
 }
 

import edu.wpi.first.wpilibj.SPI; //导入依赖的package包/类
public Robot() {
     myRobot = new RobotDrive(frontLeftChannel, rearLeftChannel,
     		frontRightChannel, rearRightChannel);
     myRobot.setExpiration(0.1);
     stick = new Joystick(0);
     try {
/***********************************************************************
 * navX-MXP:
 * - Communication via RoboRIO MXP (SPI, I2C, TTL UART) and USB.            
 * - See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface.
 * 
 * navX-Micro:
 * - Communication via I2C (RoboRIO MXP or Onboard) and USB.
 * - See http://navx-micro.kauailabs.com/guidance/selecting-an-interface.
 * 
 * Multiple navX-model devices on a single robot are supported.
 ************************************************************************/
         ahrs = new AHRS(SPI.Port.kMXP); 
     } catch (RuntimeException ex ) {
         DriverStation.reportError("Error instantiating navX MXP:  " + ex.getMessage(), true);
     }
 }
 

import edu.wpi.first.wpilibj.SPI; //导入依赖的package包/类
public Robot() {
    myRobot = new RobotDrive(frontLeftChannel, rearLeftChannel,
    		frontRightChannel, rearRightChannel);
    myRobot.setExpiration(0.1);
    stick = new Joystick(0);
    try {
/***********************************************************************
 * navX-MXP:
 * - Communication via RoboRIO MXP (SPI, I2C, TTL UART) and USB.            
 * - See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface.
 * 
 * navX-Micro:
 * - Communication via I2C (RoboRIO MXP or Onboard) and USB.
 * - See http://navx-micro.kauailabs.com/guidance/selecting-an-interface.
 * 
 * Multiple navX-model devices on a single robot are supported.
 ************************************************************************/
  	  ahrs = new AHRS(SPI.Port.kMXP); 
    } catch (RuntimeException ex ) {
        DriverStation.reportError("Error instantiating navX MXP:  " + ex.getMessage(), true);
    }
}
 

import edu.wpi.first.wpilibj.SPI; //导入依赖的package包/类
public Robot() {
    myRobot = new RobotDrive(frontLeftChannel, rearLeftChannel, 
		 frontRightChannel, rearRightChannel);
    myRobot.setExpiration(0.1);
    stick = new Joystick(0);
    try {
/***********************************************************************
 * navX-MXP:
 * - Communication via RoboRIO MXP (SPI, I2C, TTL UART) and USB.            
 * - See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface.
 * 
 * navX-Micro:
 * - Communication via I2C (RoboRIO MXP or Onboard) and USB.
 * - See http://navx-micro.kauailabs.com/guidance/selecting-an-interface.
 * 
 * Multiple navX-model devices on a single robot are supported.
 ************************************************************************/
        ahrs = new AHRS(SPI.Port.kMXP); 
    } catch (RuntimeException ex ) {
        DriverStation.reportError("Error instantiating navX MXP:  " + ex.getMessage(), true);
    }
}
 

import edu.wpi.first.wpilibj.SPI; //导入依赖的package包/类
public Robot() {
     stick = new Joystick(0);
     try {
/***********************************************************************
 * navX-MXP:
 * - Communication via RoboRIO MXP (SPI, I2C, TTL UART) and USB.            
 * - See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface.
 * 
 * navX-Micro:
 * - Communication via I2C (RoboRIO MXP or Onboard) and USB.
 * - See http://navx-micro.kauailabs.com/guidance/selecting-an-interface.
 * 
 * Multiple navX-model devices on a single robot are supported.
 ************************************************************************/
         ahrs = new AHRS(SPI.Port.kMXP);
     } catch (RuntimeException ex ) {
         DriverStation.reportError("Error instantiating navX MXP:  " + ex.getMessage(), true);
     }
 }
 

import edu.wpi.first.wpilibj.SPI; //导入依赖的package包/类
public void robotInit() {

	// Create subsystems
	drive = new Drive();
	intake = new Intake();
	arm = new Arm();
	shooter = new Shooter();
	hanger = new Hanger();

	oi = new OI();

	// Create motion profiles
	crossLowBar = new Profile(AutoMap.crossLowBar);
	reach = new Profile(AutoMap.reach);
	toShoot = new Profile(AutoMap.toShoot);
	toLowGoal = new Profile(AutoMap.toLowGoal);

	// Pick an auto
	chooser = new SendableChooser();
	chooser.addDefault("Do Nothing", new DoNothing());
	chooser.addObject("Low Bar", new LowBarRawtonomous());
	chooser.addObject("Low Bar (profile)", new CrossLowBar(crossLowBar));
	chooser.addObject("Reach", new Reach(reach));
	chooser.addObject("Forward Rawto", new ForwardRawtonomous());
	chooser.addObject("Backward Rawto", new BackwardRawtonomous());
	chooser.addObject("Shoot", new AutoShoot());
	SmartDashboard.putData("Auto mode", chooser);

	// Start camera stream
	camera = new USBCamera();
	server = CameraServer.getInstance();
	server.setQuality(40);
	server.startAutomaticCapture(camera);

	// Start compressor
	compressor = new Compressor();
	compressor.start();

	navx = new AHRS(SPI.Port.kMXP);
    }
 

import edu.wpi.first.wpilibj.SPI; //导入依赖的package包/类
/**
 * Default constructor.
 *
 * @param port      The port the NavX is plugged into. It seems like only kMXP (the port on the RIO) works.
 * @param invertYaw Whether or not to invert the yaw axis. Defaults to true.
 */
@JsonCreator
public MappedAHRS(@JsonProperty(required = true) SPI.Port port,
                  Boolean invertYaw) {
    this.ahrs = new AHRS(port);
    ahrs.reset();
    if (invertYaw == null || invertYaw) {
        this.invertYaw = -1;
    } else {
        this.invertYaw = 1;
    }
}
 

import edu.wpi.first.wpilibj.SPI; //导入依赖的package包/类
/**
 * Constructor for led strip class
 * 
 * @param numLEDs number of LED's in the total strip.
 */
public DotStarsLEDStrip(int numLEDs) {

    // Number of bytes in color buffer needed - each LED has 4 bytes (1 brightness, then 1 for
    // RGB each),
    // plus the start and end frame.
    num_leds = numLEDs;
    int num_bytes_for_strip = 4 * numLEDs + startFrame.length + endFrame.length;
    endFrameOffset = 4 * numLEDs + startFrame.length;

    // Initialize color buffer
    ledBuffer = new byte[num_bytes_for_strip];

    // Write in the start/end buffers
    for (int i = 0; i < startFrame.length; i++)
        ledBuffer[i] = startFrame[i];
    for (int i = 0; i < endFrame.length; i++)
        ledBuffer[i + endFrameOffset] = endFrame[i];

    // mark buffer as not-yet-written-to-the-LEDs
    newBuffer = true;

    // Initialize SPI coms on the Offboard port
    spi = new SPI(SPI.Port.kMXP);
    spi.setMSBFirst();
    spi.setClockActiveLow();
    spi.setClockRate(SPI_CLK_RATE);
    spi.setSampleDataOnFalling();
    timerThread = new java.util.Timer("DotStar LED Strip Control");
    timerThread.schedule(new DotStarsTask(this), (long) (m_update_period_ms), (long) (m_update_period_ms));

}
 

import edu.wpi.first.wpilibj.SPI; //导入依赖的package包/类
public Sensors() {
	ahrs = new AHRS(SPI.Port.kMXP);
	ahrs.reset();
	intakeLidar = new AnalogInput(RobotMap.INTAKE_LIDAR_PORT);
	longLidar = new AnalogInput(RobotMap.LONG_LIDAR_PORT);
	table = NetworkTable.getTable("SmartDashboard");
}
 

import edu.wpi.first.wpilibj.SPI; //导入依赖的package包/类
public Robot() {
     myRobot = new RobotDrive(frontLeftChannel, rearLeftChannel,
     		frontRightChannel, rearRightChannel);
     myRobot.setExpiration(0.1);
     stick = new Joystick(0);
     try {
/***********************************************************************
 * navX-MXP:
 * - Communication via RoboRIO MXP (SPI, I2C, TTL UART) and USB.            
 * - See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface.
 * 
 * navX-Micro:
 * - Communication via I2C (RoboRIO MXP or Onboard) and USB.
 * - See http://navx-micro.kauailabs.com/guidance/selecting-an-interface.
 * 
 * Multiple navX-model devices on a single robot are supported.
 ************************************************************************/
         ahrs = new AHRS(SPI.Port.kMXP); 
     } catch (RuntimeException ex ) {
         DriverStation.reportError("Error instantiating navX MXP:  " + ex.getMessage(), true);
     }
     turnController = new PIDController(kP, kI, kD, kF, ahrs, this);
     turnController.setInputRange(-180.0f,  180.0f);
     turnController.setOutputRange(-1.0, 1.0);
     turnController.setAbsoluteTolerance(kToleranceDegrees);
     turnController.setContinuous(true);
     
     /* Add the PID Controller to the Test-mode dashboard, allowing manual  */
     /* tuning of the Turn Controller's P, I and D coefficients.            */
     /* Typically, only the P value needs to be modified.                   */
     LiveWindow.addActuator("DriveSystem", "RotateController", turnController);
 }
 

import edu.wpi.first.wpilibj.SPI; //导入依赖的package包/类
@Override
public void robotInit() {

        chooser = new SendableChooser<Integer>();
        chooser.addDefault("center red", 1);
        chooser.addObject("center blue", 2);
        chooser.addObject("boiler red", 3);
        chooser.addObject("boiler blue", 4);
        chooser.addObject("ret red", 5);
        chooser.addObject("ret blue", 6);
        chooser.addObject("current test", 7);

        SmartDashboard.putData("Auto choices", chooser);

        //NETWORK TABLE VARIABLES
        table = NetworkTable.getTable("dataTable");

        //POWER DIST PANEL
        pdp = new PowerDistributionPanel();

        //NAVX
        navx = new AHRS(SPI.Port.kMXP);

        // CONTROLLER
        jsLeft = new Joystick(0);
        jsRight = new Joystick(1);
        xbox = new XboxController(5);

        // MOTORS
        leftFront = new Talon(pwm5);
        leftMid = new Talon(pwm3);
        leftBack = new Talon(pwm1);
        rightFront = new Talon(pwm4);
        rightMid = new Talon(pwm2);
        rightBack = new Talon(pwm0);

        // ENCODERS
        encLeftDrive = new Encoder(0,1);
        encRightDrive = new Encoder(2,3);

        // COMPRESSOR
        compressor = new Compressor();
        compressor.start();

        //SOLENOIDs
        driveChain = new DoubleSolenoid(0,1);
        driveChain.set(Value.kReverse);
        presser = new DoubleSolenoid(2,3);
        presser.set(Value.kReverse);
        gearpiston = new DoubleSolenoid(4,5);
        gearpiston.set(Value.kReverse);


        //CANTALONS
        climber = new CANTalon(2);
        shooter = new CANTalon(4);
        intake = new CANTalon(9);
        feeder = new CANTalon(13);

        // CAMERA
        //DON'T DELETE THE COMMENTED THREAD-IT IS USED FOR CALIBRATION
        /*
        UsbCamera usbCam = new UsbCamera("mscam", 0);
        usbCam.setVideoMode(VideoMode.PixelFormat.kMJPEG, 160, 120, 20);
        MjpegServer server1 = new MjpegServer("cam1", 1181);
        server1.setSource(usbCam);
        */
        
        UsbCamera camera = CameraServer.getInstance().startAutomaticCapture();
        camera.setVideoMode(VideoMode.PixelFormat.kMJPEG, 160, 120, 20);
        
        //SMARTDASBOARD
        driveSetting = "LOW START";
        gearSetting = "GEAR CLOSE START";
}
 

import edu.wpi.first.wpilibj.SPI; //导入依赖的package包/类
/**
 * Constructor.
 */
public ADIS16448_IMU() {
  m_spi = new SPI(SPI.Port.kMXP);
  m_spi.setClockRate(1000000);
  m_spi.setMSBFirst();
  m_spi.setSampleDataOnFalling();
  m_spi.setClockActiveLow();
  m_spi.setChipSelectActiveLow();

  readRegister(kRegPROD_ID); // dummy read
  
  // Validate the part ID
  if (readRegister(kRegPROD_ID) != 16448) {
    m_spi.free();
    m_spi = null;
    DriverStation.reportError("IMU is NOT Responding!!! Cannot Initalize!", false);
    return;
  }

  // Set IMU internal decimation to 102.4 SPS
  writeRegister(kRegSMPL_PRD, 769);

  // Enable Data Ready (LOW = Good Data) on DIO1 (PWM0 on MXP)
  writeRegister(kRegMSC_CTRL, 4);

  // Configure IMU internal Bartlett filter
  writeRegister(kRegSENS_AVG, 1030);

  m_cmd = ByteBuffer.allocateDirect(26);
  m_cmd.put(0, (byte) kGLOB_CMD);
  m_cmd.put(1, (byte) 0);
  m_resp = ByteBuffer.allocateDirect(26);
  m_resp.order(ByteOrder.BIG_ENDIAN);

  // Configure interrupt on MXP DIO0
  m_interrupt = new DigitalInput(10);  // MXP DIO0
  m_task = new Thread(new ReadTask(this));
  m_interrupt.requestInterrupts();
  m_interrupt.setUpSourceEdge(false, true);
  m_task.setDaemon(true);
  m_task.start();

  calibrate();
}
 

import edu.wpi.first.wpilibj.SPI; //导入依赖的package包/类
/**
 * Constructor.
 */
public ADIS16448_IMU() {
  m_spi = new SPI(SPI.Port.kMXP);
  m_spi.setClockRate(1000000);
  m_spi.setMSBFirst();
  m_spi.setSampleDataOnFalling();
  m_spi.setClockActiveLow();
  m_spi.setChipSelectActiveLow();

  System.out.println("ADIS16448_IMU.java");
  readRegister(kRegPROD_ID); // dummy read
  
  // Validate the part ID
  if (readRegister(kRegPROD_ID) != 16448) {
    m_spi.free();
    m_spi = null;
    DriverStation.reportError("could not find ADIS16448", false);
    return;
  }

  // Set IMU internal decimation to 102.4 SPS
  writeRegister(kRegSMPL_PRD, 769);

  // Enable Data Ready (LOW = Good Data) on DIO1 (PWM0 on MXP)
  writeRegister(kRegMSC_CTRL, 4);

  // Configure IMU internal Bartlett filter
  writeRegister(kRegSENS_AVG, 1030);

  m_cmd = ByteBuffer.allocateDirect(26);
  m_cmd.put(0, (byte) kGLOB_CMD);
  m_cmd.put(1, (byte) 0);
  m_resp = ByteBuffer.allocateDirect(26);
  m_resp.order(ByteOrder.BIG_ENDIAN);

  // Configure interrupt on MXP DIO0
  m_interrupt = new InterruptSource(10);  // MXP DIO0
  m_task = new Thread(new ReadTask(this));
  m_interrupt.requestInterrupts();
  m_interrupt.setUpSourceEdge(false, true);
  m_task.setDaemon(true);
  m_task.start();

  calibrate();

  //UsageReporting.report(tResourceType.kResourceType_ADIS16448, 0);
  LiveWindow.addSensor("ADIS16448_IMU", 0, this);
}
 

import edu.wpi.first.wpilibj.SPI; //导入依赖的package包/类
public RegisterIO_SPI( SPI spi_port ) {
    port = spi_port;
    bitrate = DEFAULT_SPI_BITRATE_HZ;
}