Java AHRS类代码示例

import com.kauailabs.navx.frc.AHRS; //导入依赖的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(SerialPort.Port.kUSB);
	} catch (RuntimeException ex ) {
		DriverStation.reportError("Error instantiating navX MXP:  " + ex.getMessage(), true);
	}

}
 

import com.kauailabs.navx.frc.AHRS; //导入依赖的package包/类
private HardwareAdaptor(){
	pdp = new PowerDistributionPanel();
	comp = new Compressor();
	shifter = new DoubleSolenoid(SolenoidMap.SHIFTER_FORWARD, SolenoidMap.SHIFTER_REVERSE);
	
	navx = new AHRS(CoprocessorMap.NAVX_PORT);
	
	dtLeftEncoder = new Encoder(EncoderMap.DT_LEFT_A, EncoderMap.DT_LEFT_B, EncoderMap.DT_LEFT_INVERTED);
	S_DTLeft.linkEncoder(dtLeftEncoder);
	dtRightEncoder = new Encoder(EncoderMap.DT_RIGHT_A, EncoderMap.DT_RIGHT_B, EncoderMap.DT_RIGHT_INVERTED);
	S_DTRight.linkEncoder(dtRightEncoder);
	
	dtLeft = S_DTLeft.getInstance();
	dtRight = S_DTRight.getInstance();
	S_DTWhole.linkDTSides(dtLeft, dtRight);
	dtWhole = S_DTWhole.getInstance();
	
	arduino = S_Arduino.getInstance();
}
 

import com.kauailabs.navx.frc.AHRS; //导入依赖的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 com.kauailabs.navx.frc.AHRS; //导入依赖的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 com.kauailabs.navx.frc.AHRS; //导入依赖的package包/类
/**
 * Initialize all hardware
 */
void initHardware() {
	Logger.getInstance().logRobotThread("Init hardware");
	configureTalons(true);
	AHRS gyro = HardwareAdapter.getInstance().getDrivetrain().gyro;
	if (gyro != null) {
		int i = 0;
		while (!gyro.isConnected()) {
			i++;
			if (i > 1000) {
				System.out.println("waited for gyro to connect, didn't find");
				break;
			}
		}
	}
	gyro.zeroYaw();
}
 

import com.kauailabs.navx.frc.AHRS; //导入依赖的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 com.kauailabs.navx.frc.AHRS; //导入依赖的package包/类
public DriveSubsystem() {
	talonFrontLeft = new CANTalon(Ports.FRONT_LEFT_MOTOR);
	talonFrontRight = new CANTalon(Ports.FRONT_RIGHT_MOTOR);
	talonBackLeft = new Talon(Ports.BACK_LEFT_MOTOR);
	talonBackRight = new Talon(Ports.BACK_RIGHT_MOTOR);
	ultraSanic.setAutomaticMode(true);
	
	talonFrontLeft.setFeedbackDevice(FeedbackDevice.QuadEncoder);
	talonFrontRight.setFeedbackDevice(FeedbackDevice.QuadEncoder);
	talonFrontLeft.configEncoderCodesPerRev(PulsesPerRevolution);
	talonFrontRight.configEncoderCodesPerRev(PulsesPerRevolution);

	try {
		ahrs = new AHRS(Port.kMXP);
	} catch (Exception ex) {
		//System.out.println(ex);
	}
	// SPEED MODE CODE
	// setDriveControlMode(TalonControlMode.Speed);
	drivingStraight = false;
	driveLowerSpeed = false;
	reversed = true;
	enableForwardSoftLimit(false);
	enableReverseSoftLimit(false);
}
 

import com.kauailabs.navx.frc.AHRS; //导入依赖的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 com.kauailabs.navx.frc.AHRS; //导入依赖的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 com.kauailabs.navx.frc.AHRS; //导入依赖的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 com.kauailabs.navx.frc.AHRS; //导入依赖的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 com.kauailabs.navx.frc.AHRS; //导入依赖的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 com.kauailabs.navx.frc.AHRS; //导入依赖的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 com.kauailabs.navx.frc.AHRS; //导入依赖的package包/类
/**
 * This function is run when the robot is first started up and should be
 * used for any initialization code.
 */
public void robotInit()
{
    logFile = new File("/home/lvuser/log.txt");
    FileWriter fileWriter = null;
    try
    {
        logFile.createNewFile();
        fileWriter = new FileWriter(logFile);
    }
    catch(IOException e) { e.printStackTrace(); }
    LOG_OUTPUT = new PrintWriter(fileWriter == null ? new OutputStreamWriter(System.out) : fileWriter, true);

    Robot.write("tester");
    // System.out.println("writing tester");

    Log.createLogger(true);
    DRIVE_TRAIN = new DriveTrainSubsystem();
    CLIMBER = new ClimberSubsystem();
    NAVX = new AHRS(SPI.Port.kMXP);
    TRANSMISSION = new TransmissionSubsystem();
    COMPRESSOR = new Compressor();

    AutoSwitcher.putToSmartDashboard();

    SendableNavX.init();
    SendableDriveTrain.init();

    DashboardData.addUpdater(SendableDriveTrain.getInstance());
    DashboardData.addUpdater(SendableNavX.getInstance());

    OI.init();

    NAVX.resetDisplacement();
    // DashboardData.addUpdater(DRIVE_TRAIN);
}
 

import com.kauailabs.navx.frc.AHRS; //导入依赖的package包/类
public NavXGyroWrapper(PIDSource wrappedSource) throws IllegalSourceException {
	super(wrappedSource);
	if (wrappedSource.getClass() != AHRS.class) {
		throw new IllegalSourceException();
	}
	else {
		m_navxSource = ((AHRS) m_pidSource);
	}
}