Permalink
Cannot retrieve contributors at this time
Fetching contributors…
| package org.usfirst.frc.team2465.robot; | |
| import edu.wpi.first.wpilibj.AnalogInput; | |
| import edu.wpi.first.wpilibj.AnalogOutput; | |
| import edu.wpi.first.wpilibj.AnalogTrigger; | |
| import edu.wpi.first.wpilibj.Counter; | |
| import edu.wpi.first.wpilibj.DigitalInput; | |
| import edu.wpi.first.wpilibj.DigitalOutput; | |
| import edu.wpi.first.wpilibj.DriverStation; | |
| import edu.wpi.first.wpilibj.Encoder; | |
| import edu.wpi.first.wpilibj.Jaguar; | |
| import edu.wpi.first.wpilibj.SampleRobot; | |
| import edu.wpi.first.wpilibj.Joystick; | |
| import edu.wpi.first.wpilibj.Timer; | |
| import edu.wpi.first.wpilibj.Victor; | |
| import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; | |
| /** | |
| * This example program demonstrates the use of the MXP | |
| * Expansion IO capabilities of the navX MXP, including | |
| * the following capabilities: | |
| * | |
| * -- DIGITAL I/O -- | |
| * | |
| * - Pulse-Width Modulation [PWM] (e.g., Motor Control) | |
| * - Digital Inputs (e.g., Contact Switch closure) | |
| * - Digital Outputs (e.g., Relay control) | |
| * - Quadrature Encoder (e.g., Wheel Encoder) | |
| * | |
| * -- ANALOG I/O -- | |
| * | |
| * - Analog Inputs (e.g., Ultrasonic Sensor) | |
| * - Analog Input Trigger (e.g., Proximity Sensor trigger) | |
| * - Analog Trigger Counter | |
| * - Analog Output (e.g., Constant-current LED, Sound) | |
| * | |
| * This example also demonstrates a simple method for calculating the | |
| * 'RoboRIO Channel Number' which corresponds to a given navX MXP IO | |
| * Pin number. | |
| * | |
| * For more details on navX MXP Expansion I/O, please see | |
| * http://navx-mxp.kauailabs.com/installation/io-expansion/ | |
| */ | |
| public class Robot extends SampleRobot { | |
| Joystick stick; | |
| /* Digital IO */ | |
| Victor pwm_out_9; /* E.g., PWM out to motor controller */ | |
| Jaguar pwm_out_8; /* E.g., PWM out to motor controller */ | |
| DigitalInput dig_in_7; /* E.g., input from contact switch */ | |
| DigitalInput dig_in_6; /* E.g., input from contact switch */ | |
| DigitalOutput dig_out_5; /* E.g., output to relay or LED */ | |
| DigitalOutput dig_out_4; /* E.g., output to relay or LED */ | |
| Encoder enc_3and2; /* E.g., Wheel Encoder */ | |
| Encoder enc_1and0; /* E.g., Wheel Encoder */ | |
| /* Analog Inputs */ | |
| AnalogInput an_in_1; /* E.g., Ultrasonic Sensor */ | |
| AnalogTrigger an_trig_0; /* E.g., Proximity Sensor Threshold */ | |
| Counter an_trig_0_counter; /* E.g., Count of an_trig_0 events */ | |
| /* Analog Outputs */ | |
| AnalogOutput an_out_1; /* E.g., Constant-current LED output */ | |
| AnalogOutput an_out_0; /* E.g., Speaker output */ | |
| public final double MXP_IO_VOLTAGE = 3.3f; /* Alternately, 5.0f */; | |
| public final double MIN_AN_TRIGGER_VOLTAGE = 0.76f; | |
| public final double MAX_AN_TRIGGER_VOLTAGE = MXP_IO_VOLTAGE - 2.0f; | |
| public Robot() { | |
| stick = new Joystick(0); | |
| try { | |
| /* Construct Digital I/O Objects */ | |
| pwm_out_9 = new Victor( getChannelFromPin( PinType.PWM, 9 )); | |
| pwm_out_8 = new Jaguar( getChannelFromPin( PinType.PWM, 8 )); | |
| dig_in_7 = new DigitalInput( getChannelFromPin( PinType.DigitalIO, 7 )); | |
| dig_in_6 = new DigitalInput( getChannelFromPin( PinType.DigitalIO, 6 )); | |
| dig_out_5 = new DigitalOutput( getChannelFromPin( PinType.DigitalIO, 5 )); | |
| dig_out_4 = new DigitalOutput( getChannelFromPin( PinType.DigitalIO, 4 )); | |
| enc_3and2 = new Encoder( getChannelFromPin( PinType.DigitalIO, 3 ), | |
| getChannelFromPin( PinType.DigitalIO, 2 )); | |
| enc_1and0 = new Encoder( getChannelFromPin( PinType.DigitalIO, 1 ), | |
| getChannelFromPin( PinType.DigitalIO, 0 )); | |
| /* Construct Analog I/O Objects */ | |
| /* NOTE: Due to a board layout issue on the navX MXP board revision */ | |
| /* 3.3, there is noticeable crosstalk between AN IN pins 3, 2 and 1. */ | |
| /* For that reason, use of pin 3 and pin 2 is NOT RECOMMENDED. */ | |
| an_in_1 = new AnalogInput( getChannelFromPin( PinType.AnalogIn, 1 )); | |
| an_trig_0 = new AnalogTrigger( getChannelFromPin( PinType.AnalogIn, 0 )); | |
| an_trig_0_counter = new Counter( an_trig_0 ); | |
| an_out_1 = new AnalogOutput( getChannelFromPin( PinType.AnalogOut, 1 )); | |
| an_out_0 = new AnalogOutput( getChannelFromPin( PinType.AnalogOut, 0 )); | |
| /* Configure I/O Objects */ | |
| pwm_out_9.setSafetyEnabled(false); /* Disable Motor Safety for Demo */ | |
| pwm_out_8.setSafetyEnabled(false); /* Disable Motor Safety for Demo */ | |
| /* Configure Analog Trigger */ | |
| an_trig_0.setAveraged(true); | |
| an_trig_0.setLimitsVoltage(MIN_AN_TRIGGER_VOLTAGE, MAX_AN_TRIGGER_VOLTAGE); | |
| } catch (RuntimeException ex ) { | |
| DriverStation.reportError( "Error instantiating MXP pin on navX MXP: " | |
| + ex.getMessage(), true); | |
| } | |
| } | |
| /** | |
| * Wait for 2 seconds then stop | |
| */ | |
| public void autonomous() { | |
| Timer.delay(2.0); // for 2 seconds | |
| } | |
| public enum PinType { DigitalIO, PWM, AnalogIn, AnalogOut }; | |
| public final int MAX_NAVX_MXP_DIGIO_PIN_NUMBER = 9; | |
| public final int MAX_NAVX_MXP_ANALOGIN_PIN_NUMBER = 3; | |
| public final int MAX_NAVX_MXP_ANALOGOUT_PIN_NUMBER = 1; | |
| public final int NUM_ROBORIO_ONBOARD_DIGIO_PINS = 10; | |
| public final int NUM_ROBORIO_ONBOARD_PWM_PINS = 10; | |
| public final int NUM_ROBORIO_ONBOARD_ANALOGIN_PINS = 4; | |
| /* getChannelFromPin( PinType, int ) - converts from a navX MXP */ | |
| /* Pin type and number to the corresponding RoboRIO Channel */ | |
| /* Number, which is used by the WPI Library functions. */ | |
| public int getChannelFromPin( PinType type, int io_pin_number ) | |
| throws IllegalArgumentException { | |
| int roborio_channel = 0; | |
| if ( io_pin_number < 0 ) { | |
| throw new IllegalArgumentException("Error: navX MXP I/O Pin #"); | |
| } | |
| switch ( type ) { | |
| case DigitalIO: | |
| if ( io_pin_number > MAX_NAVX_MXP_DIGIO_PIN_NUMBER ) { | |
| throw new IllegalArgumentException("Error: Invalid navX MXP Digital I/O Pin #"); | |
| } | |
| roborio_channel = io_pin_number + NUM_ROBORIO_ONBOARD_DIGIO_PINS + | |
| (io_pin_number > 3 ? 4 : 0); | |
| break; | |
| case PWM: | |
| if ( io_pin_number > MAX_NAVX_MXP_DIGIO_PIN_NUMBER ) { | |
| throw new IllegalArgumentException("Error: Invalid navX MXP Digital I/O Pin #"); | |
| } | |
| roborio_channel = io_pin_number + NUM_ROBORIO_ONBOARD_PWM_PINS; | |
| break; | |
| case AnalogIn: | |
| if ( io_pin_number > MAX_NAVX_MXP_ANALOGIN_PIN_NUMBER ) { | |
| throw new IllegalArgumentException("Error: Invalid navX MXP Analog Input Pin #"); | |
| } | |
| roborio_channel = io_pin_number + NUM_ROBORIO_ONBOARD_ANALOGIN_PINS; | |
| break; | |
| case AnalogOut: | |
| if ( io_pin_number > MAX_NAVX_MXP_ANALOGOUT_PIN_NUMBER ) { | |
| throw new IllegalArgumentException("Error: Invalid navX MXP Analog Output Pin #"); | |
| } | |
| roborio_channel = io_pin_number; | |
| break; | |
| } | |
| return roborio_channel; | |
| } | |
| /** | |
| * Runs the motors with arcade steering. | |
| */ | |
| public void operatorControl() { | |
| while (isOperatorControl() && isEnabled()) { | |
| /* Process Digital I/O (Outputs controlled by joystick) */ | |
| pwm_out_9.set(stick.getX()); | |
| pwm_out_8.set(stick.getY()); | |
| SmartDashboard.putBoolean( "DigIn7", dig_in_7.get()); | |
| SmartDashboard.putBoolean( "DigIn6", dig_in_6.get()); | |
| dig_out_5.set(stick.getRawButton(1)); | |
| dig_out_4.set(stick.getRawButton(2)); | |
| SmartDashboard.putNumber( "Enc3and2", enc_3and2.get()); | |
| SmartDashboard.putNumber( "Enc1and0", enc_1and0.get()); | |
| /* Process Analog Inputs */ | |
| SmartDashboard.putNumber( "AnalogIn1", an_in_1.getAverageVoltage()); | |
| SmartDashboard.putBoolean( "AnalogTrigger0", an_trig_0.getTriggerState()); | |
| SmartDashboard.putNumber( "AnalogTriggerCounter0", an_trig_0_counter.get()); | |
| /* Process Analog Outputs (Outputs controlled by joystick) */ | |
| an_out_1.setVoltage(Math.abs(stick.getX()) * MXP_IO_VOLTAGE); | |
| an_out_0.setVoltage(Math.abs(stick.getY()) * MXP_IO_VOLTAGE); | |
| Timer.delay(0.005); /* wait for a motor update time */ | |
| } | |
| } | |
| /** | |
| * Runs during test mode | |
| */ | |
| public void test() { | |
| } | |
| } |