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); } }
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); } }
/** * 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); }
/** * 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(); }
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"); }
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; } }
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); } }
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); } }
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); } }
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); }
/** * 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; } }
/** * 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)); }
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"); }
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); }
@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"; }
/** * Constructor. Uses the onboard CS0. */ public ADXRS453_Gyro() { this(SPI.Port.kOnboardCS0); }
/** * 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(); }
/** * 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); }
public RegisterIO_SPI( SPI spi_port ) { port = spi_port; bitrate = DEFAULT_SPI_BITRATE_HZ; }
public RegisterIO_SPI( SPI spi_port, int bitrate ) { port = spi_port; this.bitrate = bitrate; }
public Sensors() { // TODO Instantiate IMU and add appropriate variables imu = new AHRS(SPI.Port.kMXP); }
/** * Constructs the AHRS class using SPI communication, overriding the default * update rate with a custom rate which may be from 4 to 60, representing * the number of updates per second sent by the sensor. * <p> * This constructor should be used if communicating via SPI. * <p> * Note that increasing the update rate may increase the CPU utilization. * <p> * * @param spi_port_id * SPI Port to use * @param update_rate_hz * Custom Update Rate (Hz) */ public AHRS(SPI.Port spi_port_id, byte update_rate_hz) { commonInit(update_rate_hz); io = new RegisterIO(new RegisterIO_SPI(new SPI(spi_port_id)), update_rate_hz, io_complete_sink, board_capabilities); io_thread.start(); }
/** * The AHRS class provides an interface to AHRS capabilities of the * KauaiLabs navX Robotics Navigation Sensor via SPI, I2C and Serial (TTL * UART and USB) communications interfaces on the RoboRIO. * * The AHRS class enables access to basic connectivity and state * information, as well as key 6-axis and 9-axis orientation information * (yaw, pitch, roll, compass heading, fused (9-axis) heading and magnetic * disturbance detection. * * Additionally, the ARHS class also provides access to extended information * including linear acceleration, motion detection, rotation detection and * sensor temperature. * * If used with the navX Aero, the AHRS class also provides access to * altitude, barometric pressure and pressure sensor temperature data * * This constructor allows the specification of a custom SPI bitrate, in * bits/second. * * @param spi_port_id * SPI Port to use * @param spi_bitrate * SPI bitrate (Maximum: 2,000,000) * @param update_rate_hz * Custom Update Rate (Hz) */ public AHRS(SPI.Port spi_port_id, int spi_bitrate, byte update_rate_hz) { commonInit(update_rate_hz); io = new RegisterIO(new RegisterIO_SPI(new SPI(spi_port_id), spi_bitrate), update_rate_hz, io_complete_sink, board_capabilities); io_thread.start(); }
