380 src/com/spartronics4915/lib/util/control/PathFollower.java
View file

Large diffs are not rendered by default.

553 src/com/spartronics4915/lib/util/control/PathSegment.java
View file

Large diffs are not rendered by default.

605 src/com/spartronics4915/lib/util/control/SynchronousPIDF.java
View file

Large diffs are not rendered by default.

312 src/com/spartronics4915/lib/util/drivers/ADXRS453_Gyro.java
View file
@@ -8,196 +8,182 @@

package com.spartronics4915.lib.util.drivers;

import java.nio.ByteBuffer;
import java.nio.ByteOrder;

import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.GyroBase;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpilibj.SPI;
import edu.wpi.first.wpilibj.Sendable;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.interfaces.Gyro;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.Sendable;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;

/**
* Use a rate gyro to return the robots heading relative to a starting position. The Gyro class tracks the robots
* heading based on the starting position. As the robot rotates the new heading is computed by integrating the rate of
* rotation returned by the sensor. When the class is instantiated, it does a short calibration routine where it samples
* the gyro while at rest to determine the default offset. This is subtracted from each sample to determine the heading.
* Use a rate gyro to return the robots heading relative to a starting position. The Gyro class
* tracks the robots heading based on the starting position. As the robot rotates the new heading is
* computed by integrating the rate of rotation returned by the sensor. When the class is
* instantiated, it does a short calibration routine where it samples the gyro while at rest to
* determine the default offset. This is subtracted from each sample to determine the heading.
*
* This class is for the digital ADXRS453 gyro sensor that connects via SPI. A datasheet can be found here:
* http://www.analog.com/media/en/technical-documentation/data-sheets/ADXRS453. pdf
* <p>This class is for the digital ADXRS453 gyro sensor that connects via SPI. A datasheet can be
* found here: http://www.analog.com/media/en/technical-documentation/data-sheets/ADXRS453. pdf
*/
public class ADXRS453_Gyro extends GyroBase implements Gyro, PIDSource, Sendable {
public static final double kCalibrationSampleTime = 5.0;

private static final double kSamplePeriod = 0.001;
private static final double kDegreePerSecondPerLSB = -0.0125;

private static final int kPIDRegister = 0x0C;

private SPI m_spi;

private boolean m_is_calibrating;
private double m_last_center;

/**
* Constructor. Uses the onboard CS0.
*/
public ADXRS453_Gyro() {
this(SPI.Port.kOnboardCS0);
public static final double kCalibrationSampleTime = 5.0;

private static final double kSamplePeriod = 0.001;
private static final double kDegreePerSecondPerLSB = -0.0125;

private static final int kPIDRegister = 0x0C;

private SPI m_spi;

private boolean m_is_calibrating;
private double m_last_center;

/** Constructor. Uses the onboard CS0. */
public ADXRS453_Gyro() {
this(SPI.Port.kOnboardCS0);
}

/**
* 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;
}

/**
* 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);
this.setName("ADXR5453_Gyro", port.value); // from SendableBase
m_spi.initAccumulator(kSamplePeriod, 0x20000000, 4, 0x0c00000E, 0x04000000, 10, 16, true, true);

calibrate();

// LiveWindow.addSensor("ADXRS453_Gyro", port.value, this);
this.setName("ADXR5453_Gyro", port.value); // from SendableBase
}

/**
* This is a blocking calibration call. There are also non-blocking options available in this
* class!
*
* <p>{@inheritDoc}
*/
@Override
public synchronized void calibrate() {
Timer.delay(0.1);
startCalibrate();
Timer.delay(kCalibrationSampleTime);
endCalibrate();
}

public synchronized void startCalibrate() {
if (m_spi == null) return;

if (!m_is_calibrating) {
m_is_calibrating = true;
m_spi.setAccumulatorCenter(0);
m_spi.resetAccumulator();
}

/**
* This is a blocking calibration call. There are also non-blocking options available in this class!
*
* {@inheritDoc}
*/
@Override
public synchronized void calibrate() {
Timer.delay(0.1);
startCalibrate();
Timer.delay(kCalibrationSampleTime);
endCalibrate();
}

public synchronized void startCalibrate() {
if (m_spi == null)
return;

if (!m_is_calibrating) {
m_is_calibrating = true;
m_spi.setAccumulatorCenter(0);
m_spi.resetAccumulator();
}
}

public synchronized void endCalibrate() {
if (m_is_calibrating) {
m_is_calibrating = false;
m_last_center = m_spi.getAccumulatorAverage();
m_spi.setAccumulatorCenter((int) Math.round(m_last_center));
m_spi.resetAccumulator();
}
}

public synchronized void endCalibrate() {
if (m_is_calibrating) {
m_is_calibrating = false;
m_last_center = m_spi.getAccumulatorAverage();
m_spi.setAccumulatorCenter((int) Math.round(m_last_center));
m_spi.resetAccumulator();
}
public synchronized void cancelCalibrate() {
if (m_is_calibrating) {
m_is_calibrating = false;
m_spi.setAccumulatorCenter((int) Math.round(m_last_center));
m_spi.resetAccumulator();
}
}

public synchronized void cancelCalibrate() {
if (m_is_calibrating) {
m_is_calibrating = false;
m_spi.setAccumulatorCenter((int) Math.round(m_last_center));
m_spi.resetAccumulator();
}
}
public synchronized double getCenter() {
return m_last_center;
}

public synchronized double getCenter() {
return m_last_center;
private boolean calcParity(int v) {
boolean parity = false;
while (v != 0) {
parity = !parity;
v = v & (v - 1);
}
return parity;
}

private boolean calcParity(int v) {
boolean parity = false;
while (v != 0) {
parity = !parity;
v = v & (v - 1);
}
return parity;
}
private int readRegister(int reg) {
int cmdhi = 0x8000 | (reg << 1);
boolean parity = calcParity(cmdhi);

private int readRegister(int reg) {
int cmdhi = 0x8000 | (reg << 1);
boolean parity = calcParity(cmdhi);
ByteBuffer buf = ByteBuffer.allocateDirect(4);
buf.order(ByteOrder.BIG_ENDIAN);
buf.put(0, (byte) (cmdhi >> 8));
buf.put(1, (byte) (cmdhi & 0xff));
buf.put(2, (byte) 0);
buf.put(3, (byte) (parity ? 0 : 1));

ByteBuffer buf = ByteBuffer.allocateDirect(4);
buf.order(ByteOrder.BIG_ENDIAN);
buf.put(0, (byte) (cmdhi >> 8));
buf.put(1, (byte) (cmdhi & 0xff));
buf.put(2, (byte) 0);
buf.put(3, (byte) (parity ? 0 : 1));
m_spi.write(buf, 4);
m_spi.read(false, buf, 4);

m_spi.write(buf, 4);
m_spi.read(false, buf, 4);

if ((buf.get(0) & 0xe0) == 0) {
return 0;
}
return (buf.getInt(0) >> 5) & 0xffff;
if ((buf.get(0) & 0xe0) == 0) {
return 0;
}

/**
* {@inheritDoc}
*/
@Override
public synchronized void reset() {
if (m_is_calibrating) {
cancelCalibrate();
}
m_spi.resetAccumulator();
return (buf.getInt(0) >> 5) & 0xffff;
}

/** {@inheritDoc} */
@Override
public synchronized void reset() {
if (m_is_calibrating) {
cancelCalibrate();
}

/**
* Delete (free) the spi port used for the gyro and stop accumulating.
*/
@Override
public void free() {
if (m_spi != null) {
m_spi.free();
m_spi = null;
}
m_spi.resetAccumulator();
}

/** Delete (free) the spi port used for the gyro and stop accumulating. */
@Override
public void free() {
if (m_spi != null) {
m_spi.free();
m_spi = null;
}

/**
* {@inheritDoc}
*/
@Override
public synchronized double getAngle() {
if (m_spi == null)
return 0.0;
if (m_is_calibrating) {
return 0.0;
}
return m_spi.getAccumulatorValue() * kDegreePerSecondPerLSB * kSamplePeriod;
}

/** {@inheritDoc} */
@Override
public synchronized double getAngle() {
if (m_spi == null) return 0.0;
if (m_is_calibrating) {
return 0.0;
}

/**
* {@inheritDoc}
*/
@Override
public synchronized double getRate() {
if (m_spi == null)
return 0.0;
if (m_is_calibrating) {
return 0.0;
}
return m_spi.getAccumulatorLastValue() * kDegreePerSecondPerLSB;
return m_spi.getAccumulatorValue() * kDegreePerSecondPerLSB * kSamplePeriod;
}

/** {@inheritDoc} */
@Override
public synchronized double getRate() {
if (m_spi == null) return 0.0;
if (m_is_calibrating) {
return 0.0;
}
return m_spi.getAccumulatorLastValue() * kDegreePerSecondPerLSB;
}
}
1,606 src/com/spartronics4915/lib/util/drivers/BNO055.java
View file

Large diffs are not rendered by default.

92 src/com/spartronics4915/lib/util/drivers/BetterBNO.java
View file
@@ -3,52 +3,58 @@
import com.spartronics4915.lib.util.math.Rotation2d;

/**
* This class implements some of the functionality missing from the BNO055
* that is present in the NavX (mostly related to resetting and zeroing).
* This class closely follows {@link NavX}, especially in method names,
* and access and concurrency keywords.
*
* This class implements some of the functionality missing from the BNO055 that is present in the
* NavX (mostly related to resetting and zeroing). This class closely follows {@link NavX},
* especially in method names, and access and concurrency keywords.
*
* @author declan
* @see NavX
* @see com.spartronics4915.lib.util.math.Rotation2d
*/
public class BetterBNO {
private BNO055 mBNO055;

private Rotation2d mAngleZeroingOffset = Rotation2d.identity();
private Rotation2d mAngleAdjustment = Rotation2d.identity(); // For the user to adjust the angle

public BetterBNO() {
mBNO055 = BNO055.getInstance(BNO055.opmode_t.OPERATION_MODE_IMUPLUS, BNO055.vector_type_t.VECTOR_EULER);
}

public synchronized void reset() {
mBNO055.resetState();
mAngleZeroingOffset = Rotation2d.identity(); // Set the offset to zero
}

public synchronized void zeroYaw() {
// We use the offset to zero ourselves
mAngleZeroingOffset = Rotation2d.fromDegrees(mBNO055.getNormalizedHeading()).inverse();
}

public synchronized void setAngleAdjustment(Rotation2d adjustment) {
mAngleAdjustment = adjustment;
}

public synchronized double getRawYawDegrees() {
return mBNO055.getNormalizedHeading();
}

public Rotation2d getYaw() {
return mAngleZeroingOffset.rotateBy(Rotation2d.fromDegrees(getRawYawDegrees())).rotateBy(mAngleAdjustment);
}

public double getRawAccelX() {
return mBNO055.getAccel()[0]; // XXX: I'm pretty sure that index 0 is the X-axis, but there isn't any documentation on it
}

public boolean isPresent() {
return mBNO055.isSensorPresent();
}
private BNO055 mBNO055;

private Rotation2d mAngleZeroingOffset = Rotation2d.identity();
private Rotation2d mAngleAdjustment = Rotation2d.identity(); // For the user to adjust the angle

public BetterBNO() {
mBNO055 =
BNO055.getInstance(
BNO055.opmode_t.OPERATION_MODE_IMUPLUS, BNO055.vector_type_t.VECTOR_EULER);
}

public synchronized void reset() {
mBNO055.resetState();
mAngleZeroingOffset = Rotation2d.identity(); // Set the offset to zero
}

public synchronized void zeroYaw() {
// We use the offset to zero ourselves
mAngleZeroingOffset = Rotation2d.fromDegrees(mBNO055.getNormalizedHeading()).inverse();
}

public synchronized void setAngleAdjustment(Rotation2d adjustment) {
mAngleAdjustment = adjustment;
}

public synchronized double getRawYawDegrees() {
return mBNO055.getNormalizedHeading();
}

public Rotation2d getYaw() {
return mAngleZeroingOffset
.rotateBy(Rotation2d.fromDegrees(getRawYawDegrees()))
.rotateBy(mAngleAdjustment);
}

public double getRawAccelX() {
return mBNO055
.getAccel()[
0]; // XXX: I'm pretty sure that index 0 is the X-axis, but there isn't any documentation on
// it
}

public boolean isPresent() {
return mBNO055.isSensorPresent();
}
}
1,782 src/com/spartronics4915/lib/util/drivers/CANTalon.java
View file

Large diffs are not rendered by default.

540 src/com/spartronics4915/lib/util/drivers/CANTalonFactory.java
View file

Large diffs are not rendered by default.

51 src/com/spartronics4915/lib/util/drivers/IRSensor.java
View file
@@ -6,36 +6,37 @@
import edu.wpi.first.wpilibj.Counter;

/**
* Driver for an analog Sharp IR sensor (or any distance sensor where output voltage is a function of range, really).
* Driver for an analog Sharp IR sensor (or any distance sensor where output voltage is a function
* of range, really).
*/
public class IRSensor {
protected final AnalogInput mAnalogInput;
protected final AnalogTrigger mAnalogTrigger;
protected final Counter mCounter;
protected final AnalogInput mAnalogInput;
protected final AnalogTrigger mAnalogTrigger;
protected final Counter mCounter;

public IRSensor(int port, double min_trigger_voltage, double max_trigger_voltage) {
mAnalogInput = new AnalogInput(port);
mAnalogInput.setAverageBits(6);
mAnalogTrigger = new AnalogTrigger(mAnalogInput);
mAnalogTrigger.setAveraged(true);
mAnalogTrigger.setFiltered(false);
mAnalogTrigger.setLimitsVoltage(min_trigger_voltage, max_trigger_voltage);
mCounter = new Counter(mAnalogTrigger.createOutput(AnalogTriggerType.kState));
}
public IRSensor(int port, double min_trigger_voltage, double max_trigger_voltage) {
mAnalogInput = new AnalogInput(port);
mAnalogInput.setAverageBits(6);
mAnalogTrigger = new AnalogTrigger(mAnalogInput);
mAnalogTrigger.setAveraged(true);
mAnalogTrigger.setFiltered(false);
mAnalogTrigger.setLimitsVoltage(min_trigger_voltage, max_trigger_voltage);
mCounter = new Counter(mAnalogTrigger.createOutput(AnalogTriggerType.kState));
}

public int getCount() {
return mCounter.get();
}
public int getCount() {
return mCounter.get();
}

public double getVoltage() {
return mAnalogInput.getAverageVoltage();
}
public double getVoltage() {
return mAnalogInput.getAverageVoltage();
}

public boolean seesBall() {
return mAnalogTrigger.getTriggerState();
}
public boolean seesBall() {
return mAnalogTrigger.getTriggerState();
}

public void resetCount() {
mCounter.reset();
}
public void resetCount() {
mCounter.reset();
}
}
41 src/com/spartronics4915/lib/util/drivers/LazyCANTalon.java
View file
@@ -1,32 +1,33 @@
package com.spartronics4915.lib.util.drivers;

import com.ctre.phoenix.motorcontrol.ControlMode;

/**
* This class is a thin wrapper around the CANTalon that reduces CAN bus / CPU overhead by skipping duplicate set
* commands. (By default the Talon flushes the Tx buffer on every set call).
* This class is a thin wrapper around the CANTalon that reduces CAN bus / CPU overhead by skipping
* duplicate set commands. (By default the Talon flushes the Tx buffer on every set call).
*/
public class LazyCANTalon extends CANTalon {
protected double mLastSet = Double.NaN;
protected ControlMode mLastControlMode = null;
protected double mLastSet = Double.NaN;
protected ControlMode mLastControlMode = null;

public LazyCANTalon(int deviceNumber, int controlPeriodMs, int enablePeriodMs) {
super(deviceNumber, controlPeriodMs, enablePeriodMs);
}
public LazyCANTalon(int deviceNumber, int controlPeriodMs, int enablePeriodMs) {
super(deviceNumber, controlPeriodMs, enablePeriodMs);
}

public LazyCANTalon(int deviceNumber, int controlPeriodMs) {
super(deviceNumber, controlPeriodMs);
}
public LazyCANTalon(int deviceNumber, int controlPeriodMs) {
super(deviceNumber, controlPeriodMs);
}

public LazyCANTalon(int deviceNumber) {
super(deviceNumber);
}
public LazyCANTalon(int deviceNumber) {
super(deviceNumber);
}

@Override
public void set(double value) {
if (value != mLastSet || getControlMode() != mLastControlMode) {
mLastSet = value;
mLastControlMode = getControlMode();
super.set(value);
}
@Override
public void set(double value) {
if (value != mLastSet || getControlMode() != mLastControlMode) {
mLastSet = value;
mLastControlMode = getControlMode();
super.set(value);
}
}
}
122 src/com/spartronics4915/lib/util/drivers/LidarLiteSensor.java
View file
@@ -1,84 +1,74 @@
package com.spartronics4915.lib.util.drivers;

import java.util.TimerTask;

import edu.wpi.first.wpilibj.I2C;
import edu.wpi.first.wpilibj.I2C.Port;
import edu.wpi.first.wpilibj.Timer;
import java.util.TimerTask;

/**
* Driver for a Lidar Lite sensor
*/
/** Driver for a Lidar Lite sensor */
public class LidarLiteSensor {
private I2C mI2C;
private byte[] mDistance;
private java.util.Timer mUpdater;
private boolean mHasSignal;
private I2C mI2C;
private byte[] mDistance;
private java.util.Timer mUpdater;
private boolean mHasSignal;

private final static int LIDAR_ADDR = 0x62;
private final static int LIDAR_CONFIG_REGISTER = 0x00;
private final static int LIDAR_DISTANCE_REGISTER = 0x8f;
private static final int LIDAR_ADDR = 0x62;
private static final int LIDAR_CONFIG_REGISTER = 0x00;
private static final int LIDAR_DISTANCE_REGISTER = 0x8f;

public LidarLiteSensor(Port port) {
mI2C = new I2C(port, LIDAR_ADDR);
mDistance = new byte[2];
mUpdater = new java.util.Timer();
mHasSignal = false;
}
public LidarLiteSensor(Port port) {
mI2C = new I2C(port, LIDAR_ADDR);
mDistance = new byte[2];
mUpdater = new java.util.Timer();
mHasSignal = false;
}

/**
* @return Distance in meters
*/
public double getDistance() {
int distCm = (int) Integer.toUnsignedLong(mDistance[0] << 8) + Byte.toUnsignedInt(mDistance[1]);
return distCm / 100.0;
}
/** @return Distance in meters */
public double getDistance() {
int distCm = (int) Integer.toUnsignedLong(mDistance[0] << 8) + Byte.toUnsignedInt(mDistance[1]);
return distCm / 100.0;
}

/**
* @return true iff the sensor successfully provided data last loop
*/
public boolean hasSignal() {
return mHasSignal;
}
/** @return true iff the sensor successfully provided data last loop */
public boolean hasSignal() {
return mHasSignal;
}

/**
* Start 10Hz polling
*/
public void start() {
start(100);
}
/** Start 10Hz polling */
public void start() {
start(100);
}

/**
* Start polling for period in milliseconds
*/
public void start(int period) {
TimerTask task = new TimerTask() {
@Override
public void run() {
update();
}
/** Start polling for period in milliseconds */
public void start(int period) {
TimerTask task =
new TimerTask() {
@Override
public void run() {
update();
}
};
mUpdater.scheduleAtFixedRate(task, 0, period);
}
mUpdater.scheduleAtFixedRate(task, 0, period);
}

public void stop() {
mUpdater.cancel();
mUpdater = new java.util.Timer();
}
public void stop() {
mUpdater.cancel();
mUpdater = new java.util.Timer();
}

private void update() {
if (mI2C.write(LIDAR_CONFIG_REGISTER, 0x04)) {
// the write failed to ack
mHasSignal = false;
return;
}
Timer.delay(0.04); // Delay for measurement to be taken
if (!mI2C.read(LIDAR_DISTANCE_REGISTER, 2, mDistance)) {
// the read failed
mHasSignal = false;
return;
}
mHasSignal = true;
Timer.delay(0.005); // Delay to prevent over polling
private void update() {
if (mI2C.write(LIDAR_CONFIG_REGISTER, 0x04)) {
// the write failed to ack
mHasSignal = false;
return;
}
Timer.delay(0.04); // Delay for measurement to be taken
if (!mI2C.read(LIDAR_DISTANCE_REGISTER, 2, mDistance)) {
// the read failed
mHasSignal = false;
return;
}
mHasSignal = true;
Timer.delay(0.005); // Delay to prevent over polling
}
}
81 src/com/spartronics4915/lib/util/drivers/MA3AnalogEncoder.java
View file
@@ -2,62 +2,63 @@

import com.spartronics4915.lib.util.CrashTrackingRunnable;
import com.spartronics4915.lib.util.math.Rotation2d;

import edu.wpi.first.wpilibj.AnalogInput;
import edu.wpi.first.wpilibj.Notifier;

/**
* A 10-bit analog MA3 absolute encoder. http://cdn.usdigital.com/assets/datasheets/MA3_datasheet.pdf
* A 10-bit analog MA3 absolute encoder.
* http://cdn.usdigital.com/assets/datasheets/MA3_datasheet.pdf
*/
public class MA3AnalogEncoder {

private final AnalogInput mAnalogInput;
private final AnalogInput mAnalogInput;

protected Notifier notifier_;
protected Rotation2d rotation_ = new Rotation2d();
protected Rotation2d home_ = new Rotation2d();
protected int num_rotations_ = 0;
protected Notifier notifier_;
protected Rotation2d rotation_ = new Rotation2d();
protected Rotation2d home_ = new Rotation2d();
protected int num_rotations_ = 0;

private CrashTrackingRunnable read_thread_ = new CrashTrackingRunnable() {
private CrashTrackingRunnable read_thread_ =
new CrashTrackingRunnable() {
@Override
public void runCrashTracked() {
Rotation2d new_rotation = Rotation2d.fromRadians(2 * Math.PI * mAnalogInput.getVoltage() / 5.0);

// Check for rollover
synchronized (MA3AnalogEncoder.this) {
double relative_angle = rotation_.getRadians()
+ rotation_.inverse().rotateBy(new_rotation).getRadians();
if (relative_angle > Math.PI) {
++num_rotations_;
} else if (relative_angle < -Math.PI) {
--num_rotations_;
}
rotation_ = new_rotation;
Rotation2d new_rotation =
Rotation2d.fromRadians(2 * Math.PI * mAnalogInput.getVoltage() / 5.0);

// Check for rollover
synchronized (MA3AnalogEncoder.this) {
double relative_angle =
rotation_.getRadians() + rotation_.inverse().rotateBy(new_rotation).getRadians();
if (relative_angle > Math.PI) {
++num_rotations_;
} else if (relative_angle < -Math.PI) {
--num_rotations_;
}
rotation_ = new_rotation;
}
}
};

public MA3AnalogEncoder(int port) {
mAnalogInput = new AnalogInput(port);
notifier_ = new Notifier(read_thread_);
notifier_.startPeriodic(0.01); // 100 Hz
}
};

public synchronized Rotation2d getCalibratedAngle() {
return home_.rotateBy(rotation_);
}
public MA3AnalogEncoder(int port) {
mAnalogInput = new AnalogInput(port);
notifier_ = new Notifier(read_thread_);
notifier_.startPeriodic(0.01); // 100 Hz
}

public synchronized void zero() {
num_rotations_ = 0;
home_ = rotation_.inverse();
}
public synchronized Rotation2d getCalibratedAngle() {
return home_.rotateBy(rotation_);
}

public synchronized Rotation2d getRawAngle() {
return rotation_;
}
public synchronized void zero() {
num_rotations_ = 0;
home_ = rotation_.inverse();
}

public synchronized double getContinuousAngleDegrees() {
return getRawAngle().getDegrees() + num_rotations_ * 360.0 + home_.getDegrees();
}
public synchronized Rotation2d getRawAngle() {
return rotation_;
}

public synchronized double getContinuousAngleDegrees() {
return getRawAngle().getDegrees() + num_rotations_ * 360.0 + home_.getDegrees();
}
}
137 src/com/spartronics4915/lib/util/drivers/MA3Encoder.java
View file
@@ -2,7 +2,6 @@

import com.spartronics4915.lib.util.CrashTrackingRunnable;
import com.spartronics4915.lib.util.math.Rotation2d;

import edu.wpi.first.wpilibj.Counter;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.DriverStation;
@@ -12,84 +11,86 @@
* A 12-bit PWM MA3 absolute encoder. http://cdn.usdigital.com/assets/datasheets/MA3_datasheet.pdf
*/
public class MA3Encoder {
static final double kNominalPeriodS = 4098 * 1E-6;
static final double kPeriodToleranceS = 200 * 1E-6;
static final double kNominalPeriodS = 4098 * 1E-6;
static final double kPeriodToleranceS = 200 * 1E-6;

protected DigitalInput digital_input_;
protected Counter high_counter_; // access only from inner class after
// construction
protected Counter period_counter_; // access only from inner class after
// construction
protected Notifier notifier_;
protected Rotation2d rotation_ = new Rotation2d();
protected Rotation2d home_ = new Rotation2d();
protected int num_rotations_ = 0;
protected boolean error_ = false;
protected DigitalInput digital_input_;
protected Counter high_counter_; // access only from inner class after
// construction
protected Counter period_counter_; // access only from inner class after
// construction
protected Notifier notifier_;
protected Rotation2d rotation_ = new Rotation2d();
protected Rotation2d home_ = new Rotation2d();
protected int num_rotations_ = 0;
protected boolean error_ = false;

private CrashTrackingRunnable read_thread_ = new CrashTrackingRunnable() {
private CrashTrackingRunnable read_thread_ =
new CrashTrackingRunnable() {
@Override
public void runCrashTracked() {
if (high_counter_.getStopped()) {
if (!error_) {
DriverStation.reportError("No MA3Encoder on channel " + digital_input_.getChannel(), false);
}
error_ = true;
return;
}
error_ = false;
double t_high = high_counter_.getPeriod();
double t_total = period_counter_.getPeriod();
if (t_total > kNominalPeriodS + kPeriodToleranceS || t_total < kNominalPeriodS - kPeriodToleranceS) {
// We got a nonsensical rising-to-rising edge period, so ignore
// this sample.
return;
if (high_counter_.getStopped()) {
if (!error_) {
DriverStation.reportError(
"No MA3Encoder on channel " + digital_input_.getChannel(), false);
}
double x = (t_high * 4098) / (t_total) - 1;
if (x > 4095) {
x = 4095;
}
Rotation2d new_rotation = Rotation2d.fromRadians(2 * Math.PI * x / 4096);
error_ = true;
return;
}
error_ = false;
double t_high = high_counter_.getPeriod();
double t_total = period_counter_.getPeriod();
if (t_total > kNominalPeriodS + kPeriodToleranceS
|| t_total < kNominalPeriodS - kPeriodToleranceS) {
// We got a nonsensical rising-to-rising edge period, so ignore
// this sample.
return;
}
double x = (t_high * 4098) / (t_total) - 1;
if (x > 4095) {
x = 4095;
}
Rotation2d new_rotation = Rotation2d.fromRadians(2 * Math.PI * x / 4096);

// Check for rollover
synchronized (MA3Encoder.this) {
double relative_angle = rotation_.getRadians()
+ rotation_.inverse().rotateBy(new_rotation).getRadians();
if (relative_angle > Math.PI) {
++num_rotations_;
} else if (relative_angle < -Math.PI) {
--num_rotations_;
}
rotation_ = new_rotation;
// Check for rollover
synchronized (MA3Encoder.this) {
double relative_angle =
rotation_.getRadians() + rotation_.inverse().rotateBy(new_rotation).getRadians();
if (relative_angle > Math.PI) {
++num_rotations_;
} else if (relative_angle < -Math.PI) {
--num_rotations_;
}
rotation_ = new_rotation;
}
}
};

public MA3Encoder(int port) {
digital_input_ = new DigitalInput(port);
high_counter_ = new Counter(digital_input_);
period_counter_ = new Counter(digital_input_);
high_counter_.setSamplesToAverage(1);
high_counter_.setSemiPeriodMode(true);
period_counter_.setSamplesToAverage(1);
notifier_ = new Notifier(read_thread_);
notifier_.startPeriodic(0.01); // 100 Hz
}
};

public synchronized Rotation2d getCalibratedAngle() {
return home_.rotateBy(rotation_);
}
public MA3Encoder(int port) {
digital_input_ = new DigitalInput(port);
high_counter_ = new Counter(digital_input_);
period_counter_ = new Counter(digital_input_);
high_counter_.setSamplesToAverage(1);
high_counter_.setSemiPeriodMode(true);
period_counter_.setSamplesToAverage(1);
notifier_ = new Notifier(read_thread_);
notifier_.startPeriodic(0.01); // 100 Hz
}

public synchronized void zero() {
num_rotations_ = 0;
home_ = rotation_.inverse();
}
public synchronized Rotation2d getCalibratedAngle() {
return home_.rotateBy(rotation_);
}

public synchronized Rotation2d getRawAngle() {
return rotation_;
}
public synchronized void zero() {
num_rotations_ = 0;
home_ = rotation_.inverse();
}

public synchronized double getContinuousAngleDegrees() {
return getRawAngle().getDegrees() + num_rotations_ * 360.0 + home_.getDegrees();
}
public synchronized Rotation2d getRawAngle() {
return rotation_;
}

public synchronized double getContinuousAngleDegrees() {
return getRawAngle().getDegrees() + num_rotations_ * 360.0 + home_.getDegrees();
}
}
24 src/com/spartronics4915/lib/util/drivers/MB1043.java
View file
@@ -1,19 +1,17 @@
package com.spartronics4915.lib.util.drivers;

/**
* Driver for a MB1043 ultrasonic sensor
*/
/** Driver for a MB1043 ultrasonic sensor */
public class MB1043 extends UltrasonicSensor {
public MB1043(int port) {
super(port);
this.mScalingFactor = 1024.0; // Per 1 mm (Vcc/1024 per 5mm, Vcc = 5v)
}
public MB1043(int port) {
super(port);
this.mScalingFactor = 1024.0; // Per 1 mm (Vcc/1024 per 5mm, Vcc = 5v)
}

public double getAverageDistanceInches() {
return getAverageDistance() * 0.0393701; // Inches per mm
}
public double getAverageDistanceInches() {
return getAverageDistance() * 0.0393701; // Inches per mm
}

public double getRawDistanceInches() {
return super.getLatestDistance() * 0.0393701; // Inches per mm;
}
public double getRawDistanceInches() {
return super.getLatestDistance() * 0.0393701; // Inches per mm;
}
}
118 src/com/spartronics4915/lib/util/drivers/NavX.java
View file
@@ -4,80 +4,80 @@
import com.kauailabs.navx.frc.AHRS;
import com.kauailabs.navx.frc.ITimestampedDataSubscriber;
import com.spartronics4915.lib.util.math.Rotation2d;

import edu.wpi.first.wpilibj.SPI;

/**
* Driver for a NavX board. Basically a wrapper for the {@link AHRS} class
*/
/** Driver for a NavX board. Basically a wrapper for the {@link AHRS} class */
public class NavX {
protected class Callback implements ITimestampedDataSubscriber {
@Override
public void timestampedDataReceived(long system_timestamp, long sensor_timestamp, AHRSUpdateBase update,
Object context) {
synchronized (NavX.this) {
// This handles the fact that the sensor is inverted from our coordinate conventions.
if (mLastSensorTimestampMs != kInvalidTimestamp && mLastSensorTimestampMs < sensor_timestamp) {
mYawRateDegreesPerSecond = 1000.0 * (-mYawDegrees - update.yaw)
/ (double) (sensor_timestamp - mLastSensorTimestampMs);
}
mLastSensorTimestampMs = sensor_timestamp;
mYawDegrees = -update.yaw;
}
protected class Callback implements ITimestampedDataSubscriber {
@Override
public void timestampedDataReceived(
long system_timestamp, long sensor_timestamp, AHRSUpdateBase update, Object context) {
synchronized (NavX.this) {
// This handles the fact that the sensor is inverted from our coordinate conventions.
if (mLastSensorTimestampMs != kInvalidTimestamp
&& mLastSensorTimestampMs < sensor_timestamp) {
mYawRateDegreesPerSecond =
1000.0
* (-mYawDegrees - update.yaw)
/ (double) (sensor_timestamp - mLastSensorTimestampMs);
}
mLastSensorTimestampMs = sensor_timestamp;
mYawDegrees = -update.yaw;
}
}
}

protected AHRS mAHRS;
protected AHRS mAHRS;

protected Rotation2d mAngleAdjustment = Rotation2d.identity();
protected double mYawDegrees;
protected double mYawRateDegreesPerSecond;
protected final long kInvalidTimestamp = -1;
protected long mLastSensorTimestampMs;
protected Rotation2d mAngleAdjustment = Rotation2d.identity();
protected double mYawDegrees;
protected double mYawRateDegreesPerSecond;
protected final long kInvalidTimestamp = -1;
protected long mLastSensorTimestampMs;

public NavX(SPI.Port spi_port_id) {
mAHRS = new AHRS(spi_port_id, (byte) 200);
resetState();
mAHRS.registerCallback(new Callback(), null);
}
public NavX(SPI.Port spi_port_id) {
mAHRS = new AHRS(spi_port_id, (byte) 200);
resetState();
mAHRS.registerCallback(new Callback(), null);
}

public synchronized void reset() {
mAHRS.reset();
resetState();
}
public synchronized void reset() {
mAHRS.reset();
resetState();
}

public synchronized void zeroYaw() {
mAHRS.zeroYaw();
resetState();
}
public synchronized void zeroYaw() {
mAHRS.zeroYaw();
resetState();
}

private void resetState() {
mLastSensorTimestampMs = kInvalidTimestamp;
mYawDegrees = 0.0;
mYawRateDegreesPerSecond = 0.0;
}
private void resetState() {
mLastSensorTimestampMs = kInvalidTimestamp;
mYawDegrees = 0.0;
mYawRateDegreesPerSecond = 0.0;
}

public synchronized void setAngleAdjustment(Rotation2d adjustment) {
mAngleAdjustment = adjustment;
}
public synchronized void setAngleAdjustment(Rotation2d adjustment) {
mAngleAdjustment = adjustment;
}

protected synchronized double getRawYawDegrees() {
return mYawDegrees;
}
protected synchronized double getRawYawDegrees() {
return mYawDegrees;
}

public Rotation2d getYaw() {
return mAngleAdjustment.rotateBy(Rotation2d.fromDegrees(getRawYawDegrees()));
}
public Rotation2d getYaw() {
return mAngleAdjustment.rotateBy(Rotation2d.fromDegrees(getRawYawDegrees()));
}

public double getYawRateDegreesPerSec() {
return mYawRateDegreesPerSecond;
}
public double getYawRateDegreesPerSec() {
return mYawRateDegreesPerSecond;
}

public double getYawRateRadiansPerSec() {
return 180.0 / Math.PI * getYawRateDegreesPerSec();
}
public double getYawRateRadiansPerSec() {
return 180.0 / Math.PI * getYawRateDegreesPerSec();
}

public double getRawAccelX() {
return mAHRS.getRawAccelX();
}
public double getRawAccelX() {
return mAHRS.getRawAccelX();
}
}
18 src/com/spartronics4915/lib/util/drivers/RevRoboticsAirPressureSensor.java
View file
@@ -5,17 +5,17 @@
/**
* Wraps an analog input for a Rev Robotics Analog Pressure sensor.
*
* http://www.revrobotics.com/wp-content/uploads/2015/11/REV-11-1107-DS-00.pdf
* <p>http://www.revrobotics.com/wp-content/uploads/2015/11/REV-11-1107-DS-00.pdf
*/
public class RevRoboticsAirPressureSensor {
private final AnalogInput mAnalogInput;
private final AnalogInput mAnalogInput;

public RevRoboticsAirPressureSensor(int analogInputNumber) {
mAnalogInput = new AnalogInput(analogInputNumber);
}
public RevRoboticsAirPressureSensor(int analogInputNumber) {
mAnalogInput = new AnalogInput(analogInputNumber);
}

public double getAirPressurePsi() {
// taken from the datasheet
return 250.0 * mAnalogInput.getVoltage() / 5.0 - 25.0;
}
public double getAirPressurePsi() {
// taken from the datasheet
return 250.0 * mAnalogInput.getVoltage() / 5.0 - 25.0;
}
}
68 src/com/spartronics4915/lib/util/drivers/UltrasonicSensor.java
View file
@@ -1,44 +1,40 @@
package com.spartronics4915.lib.util.drivers;

import java.util.LinkedList;

import edu.wpi.first.wpilibj.AnalogInput;
import java.util.LinkedList;

/**
* Driver for an analog Ultrasonic Sensor (mainly to help smooth out noise).
*/
/** Driver for an analog Ultrasonic Sensor (mainly to help smooth out noise). */
public class UltrasonicSensor {
protected AnalogInput mAnalogInput;
private LinkedList<Double> cache;
protected double mScalingFactor = 512.0 / 5.0;

private static final int kCacheSize = 5;

public UltrasonicSensor(int port) {
mAnalogInput = new AnalogInput(port);
cache = new LinkedList<Double>();
cache.add(getRawDistance());
}

public void update() {
cache.add(getRawDistance());
if (cache.size() > kCacheSize)
cache.removeFirst();
}

public double getRawDistance() {
return mAnalogInput.getVoltage() * mScalingFactor;
protected AnalogInput mAnalogInput;
private LinkedList<Double> cache;
protected double mScalingFactor = 512.0 / 5.0;

private static final int kCacheSize = 5;

public UltrasonicSensor(int port) {
mAnalogInput = new AnalogInput(port);
cache = new LinkedList<Double>();
cache.add(getRawDistance());
}

public void update() {
cache.add(getRawDistance());
if (cache.size() > kCacheSize) cache.removeFirst();
}

public double getRawDistance() {
return mAnalogInput.getVoltage() * mScalingFactor;
}

public double getAverageDistance() {
double total = 0;
for (Double d : cache) {
total += d;
}
return total / cache.size();
}

public double getAverageDistance() {
double total = 0;
for (Double d : cache) {
total += d;
}
return total / cache.size();
}

public double getLatestDistance() {
return cache.getLast();
}
public double getLatestDistance() {
return cache.getLast();
}
}
305 src/com/spartronics4915/lib/util/math/PolynomialRegression.java
View file
@@ -1,183 +1,174 @@
// NOTE: This file is available at http://algs4.cs.princeton.edu/14analysis/PolynomialRegression.java.html
// NOTE: This file is available at
// http://algs4.cs.princeton.edu/14analysis/PolynomialRegression.java.html
package com.spartronics4915.lib.util.math;

/******************************************************************************
* Compilation: javac -cp .:jama.jar PolynomialRegression.java
* Execution: java -cp .:jama.jar PolynomialRegression
* Dependencies: jama.jar StdOut.java
*
* % java -cp .:jama.jar PolynomialRegression
* 0.01 n^3 + -1.64 n^2 + 168.92 n + -2113.73 (R^2 = 0.997)
/**
* **************************************************************************** Compilation: javac
* -cp .:jama.jar PolynomialRegression.java Execution: java -cp .:jama.jar PolynomialRegression
* Dependencies: jama.jar StdOut.java
*
* <p>% java -cp .:jama.jar PolynomialRegression 0.01 n^3 + -1.64 n^2 + 168.92 n + -2113.73 (R^2 =
* 0.997)
*
******************************************************************************/
* <p>****************************************************************************
*/

import Jama.Matrix;
import Jama.QRDecomposition;

/**
* The {@code PolynomialRegression} class performs a polynomial regression on an set of <em>N</em> data points (
* <em>y<sub>i</sub></em>, <em>x<sub>i</sub></em>). That is, it fits a polynomial <em>y</em> = &beta;<sub>0</sub> +
* &beta;<sub>1</sub> <em>x</em> + &beta;<sub>2</sub> <em>x</em><sup>2</sup> + ... + &beta;<sub><em>d</em></sub>
* <em>x</em><sup><em>d</em></sup> (where <em>y</em> is the response variable, <em>x</em> is the predictor variable, and
* the &beta;<sub><em>i</em></sub> are the regression coefficients) that minimizes the sum of squared residuals of the
* multiple regression model. It also computes associated the coefficient of determination <em>R</em><sup>2</sup>.
* <p>
* This implementation performs a QR-decomposition of the underlying Vandermonde matrix, so it is not the fastest or
* most numerically stable way to perform the polynomial regression.
* The {@code PolynomialRegression} class performs a polynomial regression on an set of <em>N</em>
* data points ( <em>y<sub>i</sub></em>, <em>x<sub>i</sub></em>). That is, it fits a polynomial
* <em>y</em> = &beta;<sub>0</sub> + &beta;<sub>1</sub> <em>x</em> + &beta;<sub>2</sub>
* <em>x</em><sup>2</sup> + ... + &beta;<sub><em>d</em></sub> <em>x</em><sup><em>d</em></sup> (where
* <em>y</em> is the response variable, <em>x</em> is the predictor variable, and the
* &beta;<sub><em>i</em></sub> are the regression coefficients) that minimizes the sum of squared
* residuals of the multiple regression model. It also computes associated the coefficient of
* determination <em>R</em><sup>2</sup>.
*
* <p>This implementation performs a QR-decomposition of the underlying Vandermonde matrix, so it is
* not the fastest or most numerically stable way to perform the polynomial regression.
*
* @author Robert Sedgewick
* @author Kevin Wayne
*/
public class PolynomialRegression {
private int degree; // degree of the polynomial regression
private Matrix beta; // the polynomial regression coefficients
private double sse; // sum of squares due to error
private double sst; // total sum of squares

public PolynomialRegression(double[][] xy, int degree) {
double[] x = new double[xy.length];
double[] y = new double[xy.length];
for (int i = 0; i < xy.length; ++i) {
x[i] = xy[i][0];
y[i] = xy[i][1];
}
solve(x, y, degree);
private int degree; // degree of the polynomial regression
private Matrix beta; // the polynomial regression coefficients
private double sse; // sum of squares due to error
private double sst; // total sum of squares

public PolynomialRegression(double[][] xy, int degree) {
double[] x = new double[xy.length];
double[] y = new double[xy.length];
for (int i = 0; i < xy.length; ++i) {
x[i] = xy[i][0];
y[i] = xy[i][1];
}

/**
* Performs a polynomial regression on the data points {@code (y[i], x[i])}.
*
* @param x
* the values of the predictor variable
* @param y
* the corresponding values of the response variable
* @param degree
* the degree of the polynomial to fit
*/
public PolynomialRegression(double[] x, double[] y, int degree) {
solve(x, y, degree);
}

private void solve(double[] x, double[] y, int degree) {
this.degree = degree;

int n = x.length;
QRDecomposition qr = null;
Matrix matrixX = null;

// in case Vandermonde matrix does not have full rank, reduce degree until it does
while (true) {

// build Vandermonde matrix
double[][] vandermonde = new double[n][this.degree + 1];
for (int i = 0; i < n; i++) {
for (int j = 0; j <= this.degree; j++) {
vandermonde[i][j] = Math.pow(x[i], j);
}
}
matrixX = new Matrix(vandermonde);

// find least squares solution
qr = new QRDecomposition(matrixX);
if (qr.isFullRank())
break;

// decrease degree and try again
this.degree--;
solve(x, y, degree);
}

/**
* Performs a polynomial regression on the data points {@code (y[i], x[i])}.
*
* @param x the values of the predictor variable
* @param y the corresponding values of the response variable
* @param degree the degree of the polynomial to fit
*/
public PolynomialRegression(double[] x, double[] y, int degree) {
solve(x, y, degree);
}

private void solve(double[] x, double[] y, int degree) {
this.degree = degree;

int n = x.length;
QRDecomposition qr = null;
Matrix matrixX = null;

// in case Vandermonde matrix does not have full rank, reduce degree until it does
while (true) {

// build Vandermonde matrix
double[][] vandermonde = new double[n][this.degree + 1];
for (int i = 0; i < n; i++) {
for (int j = 0; j <= this.degree; j++) {
vandermonde[i][j] = Math.pow(x[i], j);
}
}
matrixX = new Matrix(vandermonde);

// create matrix from vector
Matrix matrixY = new Matrix(y, n);

// linear regression coefficients
beta = qr.solve(matrixY);

// mean of y[] values
double sum = 0.0;
for (int i = 0; i < n; i++)
sum += y[i];
double mean = sum / n;

// total variation to be accounted for
for (int i = 0; i < n; i++) {
double dev = y[i] - mean;
sst += dev * dev;
}
// find least squares solution
qr = new QRDecomposition(matrixX);
if (qr.isFullRank()) break;

// variation not accounted for
Matrix residuals = matrixX.times(beta).minus(matrixY);
sse = residuals.norm2() * residuals.norm2();
// decrease degree and try again
this.degree--;
}

/**
* Returns the {@code j}th regression coefficient.
*
* @param j
* the index
* @return the {@code j}th regression coefficient
*/
public double beta(int j) {
// to make -0.0 print as 0.0
if (Math.abs(beta.get(j, 0)) < 1E-4)
return 0.0;
return beta.get(j, 0);
}
// create matrix from vector
Matrix matrixY = new Matrix(y, n);

/**
* Returns the degree of the polynomial to fit.
*
* @return the degree of the polynomial to fit
*/
public int degree() {
return degree;
}
// linear regression coefficients
beta = qr.solve(matrixY);

/**
* Returns the coefficient of determination <em>R</em><sup>2</sup>.
*
* @return the coefficient of determination <em>R</em><sup>2</sup>, which is a real number between 0 and 1
*/
public double R2() {
if (sst == 0.0)
return 1.0; // constant function
return 1.0 - sse / sst;
}
// mean of y[] values
double sum = 0.0;
for (int i = 0; i < n; i++) sum += y[i];
double mean = sum / n;

/**
* Returns the expected response {@code y} given the value of the predictor variable {@code x}.
*
* @param x
* the value of the predictor variable
* @return the expected response {@code y} given the value of the predictor variable {@code x}
*/
public double predict(double x) {
// horner's method
double y = 0.0;
for (int j = degree; j >= 0; j--)
y = beta(j) + (x * y);
return y;
// total variation to be accounted for
for (int i = 0; i < n; i++) {
double dev = y[i] - mean;
sst += dev * dev;
}

@Override
public String toString() {
StringBuilder s = new StringBuilder();
int j = degree;

// ignoring leading zero coefficients
while (j >= 0 && Math.abs(beta(j)) < 1E-5)
j--;

// create remaining terms
while (j >= 0) {
if (j == 0)
s.append(String.format("%.2f ", beta(j)));
else if (j == 1)
s.append(String.format("%.2f x + ", beta(j)));
else
s.append(String.format("%.2f x^%d + ", beta(j), j));
j--;
}
s = s.append(" (R^2 = " + String.format("%.3f", R2()) + ")");
return s.toString();
// variation not accounted for
Matrix residuals = matrixX.times(beta).minus(matrixY);
sse = residuals.norm2() * residuals.norm2();
}

/**
* Returns the {@code j}th regression coefficient.
*
* @param j the index
* @return the {@code j}th regression coefficient
*/
public double beta(int j) {
// to make -0.0 print as 0.0
if (Math.abs(beta.get(j, 0)) < 1E-4) return 0.0;
return beta.get(j, 0);
}

/**
* Returns the degree of the polynomial to fit.
*
* @return the degree of the polynomial to fit
*/
public int degree() {
return degree;
}

/**
* Returns the coefficient of determination <em>R</em><sup>2</sup>.
*
* @return the coefficient of determination <em>R</em><sup>2</sup>, which is a real number between
* 0 and 1
*/
public double R2() {
if (sst == 0.0) return 1.0; // constant function
return 1.0 - sse / sst;
}

/**
* Returns the expected response {@code y} given the value of the predictor variable {@code x}.
*
* @param x the value of the predictor variable
* @return the expected response {@code y} given the value of the predictor variable {@code x}
*/
public double predict(double x) {
// horner's method
double y = 0.0;
for (int j = degree; j >= 0; j--) y = beta(j) + (x * y);
return y;
}

@Override
public String toString() {
StringBuilder s = new StringBuilder();
int j = degree;

// ignoring leading zero coefficients
while (j >= 0 && Math.abs(beta(j)) < 1E-5) j--;

// create remaining terms
while (j >= 0) {
if (j == 0) s.append(String.format("%.2f ", beta(j)));
else if (j == 1) s.append(String.format("%.2f x + ", beta(j)));
else s.append(String.format("%.2f x^%d + ", beta(j), j));
j--;
}
s = s.append(" (R^2 = " + String.format("%.3f", R2()) + ")");
return s.toString();
}
}
345 src/com/spartronics4915/lib/util/math/RigidTransform2d.java
View file
@@ -6,179 +6,178 @@

/**
* Represents a 2d pose (rigid transform) containing translational and rotational elements.
*
* Inspired by Sophus (https://github.com/strasdat/Sophus/tree/master/sophus)
*
* <p>Inspired by Sophus (https://github.com/strasdat/Sophus/tree/master/sophus)
*/
public class RigidTransform2d implements Interpolable<RigidTransform2d> {
protected static final double kEpsilon = 1E-9;

protected static final RigidTransform2d kIdentity = new RigidTransform2d();

public static final RigidTransform2d identity() {
return kIdentity;
}

private final static double kEps = 1E-9;

protected Translation2d translation_;
protected Rotation2d rotation_;

public RigidTransform2d() {
translation_ = new Translation2d();
rotation_ = new Rotation2d();
}

public RigidTransform2d(Translation2d translation, Rotation2d rotation) {
translation_ = translation;
rotation_ = rotation;
}

public RigidTransform2d(RigidTransform2d other) {
translation_ = new Translation2d(other.translation_);
rotation_ = new Rotation2d(other.rotation_);
}

public static RigidTransform2d fromTranslation(Translation2d translation) {
return new RigidTransform2d(translation, new Rotation2d());
}

public static RigidTransform2d fromRotation(Rotation2d rotation) {
return new RigidTransform2d(new Translation2d(), rotation);
}

/**
* Obtain a new RigidTransform2d from a (constant curvature) velocity. See:
* https://github.com/strasdat/Sophus/blob/master/sophus/se2.hpp
*/
public static RigidTransform2d exp(Twist2d delta) {
double sin_theta = Math.sin(delta.dtheta);
double cos_theta = Math.cos(delta.dtheta);
double s, c;
if (Math.abs(delta.dtheta) < kEps) {
s = 1.0 - 1.0 / 6.0 * delta.dtheta * delta.dtheta;
c = .5 * delta.dtheta;
} else {
s = sin_theta / delta.dtheta;
c = (1.0 - cos_theta) / delta.dtheta;
}
return new RigidTransform2d(new Translation2d(delta.dx * s - delta.dy * c, delta.dx * c + delta.dy * s),
new Rotation2d(cos_theta, sin_theta, false));
}

/**
* Logical inverse of the above.
*/
public static Twist2d log(RigidTransform2d transform) {
final double dtheta = transform.getRotation().getRadians();
final double half_dtheta = 0.5 * dtheta;
final double cos_minus_one = transform.getRotation().cos() - 1.0;
double halftheta_by_tan_of_halfdtheta;
if (Math.abs(cos_minus_one) < kEps) {
halftheta_by_tan_of_halfdtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
} else {
halftheta_by_tan_of_halfdtheta = -(half_dtheta * transform.getRotation().sin()) / cos_minus_one;
}
final Translation2d translation_part = transform.getTranslation()
.rotateBy(new Rotation2d(halftheta_by_tan_of_halfdtheta, -half_dtheta, false));
return new Twist2d(translation_part.x(), translation_part.y(), dtheta);
}

public Translation2d getTranslation() {
return translation_;
}

public void setTranslation(Translation2d translation) {
translation_ = translation;
}

public Rotation2d getRotation() {
return rotation_;
}

public void setRotation(Rotation2d rotation) {
rotation_ = rotation;
}

/**
* Transforming this RigidTransform2d means first translating by other.translation and then rotating by
* other.rotation
*
* @param other
* The other transform.
* @return This transform * other
*/
public RigidTransform2d transformBy(RigidTransform2d other) {
return new RigidTransform2d(translation_.translateBy(other.translation_.rotateBy(rotation_)),
rotation_.rotateBy(other.rotation_));
}

/**
* The inverse of this transform "undoes" the effect of translating by this transform.
*
* @return The opposite of this transform.
*/
public RigidTransform2d inverse() {
Rotation2d rotation_inverted = rotation_.inverse();
return new RigidTransform2d(translation_.inverse().rotateBy(rotation_inverted), rotation_inverted);
}

public RigidTransform2d normal() {
return new RigidTransform2d(translation_, rotation_.normal());
}

/**
* Finds the point where the heading of this transform intersects the heading of another. Returns (+INF, +INF) if
* parallel.
*/
public Translation2d intersection(RigidTransform2d other) {
final Rotation2d other_rotation = other.getRotation();
if (rotation_.isParallel(other_rotation)) {
// Lines are parallel.
return new Translation2d(Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY);
}
if (Math.abs(rotation_.cos()) < Math.abs(other_rotation.cos())) {
return intersectionInternal(this, other);
} else {
return intersectionInternal(other, this);
}
}

/**
* Return true if the heading of this transform is colinear with the heading of another.
*/
public boolean isColinear(RigidTransform2d other) {
final Twist2d twist = log(inverse().transformBy(other));
return (epsilonEquals(twist.dy, 0.0, kEpsilon) && epsilonEquals(twist.dtheta, 0.0, kEpsilon));
}

private static Translation2d intersectionInternal(RigidTransform2d a, RigidTransform2d b) {
final Rotation2d a_r = a.getRotation();
final Rotation2d b_r = b.getRotation();
final Translation2d a_t = a.getTranslation();
final Translation2d b_t = b.getTranslation();

final double tan_b = b_r.tan();
final double t = ((a_t.x() - b_t.x()) * tan_b + b_t.y() - a_t.y())
/ (a_r.sin() - a_r.cos() * tan_b);
return a_t.translateBy(a_r.toTranslation().scale(t));
}

/**
* Do twist interpolation of this transform assuming constant curvature.
*/
@Override
public RigidTransform2d interpolate(RigidTransform2d other, double x) {
if (x <= 0) {
return new RigidTransform2d(this);
} else if (x >= 1) {
return new RigidTransform2d(other);
}
final Twist2d twist = RigidTransform2d.log(inverse().transformBy(other));
return transformBy(RigidTransform2d.exp(twist.scaled(x)));
}

@Override
public String toString() {
return "T:" + translation_.toString() + ", R:" + rotation_.toString();
}
protected static final double kEpsilon = 1E-9;

protected static final RigidTransform2d kIdentity = new RigidTransform2d();

public static final RigidTransform2d identity() {
return kIdentity;
}

private static final double kEps = 1E-9;

protected Translation2d translation_;
protected Rotation2d rotation_;

public RigidTransform2d() {
translation_ = new Translation2d();
rotation_ = new Rotation2d();
}

public RigidTransform2d(Translation2d translation, Rotation2d rotation) {
translation_ = translation;
rotation_ = rotation;
}

public RigidTransform2d(RigidTransform2d other) {
translation_ = new Translation2d(other.translation_);
rotation_ = new Rotation2d(other.rotation_);
}

public static RigidTransform2d fromTranslation(Translation2d translation) {
return new RigidTransform2d(translation, new Rotation2d());
}

public static RigidTransform2d fromRotation(Rotation2d rotation) {
return new RigidTransform2d(new Translation2d(), rotation);
}

/**
* Obtain a new RigidTransform2d from a (constant curvature) velocity. See:
* https://github.com/strasdat/Sophus/blob/master/sophus/se2.hpp
*/
public static RigidTransform2d exp(Twist2d delta) {
double sin_theta = Math.sin(delta.dtheta);
double cos_theta = Math.cos(delta.dtheta);
double s, c;
if (Math.abs(delta.dtheta) < kEps) {
s = 1.0 - 1.0 / 6.0 * delta.dtheta * delta.dtheta;
c = .5 * delta.dtheta;
} else {
s = sin_theta / delta.dtheta;
c = (1.0 - cos_theta) / delta.dtheta;
}
return new RigidTransform2d(
new Translation2d(delta.dx * s - delta.dy * c, delta.dx * c + delta.dy * s),
new Rotation2d(cos_theta, sin_theta, false));
}

/** Logical inverse of the above. */
public static Twist2d log(RigidTransform2d transform) {
final double dtheta = transform.getRotation().getRadians();
final double half_dtheta = 0.5 * dtheta;
final double cos_minus_one = transform.getRotation().cos() - 1.0;
double halftheta_by_tan_of_halfdtheta;
if (Math.abs(cos_minus_one) < kEps) {
halftheta_by_tan_of_halfdtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
} else {
halftheta_by_tan_of_halfdtheta =
-(half_dtheta * transform.getRotation().sin()) / cos_minus_one;
}
final Translation2d translation_part =
transform
.getTranslation()
.rotateBy(new Rotation2d(halftheta_by_tan_of_halfdtheta, -half_dtheta, false));
return new Twist2d(translation_part.x(), translation_part.y(), dtheta);
}

public Translation2d getTranslation() {
return translation_;
}

public void setTranslation(Translation2d translation) {
translation_ = translation;
}

public Rotation2d getRotation() {
return rotation_;
}

public void setRotation(Rotation2d rotation) {
rotation_ = rotation;
}

/**
* Transforming this RigidTransform2d means first translating by other.translation and then
* rotating by other.rotation
*
* @param other The other transform.
* @return This transform * other
*/
public RigidTransform2d transformBy(RigidTransform2d other) {
return new RigidTransform2d(
translation_.translateBy(other.translation_.rotateBy(rotation_)),
rotation_.rotateBy(other.rotation_));
}

/**
* The inverse of this transform "undoes" the effect of translating by this transform.
*
* @return The opposite of this transform.
*/
public RigidTransform2d inverse() {
Rotation2d rotation_inverted = rotation_.inverse();
return new RigidTransform2d(
translation_.inverse().rotateBy(rotation_inverted), rotation_inverted);
}

public RigidTransform2d normal() {
return new RigidTransform2d(translation_, rotation_.normal());
}

/**
* Finds the point where the heading of this transform intersects the heading of another. Returns
* (+INF, +INF) if parallel.
*/
public Translation2d intersection(RigidTransform2d other) {
final Rotation2d other_rotation = other.getRotation();
if (rotation_.isParallel(other_rotation)) {
// Lines are parallel.
return new Translation2d(Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY);
}
if (Math.abs(rotation_.cos()) < Math.abs(other_rotation.cos())) {
return intersectionInternal(this, other);
} else {
return intersectionInternal(other, this);
}
}

/** Return true if the heading of this transform is colinear with the heading of another. */
public boolean isColinear(RigidTransform2d other) {
final Twist2d twist = log(inverse().transformBy(other));
return (epsilonEquals(twist.dy, 0.0, kEpsilon) && epsilonEquals(twist.dtheta, 0.0, kEpsilon));
}

private static Translation2d intersectionInternal(RigidTransform2d a, RigidTransform2d b) {
final Rotation2d a_r = a.getRotation();
final Rotation2d b_r = b.getRotation();
final Translation2d a_t = a.getTranslation();
final Translation2d b_t = b.getTranslation();

final double tan_b = b_r.tan();
final double t =
((a_t.x() - b_t.x()) * tan_b + b_t.y() - a_t.y()) / (a_r.sin() - a_r.cos() * tan_b);
return a_t.translateBy(a_r.toTranslation().scale(t));
}

/** Do twist interpolation of this transform assuming constant curvature. */
@Override
public RigidTransform2d interpolate(RigidTransform2d other, double x) {
if (x <= 0) {
return new RigidTransform2d(this);
} else if (x >= 1) {
return new RigidTransform2d(other);
}
final Twist2d twist = RigidTransform2d.log(inverse().transformBy(other));
return transformBy(RigidTransform2d.exp(twist.scaled(x)));
}

@Override
public String toString() {
return "T:" + translation_.toString() + ", R:" + rotation_.toString();
}
}
272 src/com/spartronics4915/lib/util/math/Rotation2d.java
View file
@@ -2,145 +2,147 @@

import static com.spartronics4915.lib.util.Util.epsilonEquals;

import java.text.DecimalFormat;

import com.spartronics4915.lib.util.Interpolable;
import java.text.DecimalFormat;

/**
* A rotation in a 2d coordinate frame represented a point on the unit circle (cosine and sine).
*
* Inspired by Sophus (https://github.com/strasdat/Sophus/tree/master/sophus)
*
* <p>Inspired by Sophus (https://github.com/strasdat/Sophus/tree/master/sophus)
*/
public class Rotation2d implements Interpolable<Rotation2d> {
protected static final Rotation2d kIdentity = new Rotation2d();

public static final Rotation2d identity() {
return kIdentity;
}

protected static final double kEpsilon = 1E-9;

protected double cos_angle_;
protected double sin_angle_;

public Rotation2d() {
this(1, 0, false);
}

public Rotation2d(double x, double y, boolean normalize) {
cos_angle_ = x;
sin_angle_ = y;
if (normalize) {
normalize();
}
}

public Rotation2d(Rotation2d other) {
cos_angle_ = other.cos_angle_;
sin_angle_ = other.sin_angle_;
}

public Rotation2d(Translation2d direction, boolean normalize) {
this(direction.x(), direction.y(), normalize);
}

public static Rotation2d fromRadians(double angle_radians) {
return new Rotation2d(Math.cos(angle_radians), Math.sin(angle_radians), false);
}

public static Rotation2d fromDegrees(double angle_degrees) {
return fromRadians(Math.toRadians(angle_degrees));
}

/**
* From trig, we know that sin^2 + cos^2 == 1, but as we do math on this object we might accumulate rounding errors.
* Normalizing forces us to re-scale the sin and cos to reset rounding errors.
*/
public void normalize() {
double magnitude = Math.hypot(cos_angle_, sin_angle_);
if (magnitude > kEpsilon) {
sin_angle_ /= magnitude;
cos_angle_ /= magnitude;
} else {
sin_angle_ = 0;
cos_angle_ = 1;
}
}

public double cos() {
return cos_angle_;
}

public double sin() {
return sin_angle_;
}

public double tan() {
if (Math.abs(cos_angle_) < kEpsilon) {
if (sin_angle_ >= 0.0) {
return Double.POSITIVE_INFINITY;
} else {
return Double.NEGATIVE_INFINITY;
}
}
return sin_angle_ / cos_angle_;
}

public double getRadians() {
return Math.atan2(sin_angle_, cos_angle_);
}

public double getDegrees() {
return Math.toDegrees(getRadians());
}

/**
* We can rotate this Rotation2d by adding together the effects of it and another rotation.
*
* @param other
* The other rotation. See: https://en.wikipedia.org/wiki/Rotation_matrix
* @return This rotation rotated by other.
*/
public Rotation2d rotateBy(Rotation2d other) {
return new Rotation2d(cos_angle_ * other.cos_angle_ - sin_angle_ * other.sin_angle_,
cos_angle_ * other.sin_angle_ + sin_angle_ * other.cos_angle_, true);
}

public Rotation2d normal() {
return new Rotation2d(-sin_angle_, cos_angle_, false);
}

/**
* The inverse of a Rotation2d "undoes" the effect of this rotation.
*
* @return The opposite of this rotation.
*/
public Rotation2d inverse() {
return new Rotation2d(cos_angle_, -sin_angle_, false);
}

public boolean isParallel(Rotation2d other) {
return epsilonEquals(Translation2d.cross(toTranslation(), other.toTranslation()), 0.0, kEpsilon);
}

public Translation2d toTranslation() {
return new Translation2d(cos_angle_, sin_angle_);
}

@Override
public Rotation2d interpolate(Rotation2d other, double x) {
if (x <= 0) {
return new Rotation2d(this);
} else if (x >= 1) {
return new Rotation2d(other);
}
double angle_diff = inverse().rotateBy(other).getRadians();
return this.rotateBy(Rotation2d.fromRadians(angle_diff * x));
}

@Override
public String toString() {
final DecimalFormat fmt = new DecimalFormat("#0.000");
return "(" + fmt.format(getDegrees()) + " deg)";
}
protected static final Rotation2d kIdentity = new Rotation2d();

public static final Rotation2d identity() {
return kIdentity;
}

protected static final double kEpsilon = 1E-9;

protected double cos_angle_;
protected double sin_angle_;

public Rotation2d() {
this(1, 0, false);
}

public Rotation2d(double x, double y, boolean normalize) {
cos_angle_ = x;
sin_angle_ = y;
if (normalize) {
normalize();
}
}

public Rotation2d(Rotation2d other) {
cos_angle_ = other.cos_angle_;
sin_angle_ = other.sin_angle_;
}

public Rotation2d(Translation2d direction, boolean normalize) {
this(direction.x(), direction.y(), normalize);
}

public static Rotation2d fromRadians(double angle_radians) {
return new Rotation2d(Math.cos(angle_radians), Math.sin(angle_radians), false);
}

public static Rotation2d fromDegrees(double angle_degrees) {
return fromRadians(Math.toRadians(angle_degrees));
}

/**
* From trig, we know that sin^2 + cos^2 == 1, but as we do math on this object we might
* accumulate rounding errors. Normalizing forces us to re-scale the sin and cos to reset rounding
* errors.
*/
public void normalize() {
double magnitude = Math.hypot(cos_angle_, sin_angle_);
if (magnitude > kEpsilon) {
sin_angle_ /= magnitude;
cos_angle_ /= magnitude;
} else {
sin_angle_ = 0;
cos_angle_ = 1;
}
}

public double cos() {
return cos_angle_;
}

public double sin() {
return sin_angle_;
}

public double tan() {
if (Math.abs(cos_angle_) < kEpsilon) {
if (sin_angle_ >= 0.0) {
return Double.POSITIVE_INFINITY;
} else {
return Double.NEGATIVE_INFINITY;
}
}
return sin_angle_ / cos_angle_;
}

public double getRadians() {
return Math.atan2(sin_angle_, cos_angle_);
}

public double getDegrees() {
return Math.toDegrees(getRadians());
}

/**
* We can rotate this Rotation2d by adding together the effects of it and another rotation.
*
* @param other The other rotation. See: https://en.wikipedia.org/wiki/Rotation_matrix
* @return This rotation rotated by other.
*/
public Rotation2d rotateBy(Rotation2d other) {
return new Rotation2d(
cos_angle_ * other.cos_angle_ - sin_angle_ * other.sin_angle_,
cos_angle_ * other.sin_angle_ + sin_angle_ * other.cos_angle_,
true);
}

public Rotation2d normal() {
return new Rotation2d(-sin_angle_, cos_angle_, false);
}

/**
* The inverse of a Rotation2d "undoes" the effect of this rotation.
*
* @return The opposite of this rotation.
*/
public Rotation2d inverse() {
return new Rotation2d(cos_angle_, -sin_angle_, false);
}

public boolean isParallel(Rotation2d other) {
return epsilonEquals(
Translation2d.cross(toTranslation(), other.toTranslation()), 0.0, kEpsilon);
}

public Translation2d toTranslation() {
return new Translation2d(cos_angle_, sin_angle_);
}

@Override
public Rotation2d interpolate(Rotation2d other, double x) {
if (x <= 0) {
return new Rotation2d(this);
} else if (x >= 1) {
return new Rotation2d(other);
}
double angle_diff = inverse().rotateBy(other).getRadians();
return this.rotateBy(Rotation2d.fromRadians(angle_diff * x));
}

@Override
public String toString() {
final DecimalFormat fmt = new DecimalFormat("#0.000");
return "(" + fmt.format(getDegrees()) + " deg)";
}
}
270 src/com/spartronics4915/lib/util/math/Translation2d.java
View file
@@ -1,143 +1,139 @@
package com.spartronics4915.lib.util.math;

import java.text.DecimalFormat;

import com.spartronics4915.lib.util.Interpolable;
import java.text.DecimalFormat;

/**
* A translation in a 2d coordinate frame. Translations are simply shifts in an (x, y) plane.
*/
/** A translation in a 2d coordinate frame. Translations are simply shifts in an (x, y) plane. */
public class Translation2d implements Interpolable<Translation2d> {
protected static final Translation2d kIdentity = new Translation2d();

public static final Translation2d identity() {
return kIdentity;
}

protected double x_;
protected double y_;

public Translation2d() {
x_ = 0;
y_ = 0;
}

public Translation2d(double x, double y) {
x_ = x;
y_ = y;
}

public Translation2d(Translation2d other) {
x_ = other.x_;
y_ = other.y_;
}

public Translation2d(Translation2d start, Translation2d end) {
x_ = end.x_ - start.x_;
y_ = end.y_ - start.y_;
}

/**
* The "norm" of a transform is the Euclidean distance in x and y.
*
* @return sqrt(x^2 + y^2)
*/
public double norm() {
return Math.hypot(x_, y_);
}

public double norm2() {
return x_ * x_ + y_ * y_;
}

public double x() {
return x_;
}

public double y() {
return y_;
}

public void setX(double x) {
x_ = x;
}

public void setY(double y) {
y_ = y;
}

/**
* We can compose Translation2d's by adding together the x and y shifts.
*
* @param other
* The other translation to add.
* @return The combined effect of translating by this object and the other.
*/
public Translation2d translateBy(Translation2d other) {
return new Translation2d(x_ + other.x_, y_ + other.y_);
}

/**
* We can also rotate Translation2d's. See: https://en.wikipedia.org/wiki/Rotation_matrix
*
* @param rotation
* The rotation to apply.
* @return This translation rotated by rotation.
*/
public Translation2d rotateBy(Rotation2d rotation) {
return new Translation2d(x_ * rotation.cos() - y_ * rotation.sin(), x_ * rotation.sin() + y_ * rotation.cos());
}

public Rotation2d direction() {
return new Rotation2d(x_, y_, true);
}

/**
* The inverse simply means a Translation2d that "undoes" this object.
*
* @return Translation by -x and -y.
*/
public Translation2d inverse() {
return new Translation2d(-x_, -y_);
}

@Override
public Translation2d interpolate(Translation2d other, double x) {
if (x <= 0) {
return new Translation2d(this);
} else if (x >= 1) {
return new Translation2d(other);
}
return extrapolate(other, x);
}

public Translation2d extrapolate(Translation2d other, double x) {
return new Translation2d(x * (other.x_ - x_) + x_, x * (other.y_ - y_) + y_);
}

public Translation2d scale(double s) {
return new Translation2d(x_ * s, y_ * s);
}

@Override
public String toString() {
final DecimalFormat fmt = new DecimalFormat("#0.000");
return "(" + fmt.format(x_) + "," + fmt.format(y_) + ")";
}

public static double dot(Translation2d a, Translation2d b) {
return a.x_ * b.x_ + a.y_ * b.y_;
}

public static Rotation2d getAngle(Translation2d a, Translation2d b) {
double cos_angle = dot(a, b) / (a.norm() * b.norm());
if (Double.isNaN(cos_angle)) {
return new Rotation2d();
}
return Rotation2d.fromRadians(Math.acos(Math.min(1.0, Math.max(cos_angle, -1.0))));
}

public static double cross(Translation2d a, Translation2d b) {
return a.x_ * b.y_ - a.y_ * b.x_;
}
protected static final Translation2d kIdentity = new Translation2d();

public static final Translation2d identity() {
return kIdentity;
}

protected double x_;
protected double y_;

public Translation2d() {
x_ = 0;
y_ = 0;
}

public Translation2d(double x, double y) {
x_ = x;
y_ = y;
}

public Translation2d(Translation2d other) {
x_ = other.x_;
y_ = other.y_;
}

public Translation2d(Translation2d start, Translation2d end) {
x_ = end.x_ - start.x_;
y_ = end.y_ - start.y_;
}

/**
* The "norm" of a transform is the Euclidean distance in x and y.
*
* @return sqrt(x^2 + y^2)
*/
public double norm() {
return Math.hypot(x_, y_);
}

public double norm2() {
return x_ * x_ + y_ * y_;
}

public double x() {
return x_;
}

public double y() {
return y_;
}

public void setX(double x) {
x_ = x;
}

public void setY(double y) {
y_ = y;
}

/**
* We can compose Translation2d's by adding together the x and y shifts.
*
* @param other The other translation to add.
* @return The combined effect of translating by this object and the other.
*/
public Translation2d translateBy(Translation2d other) {
return new Translation2d(x_ + other.x_, y_ + other.y_);
}

/**
* We can also rotate Translation2d's. See: https://en.wikipedia.org/wiki/Rotation_matrix
*
* @param rotation The rotation to apply.
* @return This translation rotated by rotation.
*/
public Translation2d rotateBy(Rotation2d rotation) {
return new Translation2d(
x_ * rotation.cos() - y_ * rotation.sin(), x_ * rotation.sin() + y_ * rotation.cos());
}

public Rotation2d direction() {
return new Rotation2d(x_, y_, true);
}

/**
* The inverse simply means a Translation2d that "undoes" this object.
*
* @return Translation by -x and -y.
*/
public Translation2d inverse() {
return new Translation2d(-x_, -y_);
}

@Override
public Translation2d interpolate(Translation2d other, double x) {
if (x <= 0) {
return new Translation2d(this);
} else if (x >= 1) {
return new Translation2d(other);
}
return extrapolate(other, x);
}

public Translation2d extrapolate(Translation2d other, double x) {
return new Translation2d(x * (other.x_ - x_) + x_, x * (other.y_ - y_) + y_);
}

public Translation2d scale(double s) {
return new Translation2d(x_ * s, y_ * s);
}

@Override
public String toString() {
final DecimalFormat fmt = new DecimalFormat("#0.000");
return "(" + fmt.format(x_) + "," + fmt.format(y_) + ")";
}

public static double dot(Translation2d a, Translation2d b) {
return a.x_ * b.x_ + a.y_ * b.y_;
}

public static Rotation2d getAngle(Translation2d a, Translation2d b) {
double cos_angle = dot(a, b) / (a.norm() * b.norm());
if (Double.isNaN(cos_angle)) {
return new Rotation2d();
}
return Rotation2d.fromRadians(Math.acos(Math.min(1.0, Math.max(cos_angle, -1.0))));
}

public static double cross(Translation2d a, Translation2d b) {
return a.x_ * b.y_ - a.y_ * b.x_;
}
}
41 src/com/spartronics4915/lib/util/math/Twist2d.java
View file
@@ -1,29 +1,30 @@
package com.spartronics4915.lib.util.math;

/**
* A movement along an arc at constant curvature and velocity. We can use ideas from "differential calculus" to create
* new RigidTransform2d's from a Twist2d and visa versa.
*
* A Twist can be used to represent a difference between two poses, a velocity, an acceleration, etc.
* A movement along an arc at constant curvature and velocity. We can use ideas from "differential
* calculus" to create new RigidTransform2d's from a Twist2d and visa versa.
*
* <p>A Twist can be used to represent a difference between two poses, a velocity, an acceleration,
* etc.
*/
public class Twist2d {
protected static final Twist2d kIdentity = new Twist2d(0.0, 0.0, 0.0);
protected static final Twist2d kIdentity = new Twist2d(0.0, 0.0, 0.0);

public static final Twist2d identity() {
return kIdentity;
}
public static final Twist2d identity() {
return kIdentity;
}

public final double dx;
public final double dy;
public final double dtheta; // Radians!
public final double dx;
public final double dy;
public final double dtheta; // Radians!

public Twist2d(double dx, double dy, double dtheta) {
this.dx = dx;
this.dy = dy;
this.dtheta = dtheta;
}
public Twist2d(double dx, double dy, double dtheta) {
this.dx = dx;
this.dy = dy;
this.dtheta = dtheta;
}

public Twist2d scaled(double scale) {
return new Twist2d(dx * scale, dy * scale, dtheta * scale);
}
}
public Twist2d scaled(double scale) {
return new Twist2d(dx * scale, dy * scale, dtheta * scale);
}
}
72 src/com/spartronics4915/lib/util/motion/HeadingProfileFollower.java
View file
@@ -3,41 +3,51 @@
import com.spartronics4915.lib.util.math.Rotation2d;

/**
* Class to deal with angle wrapping for following a heading profile. All states are assumed to be in units of degrees,
* and wrap on the interval of [-180, 180].
* Class to deal with angle wrapping for following a heading profile. All states are assumed to be
* in units of degrees, and wrap on the interval of [-180, 180].
*/
public class HeadingProfileFollower extends ProfileFollower {

public HeadingProfileFollower(double kp, double ki, double kv, double kffv, double kffa) {
super(kp, ki, kv, kffv, kffa);
}
public HeadingProfileFollower(double kp, double ki, double kv, double kffv, double kffa) {
super(kp, ki, kv, kffv, kffa);
}

@Override
public double update(MotionState latest_state, double t) {
final Rotation2d goal_rotation_inverse = Rotation2d.fromDegrees(mGoal.pos()).inverse();
// Update both the setpoint and latest state to be relative to the new goal.
if (mLatestSetpoint != null) {
mLatestSetpoint.motion_state = new MotionState(mLatestSetpoint.motion_state.t(),
mGoal.pos() + goal_rotation_inverse
.rotateBy(Rotation2d.fromDegrees(mLatestSetpoint.motion_state.pos())).getDegrees(),
mLatestSetpoint.motion_state.vel(), mLatestSetpoint.motion_state.acc());
}
final MotionState latest_state_unwrapped = new MotionState(latest_state.t(),
mGoal.pos() + goal_rotation_inverse.rotateBy(Rotation2d.fromDegrees(latest_state.pos())).getDegrees(),
latest_state.vel(), latest_state.acc());
double result = super.update(latest_state_unwrapped, t);
// Reset the integrator when we are close to the goal (encourage stiction!).
if (Math.abs(latest_state_unwrapped.pos() - mGoal.pos()) < mGoal.pos_tolerance()) {
result = 0.0;
super.resetIntegral();
}
return result;
@Override
public double update(MotionState latest_state, double t) {
final Rotation2d goal_rotation_inverse = Rotation2d.fromDegrees(mGoal.pos()).inverse();
// Update both the setpoint and latest state to be relative to the new goal.
if (mLatestSetpoint != null) {
mLatestSetpoint.motion_state =
new MotionState(
mLatestSetpoint.motion_state.t(),
mGoal.pos()
+ goal_rotation_inverse
.rotateBy(Rotation2d.fromDegrees(mLatestSetpoint.motion_state.pos()))
.getDegrees(),
mLatestSetpoint.motion_state.vel(),
mLatestSetpoint.motion_state.acc());
}

/**
* Convert a motion state representing an angle to a properly wrapped angle.
*/
public static MotionState canonicalize(MotionState state) {
return new MotionState(state.t(), Rotation2d.fromDegrees(state.pos()).getDegrees(), state.vel(), state.acc());
final MotionState latest_state_unwrapped =
new MotionState(
latest_state.t(),
mGoal.pos()
+ goal_rotation_inverse
.rotateBy(Rotation2d.fromDegrees(latest_state.pos()))
.getDegrees(),
latest_state.vel(),
latest_state.acc());
double result = super.update(latest_state_unwrapped, t);
// Reset the integrator when we are close to the goal (encourage stiction!).
if (Math.abs(latest_state_unwrapped.pos() - mGoal.pos()) < mGoal.pos_tolerance()) {
result = 0.0;
super.resetIntegral();
}
return result;
}

/** Convert a motion state representing an angle to a properly wrapped angle. */
public static MotionState canonicalize(MotionState state) {
return new MotionState(
state.t(), Rotation2d.fromDegrees(state.pos()).getDegrees(), state.vel(), state.acc());
}
}