Gyro.java

/*
 *    Copyright (C) 2016  John H. Gaby
 *
 *    This program is free software: you can redistribute it and/or modify
 *    it under the terms of the GNU General Public License as published by
 *    the Free Software Foundation, version 3 of the License.
 *
 *    This program is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *    GNU General Public License for more details.
 *
 *    You should have received a copy of the GNU General Public License
 *    along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *    
 *    Contact: robotics@gabysoft.com
 */
 
package robotCore;
 
import java.nio.ByteBuffer;
 
import org.wpilib.math.geometry.Rotation2d;
// import frc.robot.subsystems.DriveSubsystem;
import robotCore.Device.Processor;
import robotCore.Device.RequestType;

public class Gyro {
    private static final int k_maxRetry = 5;
 
    private double m_yawZero = 0;
    private double m_yaw = 0;
    private boolean m_invert = false;
    private Device m_device;
    private Object m_lock = new Object();
 
    // Support for DummyGyro
    public Gyro(int d) {
 
    }
 
    // This function will be called from another thread
    void groupCallback(ByteBuffer data, Object object) {
        // Logger.log("Gyro", 1, "groupCallback");
 
        int angle = data.getShort();
 
        synchronized (m_lock) {
            m_yaw = angle / 100.0;
        }
    }

    public Gyro() {
        Logger.log("Gyro", 1, "Wait for initialization...");
 
        m_device = Device.getInstance();
 
        for (int i = 0; i < k_maxRetry; i++) {
            int state;
 
            m_device.sendCommand(Device.k_i2cDefAddr, TwoWire.k_initGyro);
 
            /*
             * Wait for gyro to initialize
             */
            do {
                RobotCore.sleep(1000);
 
                state = m_device.getByte(Device.k_i2cDefAddr, TwoWire.k_getGyroState);
 
                Logger.log("Gyro", 1, String.format("Waiting for gyro: state = %d", state));
 
                if (state >= 1) {
                    Processor processor = Device.getInstance().getProcessor(Device.k_i2cDefAddr);
 
                    processor.addGroupRequest(RequestType.Gyro, 0, (d, o) -> groupCallback(d, o), this);
                    Device.startGroupRequests();
                    return; // success
                }
            } while (state == 0);
 
            Logger.log("Gyro", 1, "Init failed, retrying...");
        }
 
        throw new TwoWire.HardwareException("Gyro: max retries exceeded");
    }

    public static double normalize(double angle, double range) {
        angle = angle % range;
 
        if (angle > (range / 2)) {
            angle -= range;
        } else if (angle < -(range / 2)) {
            angle += range;
        }
 
        return (angle);
    }

    public static double normalizeAngle(double angle) {
        return normalize(angle, 360);
    }
 
    private double getRawYaw() {
        synchronized (m_lock) {
            return m_invert ? -m_yaw : m_yaw;
        }
    }

    public double getYaw() {
        return normalizeAngle(getRawYaw() - m_yawZero);
    }

    public Rotation2d getRotation2d() {
        return Rotation2d.fromDegrees(getYaw());
    }

    public void reset(double yaw) {
        m_yawZero = getRawYaw();
//        Logger.log("Gyro", 1, String.format("reset=%f", m_yawZero));
    }

    public void invert(boolean invert) {
        m_invert = invert;
    }
}