// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package org.wpilib.snippets.accelerometerfilter;

import org.wpilib.framework.TimedRobot;
import org.wpilib.hardware.imu.OnboardIMU;
import org.wpilib.hardware.imu.OnboardIMU.MountOrientation;
import org.wpilib.math.filter.LinearFilter;
import org.wpilib.smartdashboard.SmartDashboard;

/**
 * Accelerometer filtering snippets for wpilib-docs.
 * https://docs.wpilib.org/en/stable/docs/software/hardware-apis/sensors/accelerometers-software.html
 */
public class Robot extends TimedRobot {
  OnboardIMU accelerometer = new OnboardIMU(MountOrientation.FLAT);
  // Create a LinearFilter that will calculate a moving average of the measured X acceleration over
  // the past 10 iterations of the main loop
  LinearFilter xAccelFilter = LinearFilter.movingAverage(10);

  @Override
  public void robotPeriodic() {
    double xAccel = accelerometer.getAccelX();
    // Get the filtered X acceleration
    double filteredXAccel = xAccelFilter.calculate(xAccel);

    SmartDashboard.putNumber("X Acceleration", xAccel);
    SmartDashboard.putNumber("Filtered X Acceleration", filteredXAccel);
  }
}
