We have been attempting to get our MaxBotics Ultrasonic Sensor to work. Whenever we attempt to deploy our code to the RoboRIO, we receive these errors on the Drive Station:
ERROR 1 ERROR Unhandled exception instantiating robot org.usfirst.frc.team498.robot.Robot java.lang.RuntimeException: Code: -1029. HAL: Resource already allocated at [edu.wpi.first.wpilibj.hal.AnalogJNI.initializeAnalogInputPort(Native Method), edu.wpi.first.wpilibj.AnalogInput.<init>(AnalogInput.java:53), org.usfirst.frc.team498.robot.AnalogUltrasonicSensor2017.<init>(AnalogUltrasonicSensor2017.java:9), org.usfirst.frc.team498.robot.Robot.<init>(Robot.java:39), sun.reflect.NativeConstructorAccessorImpl.newInstance0(Native Method), sun.reflect.NativeConstructorAccessorImpl.newInstance(NativeConstructorAccessorImpl.java:62), sun.reflect.DelegatingConstructorAccessorImpl.newInstance(DelegatingConstructorAccessorImpl.java:45), java.lang.reflect.Constructor.newInstance(Constructor.java:408), java.lang.Class.newInstance(Class.java:433), edu.wpi.first.wpilibj.RobotBase.main(RobotBase.java:216)] edu.wpi.first.wpilibj.RobotBase.main(RobotBase.java:218)
ERROR Unhandled exception instantiating robot org.usfirst.frc.team498.robot.Robot java.lang.RuntimeException: Code: -1029. HAL: Resource already allocated at [edu.wpi.first.wpilibj.hal.AnalogJNI.initializeAnalogInputPort(Native Method), edu.wpi.first.wpilibj.AnalogInput.<init>(AnalogInput.java:53), org.usfirst.frc.team498.robot.AnalogUltrasonicSensor2017.<init>(AnalogUltrasonicSensor2017.java:9), org.usfirst.frc.team498.robot.Robot.<init>(Robot.java:39), sun.reflect.NativeConstructorAccessorImpl.newInstance0(Native Method), sun.reflect.NativeConstructorAccessorImpl.newInstance(NativeConstructorAccessorImpl.java:62), sun.reflect.DelegatingConstructorAccessorImpl.newInstance(DelegatingConstructorAccessorImpl.java:45), java.lang.reflect.Constructor.newInstance(Constructor.java:408), java.lang.Class.newInstance(Class.java:433), edu.wpi.first.wpilibj.RobotBase.main(RobotBase.java:216)]
WARNING: Robots don't quit!
ERROR: Could not instantiate robot org.usfirst.frc.team498.robot.Robot!
The sensor is an analog sensor that is connected on port 0 of the PCM (Since it needs to be powered of 24v), but is also connected to the analog in port 3.
Here is our Robot.java class:
package org.usfirst.frc.team498.robot;
import java.util.HashMap;
import java.util.Map;
import edu.wpi.first.wpilibj.AnalogInput;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.I2C;
import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
import edu.wpi.first.wpilibj.I2C.Port;
import edu.wpi.first.wpilibj.PowerDistributionPanel;
import edu.wpi.first.wpilibj.SampleRobot;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Ultrasonic;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
//import edu.wpi.first.wpilibj.networktables.NetworkTable; *garbage*
public class Robot extends SampleRobot {
Command autonomousCommand;
/*
* //Network tables NetworkTable table; *garbage*
*/
// Drive
Ports ports = new Ports();
private Timer clock;
FancyJoystick thisStick = new FancyJoystick(0);
Drive2017 drive = new Drive2017(thisStick, ports);
REVImprovedDigitBoard digitBoard = new REVImprovedDigitBoard();
PewPew2017 shooter = new PewPew2017(digitBoard, thisStick, ports);
AutonmousController auto = new AutonmousController(drive, shooter, digitBoard, ports);
//GearIntake2017 gearIntake = new GearIntake2017(thisStick, ports);
//AnalogUltrasonicSensor2017 ultra = new AnalogUltrasonicSensor2017(ports);
PowerDistributionPanel pdp = new PowerDistributionPanel();
@Override
public void robotInit() {
// table = NetworkTable.getTable("datatable"); *garbage*
// CameraServer.getInstance().startAutomaticCapture();
/*
* new Thread(() -> { UsbCamera camera =
* CameraServer.getInstance().startAutomaticCapture();
* camera.setResolution(640, 480);
*
* CvSink cvSink = CameraServer.getInstance().getVideo(); CvSource
* outputStream = CameraServer.getInstance().putVideo("Blur", 640, 480);
*
* Mat source = new Mat(); Mat output = new Mat();
*
* while(true) { cvSink.grabFrame(source); Imgproc.cvtColor(source,
* output, Imgproc.COLOR_BGR2GRAY); outputStream.putFrame(output); }
* }).start(); }
*/
// 2 USB Cameras
/*
* frame = NIVision.imaqCreateImage(NIVision.ImageType.IMAGE_RGB, 0);
*
* sessionfront = NIVision.IMAQdxOpenCamera("cam1",
* NIVision.IMAQdxCameraControlMode.CameraControlModeController);
*
* sessionback = NIVision.IMAQdxOpenCamera("cam2",
* NIVision.IMAQdxCameraControlMode.CameraControlModeController);
*
* currSession = sessionfront;
*
* NIVision.IMAQdxConfigureGrab(currSession);
*/
}
// Select which autonomous to run
public void autonomous() {
// auto.autoInit(-1); // This autonomous method is copied from Unnamed
// Mark
// 4
}
int count = 0;
int count2 = 0;
public void operatorControl() {
// For Network table double x = 0; *garbage* double y = 0;
drive.moveValue = 0;
drive.turnValue = 0;
// auto.gyro.reset();
TeleOpMode teleMode = TeleOpMode.OPERATORCONTROL;
// digitBoard.display(2.0);
digitBoard.CreateScrollMsg("Randy Left Early ");
while (isOperatorControl() && isEnabled()) {
if (count % 1000 == 0) {
digitBoard.SlideScrollMsg();
}
count++;
while (isOperatorControl() && isEnabled()) {
if(thisStick.getButton(Button.Y)) { // 0.735 on smart dashboard is perfect!
shooter.Shoot();
} else {
shooter.StopShoot();
}
}
// network table
/*
* Timer.delay(0.25); table.putNumber("X", x); *garbage*
* table.putNumber("Y", y); x += 0.05; y +=1.0;
*/
// 2 camera code
/*
* if(button pressing code){ if(currSession == sessionfront){
* NIVision.IMAQdxStopAcquisition(currSession); currSession =
* sessionback; NIVision.IMAQdxConfigureGrab(currSession); } else
* if(currSession == sessionback){
* NIVision.IMAQdxStopAcquisition(currSession); currSession =
* sessionfront; NIVision.IMAQdxConfigureGrab(currSession); } }
*/
// Checks button
if (thisStick.getButton(Button.BACK) && thisStick.getButton(Button.B)) {
// TODO climbing feature
}
if (thisStick.getButton(Button.B)) {
teleMode = TeleOpMode.GEARALIGNMENT; // aligns robot to peg
}
if (thisStick.getButton(Button.START)) {
teleMode = TeleOpMode.OPERATORCONTROL; // makes robot go back to
// TeleOp
}
if (thisStick.getButton(Button.X)) {
teleMode = TeleOpMode.TEST; // Testing code
}
switch (teleMode) {
case OPERATORCONTROL:
// Drive the robot via controller
drive.rampedDriveListener();
//gearIntake.Listener();
// shooter.shootListener();
break;
case GEARALIGNMENT:
// teleMode = auto.AlignGearPeg();
break;
case HIGHGOALALIGNMENT:
// teleMode = auto.AlignHighGoal();
break;
case TEST:
// auto.testDrive();
drive.moveValue = 0;
drive.turnValue = 0;
break;
}
// Send stats to the driver
print();
}
}
public void disabled() {
while (isDisabled()) {
print();
if (digitBoard.getButtonA()) {
auto.autonomousSelector(); // Displays auto on digit board
}
}
}
// Sends information to the driver
private void print() {
// SmartDashboard.putNumber("Gyro Angle", auto.gyro.getAngle());
// SmartDashboard.putNumber("Gyro getRate()", auto.gyro.getRate());
// SmartDashboard.putNumber("Range (Inches)", ultra.GetRangeInches());
// SmartDashboard.putNumber("Ultrasonic Voltage", ultra.GetVoltage());
SmartDashboard.putNumber("Shooter value", digitBoard.getPot());
/*
* SmartDashboard.putNumber("Range millimeters (Analog)",
* auto.analogSensor.GetRangeMM());
* SmartDashboard.putNumber("Range Inches (Analog)",
* auto.analogSensor.GetRangeInches());
* SmartDashboard.putNumber("Voltage (Analog)",
* auto.analogSensor.GetVoltage());
*
* // These should print out GRIP's contour info into Dashboard
* SmartDashboard.putNumber("Contour1 CenterX",
* auto.vision.GetContour1CenterX());
* SmartDashboard.putNumber("Contour1 CenterY",
* auto.vision.GetContour1CenterY());
* SmartDashboard.putNumber("Contour1 Height",
* auto.vision.GetContour1Height());
* SmartDashboard.putNumber("Contour2 CenterX",
* auto.vision.GetContour2CenterX());
* SmartDashboard.putNumber("Contour2 CenterY",
* auto.vision.GetContour2CenterY());
* SmartDashboard.putNumber("Contour2 Height",
* auto.vision.GetContour2Height());
*
* SmartDashboard.putBoolean("flag", auto.vision.flag);
*
* // SmartDashboard.putNumber("Battery Voltage", pdp.getVoltage());
* //SmartDashboard.putNumber("Potentiometer Value",
* digitBoard.getPot()); SmartDashboard.putNumber("Move Value",
* drive.moveValue);
*
* //SmartDashboard.putBoolean("Button A", digitBoard.getButtonA());
* //SmartDashboard.putBoolean("Button B", digitBoard.getButtonB());
* SmartDashboard.putNumber("AutoMode", auto.autoMode);
* SmartDashboard.putString("Display", auto.display); //
* SmartDashboard.putNumber("A7", potMaybe.getVoltage());
*
* // digitBoard.display(pdp.getVoltage());
*
* // SmartDashboard.putNumber("Ramp Clock", //
* drive.forwardDriveRamp.clock.get());
*
* // 2 camera code /* 8NIVision.IMAQdxGrab(currSession, frame, 1);
* CameraServer.getInstance().setImage(frame);
*/
}
}
Here is our Port class:
package org.usfirst.frc.team498.robot;
public class Ports {
// Motor controllers
final int LEFT_FRONT_PWM_PORT = 2; // 8, 2
final int LEFT_BACK_PWM_PORT = 1; // 9, 1
final int RIGHT_FRONT_PWM_PORT = 4;// 6, 4
final int RIGHT_BACK_PWM_PORT = 3;// 7, 3
final int SHOOTER_PWM_PORT = 5;
final int GEAR_INTAKE_FORWARD_CHANNEL = 0;
final int GEAR_INTAKE_REVERSE_CHANNEL = 1;
// 1 is 9, 2 is 8, 3 is 7, 4 is 6
// We have two talons with CAN wires
// analog sensors
final int ULTRASONIC_SENSOR_ANALOG_PORT = 0; // TODO: change port to
// something other than 0 or
// 1 on robot
// ramp code
final double forwardRampIncreaseValue = 1.3;
final double reverseRampIncreaseValue = .1;
final double turningRampIncreaseValue = 1.3;
final double speedCap = .85;
}
Here is our AnalogUltrasonicSensor class:
package org.usfirst.frc.team498.robot;
import edu.wpi.first.wpilibj.AnalogInput;
public class AnalogUltrasonicSensor2017 {
AnalogInput AI;
public AnalogUltrasonicSensor2017(Ports ports) {
AI = new AnalogInput(ports.ULTRASONIC_SENSOR_ANALOG_PORT); // !!!MUY
// IMPORTANTE!!!
}
public double GetRangeMM() {
double output = 0;
output = AI.getVoltage();
output = output / 1000; // Measured in millivolts
output = output / 0.977;
return output;
}
public double GetRangeInches() {
double output = GetRangeMM();
output = output * 25.4;
return output;
}
public double GetVoltage() {
return AI.getVoltage();
}
}
Here is the AnalogInput class from the frc Referenced Libraries:
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008-2017. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import edu.wpi.first.wpilibj.hal.AnalogJNI;
import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType;
import edu.wpi.first.wpilibj.hal.HAL;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable;
import edu.wpi.first.wpilibj.tables.ITable;
import edu.wpi.first.wpilibj.util.AllocationException;
/**
* Analog channel class.
*
* <p>Each analog channel is read from hardware as a 12-bit number representing 0V to 5V.
*
* <p>Connected to each analog channel is an averaging and oversampling engine. This engine
* accumulates the specified ( by setAverageBits() and setOversampleBits() ) number of samples
* before returning a new value. This is not a sliding window average. The only difference between
* the oversampled samples and the averaged samples is that the oversampled samples are simply
* accumulated effectively increasing the resolution, while the averaged samples are divided by the
* number of samples to retain the resolution, but get more stable values.
*/
public class AnalogInput extends SensorBase implements PIDSource, LiveWindowSendable {
private static final int kAccumulatorSlot = 1;
int m_port; // explicit no modifier, private and package accessable.
private int m_channel;
private static final int] kAccumulatorChannels = {0, 1};
private long m_accumulatorOffset;
protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
/**
* Construct an analog channel.
*
* @param channel The channel number to represent. 0-3 are on-board 4-7 are on the MXP port.
*/
public AnalogInput(final int channel) {
m_channel = channel;
SensorBase.checkAnalogInputChannel(channel);
final int portHandle = AnalogJNI.getPort((byte) channel);
m_port = AnalogJNI.initializeAnalogInputPort(portHandle);
LiveWindow.addSensor("AnalogInput", channel, this);
HAL.report(tResourceType.kResourceType_AnalogChannel, channel);
}
/**
* Channel destructor.
*/
public void free() {
AnalogJNI.freeAnalogInputPort(m_port);
m_port = 0;
m_channel = 0;
m_accumulatorOffset = 0;
}
/**
* Get a sample straight from this channel. The sample is a 12-bit value representing the 0V to 5V
* range of the A/D converter. The units are in A/D converter codes. Use GetVoltage() to get the
* analog value in calibrated units.
*
* @return A sample straight from this channel.
*/
public int getValue() {
return AnalogJNI.getAnalogValue(m_port);
}
/**
* Get a sample from the output of the oversample and average engine for this channel. The sample
* is 12-bit + the bits configured in SetOversampleBits(). The value configured in
* setAverageBits() will cause this value to be averaged 2^bits number of samples. This is not a
* sliding window. The sample will not change until 2^(OversampleBits + AverageBits) samples have
* been acquired from this channel. Use getAverageVoltage() to get the analog value in calibrated
* units.
*
* @return A sample from the oversample and average engine for this channel.
*/
public int getAverageValue() {
return AnalogJNI.getAnalogAverageValue(m_port);
}
/**
* Get a scaled sample straight from this channel. The value is scaled to units of Volts using the
* calibrated scaling data from getLSBWeight() and getOffset().
*
* @return A scaled sample straight from this channel.
*/
public double getVoltage() {
return AnalogJNI.getAnalogVoltage(m_port);
}
/**
* Get a scaled sample from the output of the oversample and average engine for this channel. The
* value is scaled to units of Volts using the calibrated scaling data from getLSBWeight() and
* getOffset(). Using oversampling will cause this value to be higher resolution, but it will
* update more slowly. Using averaging will cause this value to be more stable, but it will update
* more slowly.
*
* @return A scaled sample from the output of the oversample and average engine for this channel.
*/
public double getAverageVoltage() {
return AnalogJNI.getAnalogAverageVoltage(m_port);
}
/**
* Get the factory scaling least significant bit weight constant. The least significant bit weight
* constant for the channel that was calibrated in manufacturing and stored in an eeprom.
*
* <p>Volts = ((LSB_Weight * 1e-9) * raw) - (Offset * 1e-9)
*
* @return Least significant bit weight.
*/
public long getLSBWeight() {
return AnalogJNI.getAnalogLSBWeight(m_port);
}
/**
* Get the factory scaling offset constant. The offset constant for the channel that was
* calibrated in manufacturing and stored in an eeprom.
*
* <p>Volts = ((LSB_Weight * 1e-9) * raw) - (Offset * 1e-9)
*
* @return Offset constant.
*/
public int getOffset() {
return AnalogJNI.getAnalogOffset(m_port);
}
/**
* Get the channel number.
*
* @return The channel number.
*/
public int getChannel() {
return m_channel;
}
/**
* Set the number of averaging bits. This sets the number of averaging bits. The actual number of
* averaged samples is 2^bits. The averaging is done automatically in the FPGA.
*
* @param bits The number of averaging bits.
*/
public void setAverageBits(final int bits) {
AnalogJNI.setAnalogAverageBits(m_port, bits);
}
/**
* Get the number of averaging bits. This gets the number of averaging bits from the FPGA. The
* actual number of averaged samples is 2^bits. The averaging is done automatically in the FPGA.
*
* @return The number of averaging bits.
*/
public int getAverageBits() {
return AnalogJNI.getAnalogAverageBits(m_port);
}
/**
* Set the number of oversample bits. This sets the number of oversample bits. The actual number
* of oversampled values is 2^bits. The oversampling is done automatically in the FPGA.
*
* @param bits The number of oversample bits.
*/
public void setOversampleBits(final int bits) {
AnalogJNI.setAnalogOversampleBits(m_port, bits);
}
/**
* Get the number of oversample bits. This gets the number of oversample bits from the FPGA. The
* actual number of oversampled values is 2^bits. The oversampling is done automatically in the
* FPGA.
*
* @return The number of oversample bits.
*/
public int getOversampleBits() {
return AnalogJNI.getAnalogOversampleBits(m_port);
}
/**
* Initialize the accumulator.
*/
public void initAccumulator() {
if (!isAccumulatorChannel()) {
throw new AllocationException("Accumulators are only available on slot " + kAccumulatorSlot
+ " on channels " + kAccumulatorChannels[0] + ", " + kAccumulatorChannels[1]);
}
m_accumulatorOffset = 0;
AnalogJNI.initAccumulator(m_port);
}
/**
* Set an initial value for the accumulator.
*
* <p>This will be added to all values returned to the user.
*
* @param initialValue The value that the accumulator should start from when reset.
*/
public void setAccumulatorInitialValue(long initialValue) {
m_accumulatorOffset = initialValue;
}
/**
* Resets the accumulator to the initial value.
*/
public void resetAccumulator() {
AnalogJNI.resetAccumulator(m_port);
// Wait until the next sample, so the next call to getAccumulator*()
// won't have old values.
final double sampleTime = 1.0 / getGlobalSampleRate();
final double overSamples = 1 << getOversampleBits();
final double averageSamples = 1 << getAverageBits();
Timer.delay(sampleTime * overSamples * averageSamples);
}
/**
* Set the center value of the accumulator.
*
* <p>The center value is subtracted from each A/D value before it is added to the accumulator.
* This is used for the center value of devices like gyros and accelerometers to take the device
* offset into account when integrating.
*
* <p>This center value is based on the output of the oversampled and averaged source the
* accumulator channel. Because of this, any non-zero oversample bits will affect the size of the
* value for this field.
*/
public void setAccumulatorCenter(int center) {
AnalogJNI.setAccumulatorCenter(m_port, center);
}
/**
* Set the accumulator's deadband.
*
* @param deadband The deadband size in ADC codes (12-bit value)
*/
public void setAccumulatorDeadband(int deadband) {
AnalogJNI.setAccumulatorDeadband(m_port, deadband);
}
/**
* Read the accumulated value.
*
* <p>Read the value that has been accumulating. The accumulator is attached after the oversample
* and average engine.
*
* @return The 64-bit value accumulated since the last Reset().
*/
public long getAccumulatorValue() {
return AnalogJNI.getAccumulatorValue(m_port) + m_accumulatorOffset;
}
/**
* Read the number of accumulated values.
*
* <p>Read the count of the accumulated values since the accumulator was last Reset().
*
* @return The number of times samples from the channel were accumulated.
*/
public long getAccumulatorCount() {
return AnalogJNI.getAccumulatorCount(m_port);
}
/**
* Read the accumulated value and the number of accumulated values atomically.
*
* <p>This function reads the value and count from the FPGA atomically. This can be used for
* averaging.
*
* @param result AccumulatorResult object to store the results in.
*/
public void getAccumulatorOutput(AccumulatorResult result) {
if (result == null) {
throw new IllegalArgumentException("Null parameter `result'");
}
if (!isAccumulatorChannel()) {
throw new IllegalArgumentException(
"Channel " + m_channel + " is not an accumulator channel.");
}
ByteBuffer value = ByteBuffer.allocateDirect(8);
// set the byte order
value.order(ByteOrder.LITTLE_ENDIAN);
ByteBuffer count = ByteBuffer.allocateDirect(8);
// set the byte order
count.order(ByteOrder.LITTLE_ENDIAN);
AnalogJNI.getAccumulatorOutput(m_port, value.asLongBuffer(), count.asLongBuffer());
result.value = value.asLongBuffer().get(0) + m_accumulatorOffset;
result.count = count.asLongBuffer().get(0);
}
/**
* Is the channel attached to an accumulator.
*
* @return The analog channel is attached to an accumulator.
*/
public boolean isAccumulatorChannel() {
for (int i = 0; i < kAccumulatorChannels.length; i++) {
if (m_channel == kAccumulatorChannels*) {
return true;
}
}
return false;
}
/**
* Set the sample rate per channel.
*
* <p>This is a global setting for all channels. The maximum rate is 500kS/s divided by the number
* of channels in use. This is 62500 samples/s per channel if all 8 channels are used.
*
* @param samplesPerSecond The number of samples per second.
*/
public static void setGlobalSampleRate(final double samplesPerSecond) {
AnalogJNI.setAnalogSampleRate(samplesPerSecond);
}
/**
* Get the current sample rate.
*
* <p>This assumes one entry in the scan list. This is a global setting for all channels.
*
* @return Sample rate.
*/
public static double getGlobalSampleRate() {
return AnalogJNI.getAnalogSampleRate();
}
@Override
public void setPIDSourceType(PIDSourceType pidSource) {
m_pidSource = pidSource;
}
@Override
public PIDSourceType getPIDSourceType() {
return m_pidSource;
}
/**
* Get the average voltage for use with PIDController.
*
* @return the average voltage
*/
@Override
public double pidGet() {
return getAverageVoltage();
}
/**
* Live Window code, only does anything if live window is activated.
*/
@Override
public String getSmartDashboardType() {
return "Analog Input";
}
private ITable m_table;
@Override
public void initTable(ITable subtable) {
m_table = subtable;
updateTable();
}
@Override
public void updateTable() {
if (m_table != null) {
m_table.putNumber("Value", getAverageVoltage());
}
}
@Override
public ITable getTable() {
return m_table;
}
/**
* Analog Channels don't have to do anything special when entering the LiveWindow. {@inheritDoc}
*/
@Override
public void startLiveWindowMode() {
}
/**
* Analog Channels don't have to do anything special when exiting the LiveWindow. {@inheritDoc}
*/
@Override
public void stopLiveWindowMode() {
}
}
So in conclusion, we’re having issues with getting the analog ports on our ultrasonic sensor. Any help is appreciated :)*