<commit_before>/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008-2016. All Rights Reserved.                        */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

#include "PIDController.h"
#include <math.h>
#include <vector>
#include "HAL/HAL.h"
#include "Notifier.h"
#include "PIDOutput.h"
#include "PIDSource.h"

static const std::string kP = "p";
static const std::string kI = "i";
static const std::string kD = "d";
static const std::string kF = "f";
static const std::string kSetpoint = "setpoint";
static const std::string kEnabled = "enabled";

/**
 * Allocate a PID object with the given constants for P, I, D.
 *
 * @param Kp     the proportional coefficient
 * @param Ki     the integral coefficient
 * @param Kd     the derivative coefficient
 * @param source The PIDSource object that is used to get values
 * @param output The PIDOutput object that is set to the output value
 * @param period the loop time for doing calculations. This particularly
 *               effects calculations of the integral and differental terms.
 *               The default is 50ms.
 */
PIDController::PIDController(float Kp, float Ki, float Kd, PIDSource* source,
                             PIDOutput* output, float period) {
  Initialize(Kp, Ki, Kd, 0.0f, source, output, period);
}

/**
 * Allocate a PID object with the given constants for P, I, D.
 *
 * @param Kp     the proportional coefficient
 * @param Ki     the integral coefficient
 * @param Kd     the derivative coefficient
 * @param source The PIDSource object that is used to get values
 * @param output The PIDOutput object that is set to the output value
 * @param period the loop time for doing calculations. This particularly
 *               effects calculations of the integral and differental terms.
 *               The default is 50ms.
 */
PIDController::PIDController(float Kp, float Ki, float Kd, float Kf,
                             PIDSource* source, PIDOutput* output,
                             float period) {
  Initialize(Kp, Ki, Kd, Kf, source, output, period);
}

void PIDController::Initialize(float Kp, float Ki, float Kd, float Kf,
                               PIDSource* source, PIDOutput* output,
                               float period) {
  m_controlLoop = std::make_unique<Notifier>(&PIDController::Calculate, this);

  m_P = Kp;
  m_I = Ki;
  m_D = Kd;
  m_F = Kf;

  m_pidInput = source;
  m_pidOutput = output;
  m_period = period;

  m_controlLoop->StartPeriodic(m_period);
  m_setpointTimer.Start();

  static int32_t instances = 0;
  instances++;
  HALReport(HALUsageReporting::kResourceType_PIDController, instances);
}

PIDController::~PIDController() {
  if (m_table != nullptr) m_table->RemoveTableListener(this);
}

/**
 * Read the input, calculate the output accordingly, and write to the output.
 * This should only be called by the Notifier.
 */
void PIDController::Calculate() {<commit_msg>Explicitly stop the PIDController Notifier in the destructor (#48)<commit_after>/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008-2016. All Rights Reserved.                        */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

#include "PIDController.h"
#include <math.h>
#include <vector>
#include "HAL/HAL.h"
#include "Notifier.h"
#include "PIDOutput.h"
#include "PIDSource.h"

static const std::string kP = "p";
static const std::string kI = "i";
static const std::string kD = "d";
static const std::string kF = "f";
static const std::string kSetpoint = "setpoint";
static const std::string kEnabled = "enabled";

/**
 * Allocate a PID object with the given constants for P, I, D.
 *
 * @param Kp     the proportional coefficient
 * @param Ki     the integral coefficient
 * @param Kd     the derivative coefficient
 * @param source The PIDSource object that is used to get values
 * @param output The PIDOutput object that is set to the output value
 * @param period the loop time for doing calculations. This particularly
 *               effects calculations of the integral and differental terms.
 *               The default is 50ms.
 */
PIDController::PIDController(float Kp, float Ki, float Kd, PIDSource* source,
                             PIDOutput* output, float period) {
  Initialize(Kp, Ki, Kd, 0.0f, source, output, period);
}

/**
 * Allocate a PID object with the given constants for P, I, D.
 *
 * @param Kp     the proportional coefficient
 * @param Ki     the integral coefficient
 * @param Kd     the derivative coefficient
 * @param source The PIDSource object that is used to get values
 * @param output The PIDOutput object that is set to the output value
 * @param period the loop time for doing calculations. This particularly
 *               effects calculations of the integral and differental terms.
 *               The default is 50ms.
 */
PIDController::PIDController(float Kp, float Ki, float Kd, float Kf,
                             PIDSource* source, PIDOutput* output,
                             float period) {
  Initialize(Kp, Ki, Kd, Kf, source, output, period);
}

void PIDController::Initialize(float Kp, float Ki, float Kd, float Kf,
                               PIDSource* source, PIDOutput* output,
                               float period) {
  m_controlLoop = std::make_unique<Notifier>(&PIDController::Calculate, this);

  m_P = Kp;
  m_I = Ki;
  m_D = Kd;
  m_F = Kf;

  m_pidInput = source;
  m_pidOutput = output;
  m_period = period;

  m_controlLoop->StartPeriodic(m_period);
  m_setpointTimer.Start();

  static int32_t instances = 0;
  instances++;
  HALReport(HALUsageReporting::kResourceType_PIDController, instances);
}

PIDController::~PIDController() {
  //forcefully stopping the notifier so the callback can successfully run. 
  m_controlLoop->Stop();
  if (m_table != nullptr) m_table->RemoveTableListener(this);
}

/**
 * Read the input, calculate the output accordingly, and write to the output.
 * This should only be called by the Notifier.
 */
void PIDController::Calculate() {
{'fraction_non_alphanumeric': 0.13509036144578312, 'fraction_numerical': 0.005421686746987952, 'mean_word_length': 2.3523472993437657, 'pattern_counts': {'":': 0, '<': 9, '<?xml version=': 0, '>': 14, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 4, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '3852427', 'n_tokens_mistral': 1829, 'n_tokens_neox': 1752, 'n_words': 685}