SCDS/FtcTemplate
7
0
Fork 0
mirror of https://github.com/trc492/FtcTemplate.git synced 2025-07-01 13:01:24 -07:00
Code Issues Packages Projects Releases Wiki Activity

Updated FtcLib.

Browse Source 
Added OctoQuad support.
This commit is contained in:
Titan Robotics Club
2024-06-12 18:33:45 -07:00
parent eb18f391e1
commit a1d134b3f0
5 changed files with 1318 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

81
TeamCode/src/main/java/com/qualcomm/hardware/digitalchickenlabs/CachingOctoQuad.java Normal file
View File

@ -0,0 +1,81 @@
/*
* Copyright (c) 2022 DigitalChickenLabs
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package com.qualcomm.hardware.digitalchickenlabs;
public interface CachingOctoQuad extends OctoQuadBase
{
/**
* Controls how data is cached to reduce the number
* of Lynx transactions needed to read the encoder data
*/
enum CachingMode
{
/*
* No caching at all
*/
NONE,
/**
* The cache is only updated when you call {@link #setCachingMode(CachingMode)}
*/
MANUAL,
/**
* The cache is updated the 2nd time you call {@link #readSinglePosition_Caching(int)} (int)}
* or {@link #readSingleVelocity_Caching(int)} (int)} for the same encoder index.
*/
AUTO
}
/**
* Set the data caching mode for the OctoQuad
* @param mode mode to use
*/
void setCachingMode(CachingMode mode);
/**
* Manually refresh the position and velocity data cache
*/
void refreshCache();
/**
* Read a single position from the OctoQuad
* Note this call may return cached data based on
* the {@link CachingMode} you selected!
* Depending on the channel bank configuration, this may
* either be quadrature step count, or pulse width.
* @param idx the index of the encoder to read
* @return the position for the specified encoder
*/
int readSinglePosition_Caching(int idx);
/**
* Read a single velocity from the OctoQuad
* Note this call may return cached data based on
* the {@link CachingMode} you selected!
* NOTE: if using an absolute pulse width encoder, in order to get sane
* velocity data, you must set the channel min/max pulse width parameter.
* @param idx the index of the encoder to read
* @return the velocity for the specified encoder
*/
short readSingleVelocity_Caching(int idx);
}

225
TeamCode/src/main/java/com/qualcomm/hardware/digitalchickenlabs/OctoQuad.java Normal file
View File

@ -0,0 +1,225 @@
/*
* Copyright (c) 2022 DigitalChickenLabs
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package com.qualcomm.hardware.digitalchickenlabs;
public interface OctoQuad extends OctoQuadBase
{
/**
* Class to represent an OctoQuad firmware version
*/
class FirmwareVersion
{
public final int maj;
public final int min;
public final int eng;
public FirmwareVersion(int maj, int min, int eng)
{
this.maj = maj;
this.min = min;
this.eng = eng;
}
@Override
public String toString()
{
return String.format("%d.%d.%d", maj, min, eng);
}
}
/**
* Get the firmware version running on the OctoQuad
* @return the firmware version running on the OctoQuad
*/
FirmwareVersion getFirmwareVersion();
/**
* Read a single position from the OctoQuad
* Depending on the channel bank configuration, this may
* either be quadrature step count, or pulse width.
* @param idx the index of the encoder to read
* @return the position for the specified encoder
*/
int readSinglePosition(int idx);
/**
* Reads all positions from the OctoQuad, writing the data into
* an existing int[] object. The previous values are destroyed.
* Depending on the channel bank configuration, this may
* either be quadrature step count, or pulse width.
* @param out the int[] object to fill with new data
*/
void readAllPositions(int[] out);
/**
* Reads all positions from the OctoQuad
* Depending on the channel bank configuration, this may
* either be quadrature step count, or pulse width.
* @return an int[] object with the new data
*/
int[] readAllPositions();
/**
* Read a selected range of encoders
* Depending on the channel bank configuration, this may
* either be quadrature step count, or pulse width.
* @param idxFirst the first encoder (inclusive)
* @param idxLast the last encoder (inclusive)
* @return an array containing the requested encoder positions
*/
int[] readPositionRange(int idxFirst, int idxLast);
/*
* More Reset methods in Base interface
*/
/**
* Reset multiple encoders in the OctoQuad firmware in one command
* @param resets the encoders to be reset
*/
void resetMultiplePositions(boolean[] resets);
/**
* Reset multiple encoders in the OctoQuad firmware in one command
* @param indices the indices of the encoders to reset
*/
void resetMultiplePositions(int... indices);
/*
* More direction methods in Base interface
*/
/**
* Set the direction for all encoders
* This parameter will NOT be retained across power cycles, unless
* you call {@link #saveParametersToFlash()} ()}
* @param reverse 8-length direction array
*/
void setAllEncoderDirections(boolean[] reverse);
/**
* Read a single velocity from the OctoQuad
* NOTE: if using an absolute pulse width encoder, in order to get sane
* velocity data, you must set the channel min/max pulse width parameter.
* @param idx the index of the encoder to read
* @return the velocity for the specified encoder
*/
short readSingleVelocity(int idx);
/**
* Reads all velocities from the OctoQuad, writing the data into
* an existing short[] object. The previous values are destroyed.
* NOTE: if using an absolute pulse width encoder, in order to get sane
* velocity data, you must set the channel min/max pulse width parameter.
* @param out the short[] object to fill with new data
*/
void readAllVelocities(short[] out);
/**
* Reads all velocities from the OctoQuad
* NOTE: if using an absolute pulse width encoder, in order to get sane
* velocity data, you must set the channel min/max pulse width parameter.
* @return a short[] object with the new data
*/
short[] readAllVelocities();
/**
* Read a selected range of encoder velocities
* NOTE: if using an absolute pulse width encoder, in order to get sane
* velocity data, you must set the channel min/max pulse width parameter.
* @param idxFirst the first encoder (inclusive)
* @param idxLast the last encoder (inclusive)
* @return an array containing the requested velocities
*/
short[] readVelocityRange(int idxFirst, int idxLast);
class EncoderDataBlock
{
public int[] positions = new int[NUM_ENCODERS];
public short[] velocities = new short[NUM_ENCODERS];
}
/**
* Reads all encoder data from the OctoQuad, writing the data into
* an existing {@link EncoderDataBlock} object. The previous values are destroyed.
* @param out the {@link EncoderDataBlock} object to fill with new data
*/
void readAllEncoderData(EncoderDataBlock out);
/**
* Reads all encoder data from the OctoQuad
* @return a {@link EncoderDataBlock} object with the new data
*/
EncoderDataBlock readAllEncoderData();
/*
* More velocity sample interval methods in base interface
*/
/**
* Set the velocity sample intervals for all encoders
* This parameter will NOT be retained across power cycles, unless
* you call {@link #saveParametersToFlash()} ()}
* @param intvlms the sample intervals in milliseconds
*/
void setAllVelocitySampleIntervals(int[] intvlms);
/**
* Reads all velocity sample intervals from the OctoQuad
* @return all velocity sample intervals from the OctoQuad
*/
int[] getAllVelocitySampleIntervals();
class ChannelPulseWidthParams
{
public int min_length_us;
public int max_length_us;
public ChannelPulseWidthParams() {};
public ChannelPulseWidthParams(int min_length_us, int max_length_us)
{
this.min_length_us = min_length_us;
this.max_length_us = max_length_us;
}
}
/**
* Configure the minimum/maximum pulse width reported by an absolute encoder
* which is connected to a given channel, to allow the ability to provide
* accurate velocity data.
* These parameters will NOT be retained across power cycles, unless
* you call {@link #saveParametersToFlash()} ()}
* @param idx the channel in question
* @param params minimum/maximum pulse width
*/
void setSingleChannelPulseWidthParams(int idx, ChannelPulseWidthParams params);
/**
* Queries the OctoQuad to determine the currently set minimum/maxiumum pulse
* width for an encoder channel, to allow sane velocity data.
* @param idx the channel in question
* @return minimum/maximum pulse width
*/
ChannelPulseWidthParams getSingleChannelPulseWidthParams(int idx);
}

207
TeamCode/src/main/java/com/qualcomm/hardware/digitalchickenlabs/OctoQuadBase.java Normal file
View File

@ -0,0 +1,207 @@
/*
* Copyright (c) 2022 DigitalChickenLabs
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package com.qualcomm.hardware.digitalchickenlabs;
public interface OctoQuadBase
{
byte OCTOQUAD_CHIP_ID = 0x51;
int SUPPORTED_FW_VERSION_MAJ = 2;
int ENCODER_FIRST = 0;
int ENCODER_LAST = 7;
int NUM_ENCODERS = 8;
int MIN_VELOCITY_MEASUREMENT_INTERVAL_MS = 1;
int MAX_VELOCITY_MEASUREMENT_INTERVAL_MS = 255;
int MIN_PULSE_WIDTH_LENGTH_µS = 1;
int MAX_PULSE_WIDTH_LENGTH_µS = 0xFFFF;
/**
* Reads the CHIP_ID register of the OctoQuad
* @return the value in the CHIP_ID register of the OctoQuad
*/
byte getChipId();
/**
* Get the firmware version running on the OctoQuad
* @return the firmware version running on the OctoQuad
*/
String getFirmwareVersionString();
/**
* Reset a single encoder in the OctoQuad firmware
* @param idx the index of the encoder to reset
*/
void resetSinglePosition(int idx);
/**
* Reset all encoder counts in the OctoQuad firmware
*/
void resetAllPositions();
enum EncoderDirection
{
FORWARD,
REVERSE
}
/**
* Set the direction for a single encoder
* This parameter will NOT be retained across power cycles, unless
* you call {@link #saveParametersToFlash()} ()}
* @param idx the index of the encoder
* @param dir direction
*/
void setSingleEncoderDirection(int idx, EncoderDirection dir);
/**
* Get the direction for a single encoder
* @param idx the index of the encoder
* @return direction of the encoder in question
*/
EncoderDirection getSingleEncoderDirection(int idx);
/**
* Set the velocity sample interval for a single encoder
* This parameter will NOT be retained across power cycles, unless
* you call {@link #saveParametersToFlash()} ()}
* @param idx the index of the encoder in question
* @param intvlms the sample interval in milliseconds
*/
void setSingleVelocitySampleInterval(int idx, int intvlms);
/**
* Set the velocity sample interval for all encoders
* This parameter will NOT be retained across power cycles, unless
* you call {@link #saveParametersToFlash()} ()}
* @param intvlms the sample interval in milliseconds
*/
void setAllVelocitySampleIntervals(int intvlms);
/**
* Read a single velocity sample interval
* @param idx the index of the encoder in question
* @return the velocity sample interval
*/
int getSingleVelocitySampleInterval(int idx);
/**
* Configure the minimum/maximum pulse width reported by an absolute encoder
* which is connected to a given channel, to allow the ability to provide
* accurate velocity data.
* These parameters will NOT be retained across power cycles, unless
* you call {@link #saveParametersToFlash()} ()}
* @param idx the channel in question
* @param min_length_us minimum pulse width
* @param max_length_us maximum pulse width
*/
void setSingleChannelPulseWidthParams(int idx, int min_length_us, int max_length_us);
/**
* Run the firmware's internal reset routine
*/
void resetEverything();
enum ChannelBankConfig
{
/**
* Both channel banks are configured for Quadrature input
*/
ALL_QUADRATURE(0),
/**
* Both channel banks are configured for pulse width input
*/
ALL_PULSE_WIDTH(1),
/**
* Bank 1 (channels 0-3) is configured for Quadrature input;
* Bank 2 (channels 4-7) is configured for pulse width input.
*/
BANK1_QUADRATURE_BANK2_PULSE_WIDTH(2);
public byte bVal;
ChannelBankConfig(int bVal)
{
this.bVal = (byte) bVal;
}
}
/**
* Configures the OctoQuad's channel banks
* This parameter will NOT be retained across power cycles, unless
* you call {@link #saveParametersToFlash()} ()}
* @param config the channel bank configuration to use
*/
void setChannelBankConfig(ChannelBankConfig config);
/**
* Queries the OctoQuad to determine the current channel bank configuration
* @return the current channel bank configuration
*/
ChannelBankConfig getChannelBankConfig();
enum I2cRecoveryMode
{
/**
* Does not perform any active attempts to recover a wedged I2C bus
*/
NONE(0),
/**
* The OctoQuad will reset its I2C peripheral if 50ms elapses between
* byte transmissions or between bytes and start/stop conditions
*/
MODE_1_PERIPH_RST_ON_FRAME_ERR(1),
/**
* Mode 1 actions + the OctoQuad will toggle the clock line briefly,
* once, after 1500ms of no communications.
*/
MODE_2_M1_PLUS_SCL_IDLE_ONESHOT_TGL(2);
public byte bVal;
I2cRecoveryMode(int bVal)
{
this.bVal = (byte) bVal;
}
}
/**
* Configures the OctoQuad to use the specified I2C recovery mode.
* This parameter will NOT be retained across power cycles, unless
* you call {@link #saveParametersToFlash()} ()}
* @param mode the recovery mode to use
*/
void setI2cRecoveryMode(I2cRecoveryMode mode);
/**
* Queries the OctoQuad to determine the currently configured I2C recovery mode
* @return the currently configured I2C recovery mode
*/
I2cRecoveryMode getI2cRecoveryMode();
/**
* Stores the current state of parameters to flash, to be applied at next boot
*/
void saveParametersToFlash();
}

804
TeamCode/src/main/java/com/qualcomm/hardware/digitalchickenlabs/OctoQuadImpl.java Normal file
View File

@ -0,0 +1,804 @@
/*
* Copyright (c) 2022 DigitalChickenLabs
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package com.qualcomm.hardware.digitalchickenlabs;
import com.qualcomm.hardware.lynx.LynxI2cDeviceSynch;
import com.qualcomm.robotcore.hardware.I2cAddr;
import com.qualcomm.robotcore.hardware.I2cDeviceSynch;
import com.qualcomm.robotcore.hardware.I2cDeviceSynchDevice;
import com.qualcomm.robotcore.hardware.configuration.annotations.DeviceProperties;
import com.qualcomm.robotcore.hardware.configuration.annotations.I2cDeviceType;
import com.qualcomm.robotcore.util.Range;
import com.qualcomm.robotcore.util.RobotLog;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.util.Arrays;
@I2cDeviceType
@DeviceProperties(xmlTag = "OctoQuadFTC", name = "OctoQuadFTC")
public class OctoQuadImpl extends I2cDeviceSynchDevice<I2cDeviceSynch> implements OctoQuad, CachingOctoQuad
{
private static final int I2C_ADDRESS = 0x30;
private static final ByteOrder OCTOQUAD_ENDIAN = ByteOrder.LITTLE_ENDIAN;
private static final byte CMD_SET_PARAM = 1;
private static final byte CMD_READ_PARAM = 2;
private static final byte CMD_WRITE_PARAMS_TO_FLASH = 3;
private static final byte CMD_RESET_EVERYTHING = 20;
private static final byte CMD_RESET_ENCODERS = 21;
private static final byte PARAM_ENCODER_DIRECTIONS = 0;
private static final byte PARAM_I2C_RECOVERY_MODE = 1;
private static final byte PARAM_CHANNEL_BANK_CONFIG = 2;
private static final byte PARAM_CHANNEL_VEL_INTVL = 3;
private static final byte PARAM_CHANNEL_PULSE_WIDTH_MIN_MAX = 4;
private byte directionRegisterData = 0;
private boolean isInitialized = false;
public class OctoQuadException extends RuntimeException
{
public OctoQuadException(String msg)
{
super(msg);
}
}
public OctoQuadImpl(I2cDeviceSynch deviceClient)
{
super(deviceClient, true);
this.deviceClient.setI2cAddress(I2cAddr.create7bit(I2C_ADDRESS));
super.registerArmingStateCallback(false);
this.deviceClient.engage();
}
@Override
protected boolean doInitialize()
{
//((LynxI2cDeviceSynch) deviceClient).setBusSpeed(LynxI2cDeviceSynch.BusSpeed.FAST_400K);
isInitialized = false;
verifyInitialization();
return true;
}
@Override
public Manufacturer getManufacturer()
{
return Manufacturer.Other;
}
@Override
public String getDeviceName()
{
return "OctoQuadFTC";
}
enum RegisterType
{
uint8_t(1),
int32_t(4),
int16_t(2);
public final int length;
RegisterType(int length)
{
this.length = length;
}
}
enum Register
{
CHIP_ID (0x00, RegisterType.uint8_t),
FIRMWARE_VERSION_MAJOR (0x01, RegisterType.uint8_t),
FIRMWARE_VERSION_MINOR (0x02, RegisterType.uint8_t),
FIRMWARE_VERSION_ENGINEERING (0x03, RegisterType.uint8_t),
COMMAND (0x04, RegisterType.uint8_t),
COMMAND_DAT_0 (0x05, RegisterType.uint8_t),
COMMAND_DAT_1 (0x06, RegisterType.uint8_t),
COMMAND_DAT_2 (0x07, RegisterType.uint8_t),
COMMAND_DAT_3 (0x08, RegisterType.uint8_t),
COMMAND_DAT_4 (0x09, RegisterType.uint8_t),
COMMAND_DAT_5 (0x0A, RegisterType.uint8_t),
COMMAND_DAT_6 (0x0B, RegisterType.uint8_t),
ENCODER_0_POSITION (0x0C, RegisterType.int32_t),
ENCODER_1_POSITION (0x10, RegisterType.int32_t),
ENCODER_2_POSITION (0x14, RegisterType.int32_t),
ENCODER_3_POSITION (0x18, RegisterType.int32_t),
ENCODER_4_POSITION (0x1C, RegisterType.int32_t),
ENCODER_5_POSITION (0x20, RegisterType.int32_t),
ENCODER_6_POSITION (0x24, RegisterType.int32_t),
ENCODER_7_POSITION (0x28, RegisterType.int32_t),
ENCODER_0_VELOCITY (0x2C, RegisterType.int16_t),
ENCODER_1_VELOCITY (0x2E, RegisterType.int16_t),
ENCODER_2_VELOCITY (0x30, RegisterType.int16_t),
ENCODER_3_VELOCITY (0x32, RegisterType.int16_t),
ENCODER_4_VELOCITY (0x34, RegisterType.int16_t),
ENCODER_5_VELOCITY (0x36, RegisterType.int16_t),
ENCODER_6_VELOCITY (0x38, RegisterType.int16_t),
ENCODER_7_VELOCITY (0x3A, RegisterType.int16_t);
public final byte addr;
public final int length;
Register(int addr, RegisterType type)
{
this.addr = (byte) addr;
this.length = type.length;
}
public static final Register[] all = Register.values();
}
// --------------------------------------------------------------------------------------------------------------------------------
// PUBLIC OCTOQUAD API
//---------------------------------------------------------------------------------------------------------------------------------
public byte getChipId()
{
return readRegister(Register.CHIP_ID)[0];
}
public FirmwareVersion getFirmwareVersion()
{
byte[] fw = readContiguousRegisters(Register.FIRMWARE_VERSION_MAJOR, Register.FIRMWARE_VERSION_ENGINEERING);
int maj = fw[0] & 0xFF;
int min = fw[1] & 0xFF;
int eng = fw[2] & 0xFF;
return new FirmwareVersion(maj, min, eng);
}
public String getFirmwareVersionString()
{
return getFirmwareVersion().toString();
}
public int readSinglePosition(int idx)
{
verifyInitialization();
Range.throwIfRangeIsInvalid(idx, ENCODER_FIRST, ENCODER_LAST);
Register register = Register.all[Register.ENCODER_0_POSITION.ordinal()+idx];
return intFromBytes(readRegister(register));
}
public void readAllPositions(int[] out)
{
verifyInitialization();
if(out.length != NUM_ENCODERS)
{
throw new IllegalArgumentException("out.length != 8");
}
byte[] bytes = readContiguousRegisters(Register.ENCODER_0_POSITION, Register.ENCODER_7_POSITION);
ByteBuffer buffer = ByteBuffer.wrap(bytes);
buffer.order(OCTOQUAD_ENDIAN);
for(int i = 0; i < NUM_ENCODERS; i++)
{
out[i] = buffer.getInt();
}
}
public int[] readAllPositions()
{
verifyInitialization();
int[] block = new int[NUM_ENCODERS];
readAllPositions(block);
return block;
}
public int[] readPositionRange(int idxFirst, int idxLast)
{
verifyInitialization();
Range.throwIfRangeIsInvalid(idxFirst, ENCODER_FIRST, ENCODER_LAST);
Range.throwIfRangeIsInvalid(idxLast, ENCODER_FIRST, ENCODER_LAST);
Register registerFirst = Register.all[Register.ENCODER_0_POSITION.ordinal()+idxFirst];
Register registerLast = Register.all[Register.ENCODER_0_POSITION.ordinal()+idxLast];
byte[] data = readContiguousRegisters(registerFirst, registerLast);
ByteBuffer buffer = ByteBuffer.wrap(data);
buffer.order(ByteOrder.LITTLE_ENDIAN);
int numEncodersRead = idxLast-idxFirst+1;
int[] encoderCounts = new int[numEncodersRead];
for(int i = 0; i < numEncodersRead; i++)
{
encoderCounts[i] = buffer.getInt();
}
return encoderCounts;
}
// ALSO USED BY CACHING API !
public void resetSinglePosition(int idx)
{
verifyInitialization();
Range.throwIfRangeIsInvalid(idx, ENCODER_FIRST, ENCODER_LAST);
byte dat = (byte) (1 << idx);
writeContiguousRegisters(Register.COMMAND, Register.COMMAND_DAT_0, new byte[]{CMD_RESET_ENCODERS, dat});
if (cachingMode == CachingMode.AUTO)
{
refreshCache();
}
}
// ALSO USED BY CACHING API !
public void resetAllPositions()
{
verifyInitialization();
writeContiguousRegisters(Register.COMMAND, Register.COMMAND_DAT_0, new byte[] {CMD_RESET_ENCODERS, (byte)0xFF});
if (cachingMode == CachingMode.AUTO)
{
refreshCache();
}
}
public void resetMultiplePositions(boolean[] resets)
{
verifyInitialization();
if(resets.length != NUM_ENCODERS)
{
throw new IllegalArgumentException("resets.length != 8");
}
byte dat = 0;
for(int i = ENCODER_FIRST; i <= ENCODER_LAST; i++)
{
dat |= resets[i] ? (byte)(1 << i) : 0;
}
writeContiguousRegisters(Register.COMMAND, Register.COMMAND_DAT_0, new byte[] {CMD_RESET_ENCODERS, dat});
}
public void resetMultiplePositions(int... indices)
{
verifyInitialization();
for(int idx : indices)
{
Range.throwIfRangeIsInvalid(idx, ENCODER_FIRST, ENCODER_LAST);
}
byte dat = 0;
for(int idx : indices)
{
dat |= 1 << idx;
}
writeContiguousRegisters(Register.COMMAND, Register.COMMAND_DAT_0, new byte[] {CMD_RESET_ENCODERS, dat});
}
public void setSingleEncoderDirection(int idx, EncoderDirection direction)
{
verifyInitialization();
Range.throwIfRangeIsInvalid(idx, ENCODER_FIRST, ENCODER_LAST);
if(direction == EncoderDirection.REVERSE)
{
directionRegisterData |= (byte) (1 << idx);
}
else
{
directionRegisterData &= (byte) ~(1 << idx);
}
writeContiguousRegisters(Register.COMMAND, Register.COMMAND_DAT_1, new byte[]{CMD_SET_PARAM, PARAM_ENCODER_DIRECTIONS, directionRegisterData});
}
public EncoderDirection getSingleEncoderDirection(int idx)
{
verifyInitialization();
Range.throwIfRangeIsInvalid(idx, ENCODER_FIRST, ENCODER_LAST);
writeContiguousRegisters(Register.COMMAND, Register.COMMAND_DAT_0, new byte[]{CMD_READ_PARAM, PARAM_ENCODER_DIRECTIONS});
byte directions = readRegister(Register.COMMAND_DAT_0)[0];
boolean reversed = (directions & (1 << idx)) != 0;
return reversed ? EncoderDirection.REVERSE : EncoderDirection.FORWARD;
}
public void setAllEncoderDirections(boolean[] reverse)
{
verifyInitialization();
if(reverse.length != NUM_ENCODERS)
{
throw new IllegalArgumentException("reverse.length != 8");
}
directionRegisterData = 0;
for(int i = ENCODER_FIRST; i <= ENCODER_LAST; i++)
{
if(reverse[i])
{
directionRegisterData |= (byte) (1 << i);
}
}
writeContiguousRegisters(Register.COMMAND, Register.COMMAND_DAT_1, new byte[]{CMD_SET_PARAM, PARAM_ENCODER_DIRECTIONS, directionRegisterData});
}
public short readSingleVelocity(int idx)
{
verifyInitialization();
Range.throwIfRangeIsInvalid(idx, ENCODER_FIRST, ENCODER_LAST);
Register register = Register.all[Register.ENCODER_0_VELOCITY.ordinal()+idx];
return shortFromBytes(readRegister(register));
}
public void readAllVelocities(short[] out)
{
verifyInitialization();
if(out.length != NUM_ENCODERS)
{
throw new IllegalArgumentException("out.length != 8");
}
byte[] bytes = readContiguousRegisters(Register.ENCODER_0_VELOCITY, Register.ENCODER_7_VELOCITY);
ByteBuffer buffer = ByteBuffer.wrap(bytes);
buffer.order(OCTOQUAD_ENDIAN);
for(int i = 0; i < NUM_ENCODERS; i++)
{
out[i] = buffer.getShort();
}
}
public short[] readAllVelocities()
{
verifyInitialization();
short[] block = new short[NUM_ENCODERS];
readAllVelocities(block);
return block;
}
public short[] readVelocityRange(int idxFirst, int idxLast)
{
verifyInitialization();
Range.throwIfRangeIsInvalid(idxFirst, ENCODER_FIRST, ENCODER_LAST);
Range.throwIfRangeIsInvalid(idxLast, ENCODER_FIRST, ENCODER_LAST);
Register registerFirst = Register.all[Register.ENCODER_0_VELOCITY.ordinal()+idxFirst];
Register registerLast = Register.all[Register.ENCODER_0_VELOCITY.ordinal()+idxLast];
byte[] data = readContiguousRegisters(registerFirst, registerLast);
ByteBuffer buffer = ByteBuffer.wrap(data);
buffer.order(ByteOrder.LITTLE_ENDIAN);
int numVelocitiesRead = idxLast-idxFirst+1;
short[] velocities = new short[numVelocitiesRead];
for(int i = 0; i < numVelocitiesRead; i++)
{
velocities[i] = buffer.getShort();
}
return velocities;
}
public void readAllEncoderData(EncoderDataBlock out)
{
verifyInitialization();
if(out.positions.length != NUM_ENCODERS)
{
throw new IllegalArgumentException("out.counts.length != 8");
}
if(out.velocities.length != NUM_ENCODERS)
{
throw new IllegalArgumentException("out.velocities.length != 8");
}
byte[] bytes = readContiguousRegisters(Register.ENCODER_0_POSITION, Register.ENCODER_7_VELOCITY);
ByteBuffer buffer = ByteBuffer.wrap(bytes);
buffer.order(OCTOQUAD_ENDIAN);
for(int i = 0; i < NUM_ENCODERS; i++)
{
out.positions[i] = buffer.getInt();
}
for(int i = 0; i < NUM_ENCODERS; i++)
{
out.velocities[i] = buffer.getShort();
}
}
public EncoderDataBlock readAllEncoderData()
{
verifyInitialization();
EncoderDataBlock block = new EncoderDataBlock();
readAllEncoderData(block);
return block;
}
public void setSingleVelocitySampleInterval(int idx, int intvlms)
{
verifyInitialization();
Range.throwIfRangeIsInvalid(idx, ENCODER_FIRST, ENCODER_LAST);
Range.throwIfRangeIsInvalid(intvlms, MIN_VELOCITY_MEASUREMENT_INTERVAL_MS, MAX_VELOCITY_MEASUREMENT_INTERVAL_MS);
writeContiguousRegisters(Register.COMMAND, Register.COMMAND_DAT_2, new byte[]{CMD_SET_PARAM, PARAM_CHANNEL_VEL_INTVL, (byte)idx, (byte)intvlms});
}
public void setAllVelocitySampleIntervals(int intvlms)
{
verifyInitialization();
Range.throwIfRangeIsInvalid(intvlms, MIN_VELOCITY_MEASUREMENT_INTERVAL_MS, MAX_VELOCITY_MEASUREMENT_INTERVAL_MS);
for(int i = ENCODER_FIRST; i <= ENCODER_LAST; i++)
{
writeContiguousRegisters(Register.COMMAND, Register.COMMAND_DAT_2, new byte[]{CMD_SET_PARAM, PARAM_CHANNEL_VEL_INTVL, (byte)i, (byte)intvlms});
}
}
public void setAllVelocitySampleIntervals(int[] intvlms)
{
verifyInitialization();
if(intvlms.length != NUM_ENCODERS)
{
throw new IllegalArgumentException("intvls.length != 8");
}
for(int i : intvlms)
{
Range.throwIfRangeIsInvalid(i, MIN_VELOCITY_MEASUREMENT_INTERVAL_MS, MAX_VELOCITY_MEASUREMENT_INTERVAL_MS);
}
for(int i = ENCODER_FIRST; i <= ENCODER_LAST; i++)
{
writeContiguousRegisters(Register.COMMAND, Register.COMMAND_DAT_2, new byte[]{CMD_SET_PARAM, PARAM_CHANNEL_VEL_INTVL, (byte)i, (byte)intvlms[i]});
}
}
public int getSingleVelocitySampleInterval(int idx)
{
verifyInitialization();
Range.throwIfRangeIsInvalid(idx, ENCODER_FIRST, ENCODER_LAST);
writeContiguousRegisters(Register.COMMAND, Register.COMMAND_DAT_1, new byte[]{CMD_READ_PARAM, PARAM_CHANNEL_VEL_INTVL, (byte)idx});
byte ms = readRegister(Register.COMMAND_DAT_0)[0];
return ms & 0xFF;
}
public int[] getAllVelocitySampleIntervals()
{
verifyInitialization();
int[] ret = new int[NUM_ENCODERS];
for(int i = ENCODER_FIRST; i <= ENCODER_FIRST; i++)
{
writeContiguousRegisters(Register.COMMAND, Register.COMMAND_DAT_1, new byte[]{CMD_READ_PARAM, PARAM_CHANNEL_VEL_INTVL, (byte)i});
byte ms = readRegister(Register.COMMAND_DAT_0)[0];
ret[i] = ms & 0xFF;
}
return ret;
}
public void setSingleChannelPulseWidthParams(int idx, int min, int max)
{
setSingleChannelPulseWidthParams(idx, new ChannelPulseWidthParams(min, max));
}
public void setSingleChannelPulseWidthParams(int idx, ChannelPulseWidthParams params)
{
verifyInitialization();
Range.throwIfRangeIsInvalid(idx, ENCODER_FIRST, ENCODER_LAST);
Range.throwIfRangeIsInvalid(params.min_length_us, MIN_PULSE_WIDTH_LENGTH_µS, MAX_PULSE_WIDTH_LENGTH_µS);
Range.throwIfRangeIsInvalid(params.max_length_us, MIN_PULSE_WIDTH_LENGTH_µS, MAX_PULSE_WIDTH_LENGTH_µS);
if(params.max_length_us <= params.min_length_us)
{
throw new RuntimeException("params.max_length_us <= params.min_length_us");
}
ByteBuffer outgoing = ByteBuffer.allocate(7);
outgoing.order(OCTOQUAD_ENDIAN);
outgoing.put(CMD_SET_PARAM);
outgoing.put(PARAM_CHANNEL_PULSE_WIDTH_MIN_MAX);
outgoing.put((byte)idx);
outgoing.putShort((short)params.min_length_us);
outgoing.putShort((short)params.max_length_us);
writeContiguousRegisters(Register.COMMAND, Register.COMMAND_DAT_5, outgoing.array());
}
public ChannelPulseWidthParams getSingleChannelPulseWidthParams(int idx)
{
verifyInitialization();
Range.throwIfRangeIsInvalid(idx, ENCODER_FIRST, ENCODER_LAST);
writeContiguousRegisters(Register.COMMAND, Register.COMMAND_DAT_1, new byte[]{CMD_READ_PARAM, PARAM_CHANNEL_PULSE_WIDTH_MIN_MAX, (byte)idx});
byte[] result = readContiguousRegisters(Register.COMMAND_DAT_0, Register.COMMAND_DAT_3);
ByteBuffer buffer = ByteBuffer.wrap(result);
buffer.order(OCTOQUAD_ENDIAN);
ChannelPulseWidthParams params = new ChannelPulseWidthParams();
params.min_length_us = buffer.getShort() & 0xFFFF;
params.max_length_us = buffer.getShort() & 0xFFFF;
return params;
}
public void resetEverything()
{
verifyInitialization();
writeRegister(Register.COMMAND, new byte[]{CMD_RESET_EVERYTHING});
}
public void setChannelBankConfig(ChannelBankConfig config)
{
verifyInitialization();
writeContiguousRegisters(Register.COMMAND, Register.COMMAND_DAT_1, new byte[]{CMD_SET_PARAM, PARAM_CHANNEL_BANK_CONFIG, config.bVal});
}
public ChannelBankConfig getChannelBankConfig()
{
verifyInitialization();
writeContiguousRegisters(Register.COMMAND, Register.COMMAND_DAT_0, new byte[]{CMD_READ_PARAM, PARAM_CHANNEL_BANK_CONFIG});
byte result = readRegister(Register.COMMAND_DAT_0)[0];
for(ChannelBankConfig c : ChannelBankConfig.values())
{
if(c.bVal == result)
{
return c;
}
}
return ChannelBankConfig.ALL_QUADRATURE;
}
public void setI2cRecoveryMode(I2cRecoveryMode mode)
{
verifyInitialization();
writeContiguousRegisters(Register.COMMAND, Register.COMMAND_DAT_1, new byte[]{CMD_SET_PARAM, PARAM_I2C_RECOVERY_MODE, mode.bVal});
}
public I2cRecoveryMode getI2cRecoveryMode()
{
verifyInitialization();
writeContiguousRegisters(Register.COMMAND, Register.COMMAND_DAT_0, new byte[]{CMD_READ_PARAM, PARAM_I2C_RECOVERY_MODE});
byte result = readRegister(Register.COMMAND_DAT_0)[0];
for(I2cRecoveryMode m : I2cRecoveryMode.values())
{
if(m.bVal == result)
{
return m;
}
}
return I2cRecoveryMode.NONE;
}
public void saveParametersToFlash()
{
verifyInitialization();
writeRegister(Register.COMMAND, new byte[] {CMD_WRITE_PARAMS_TO_FLASH});
try
{
Thread.sleep(100);
}
catch (InterruptedException e)
{
Thread.currentThread().interrupt();
e.printStackTrace();
}
}
// --------------------------------------------------------------------------------------------------------------------------------
// PUBLIC CACHING OCTOQUAD API
//---------------------------------------------------------------------------------------------------------------------------------
protected CachingMode cachingMode = CachingMode.AUTO;
protected EncoderDataBlock cachedData = new EncoderDataBlock();
protected boolean[] posHasBeenRead = new boolean[NUM_ENCODERS];
protected boolean[] velHasBeenRead = new boolean[NUM_ENCODERS];
public void setCachingMode(CachingMode mode)
{
this.cachingMode = mode;
if (cachingMode != CachingMode.NONE)
{
refreshCache();
}
}
public void refreshCache()
{
readAllEncoderData(cachedData);
Arrays.fill(posHasBeenRead, false);
Arrays.fill(velHasBeenRead, false);
}
public int readSinglePosition_Caching(int idx)
{
// If we're caching we're gonna want to read from the cache
if (cachingMode == CachingMode.AUTO || cachingMode == CachingMode.MANUAL)
{
// Update cache if this is the 2nd read
if (cachingMode == CachingMode.AUTO && posHasBeenRead[idx])
{
refreshCache();
}
posHasBeenRead[idx] = true;
return cachedData.positions[idx];
}
// Not caching; read direct
else
{
return readSinglePosition(idx);
}
}
public short readSingleVelocity_Caching(int idx)
{
// If we're caching we're gonna want to read from the cache
if (cachingMode == CachingMode.AUTO || cachingMode == CachingMode.MANUAL)
{
// Update cache if this is the 2nd read
if (cachingMode == CachingMode.AUTO && velHasBeenRead[idx])
{
refreshCache();
}
velHasBeenRead[idx] = true;
return cachedData.velocities[idx];
}
// Not caching; read direct
else
{
return readSingleVelocity(idx);
}
}
// --------------------------------------------------------------------------------------------------------------------------------
// INTERNAL
//---------------------------------------------------------------------------------------------------------------------------------
private void verifyInitialization()
{
if(!isInitialized)
{
byte chipId = getChipId();
if(chipId != OCTOQUAD_CHIP_ID)
{
RobotLog.addGlobalWarningMessage("OctoQuad does not report correct CHIP_ID value; (got 0x%X; expected 0x%X) this likely indicates I2C comms are not working", chipId, OCTOQUAD_CHIP_ID);
}
FirmwareVersion fw = getFirmwareVersion();
if(fw.maj != SUPPORTED_FW_VERSION_MAJ)
{
RobotLog.addGlobalWarningMessage("OctoQuad is running a different major firmware version than this driver was built for (current=%d; expected=%d) IT IS HIGHLY LIKELY THAT NOTHING WILL WORK!", fw.maj, SUPPORTED_FW_VERSION_MAJ);
}
isInitialized = true;
}
}
private static int intFromBytes(byte[] bytes)
{
ByteBuffer byteBuffer = ByteBuffer.wrap(bytes);
byteBuffer.order(OCTOQUAD_ENDIAN);
return byteBuffer.getInt();
}
private static short shortFromBytes(byte[] bytes)
{
ByteBuffer byteBuffer = ByteBuffer.wrap(bytes);
byteBuffer.order(OCTOQUAD_ENDIAN);
return byteBuffer.getShort();
}
private byte[] readRegister(Register reg)
{
return deviceClient.read(reg.addr, reg.length);
}
private byte[] readContiguousRegisters(Register first, Register last)
{
int addrStart = first.addr;
int addrEnd = last.addr + last.length;
int bytesToRead = addrEnd-addrStart;
return deviceClient.read(addrStart, bytesToRead);
}
private void writeRegister(Register reg, byte[] bytes)
{
if(reg.length != bytes.length)
{
throw new IllegalArgumentException("reg.length != bytes.length");
}
deviceClient.write(reg.addr, bytes);
}
private void writeContiguousRegisters(Register first, Register last, byte[] dat)
{
int addrStart = first.addr;
int addrEnd = last.addr + last.length;
int bytesToWrite = addrEnd-addrStart;
if(bytesToWrite != dat.length)
{
throw new IllegalArgumentException("bytesToWrite != dat.length");
}
deviceClient.write(addrStart, dat);
}
}

2
TeamCode/src/main/java/ftclib

Submodule TeamCode/src/main/java/ftclib updated: ff5b5322d3...f1b523ca30