/// \file Joint.cpp
/// \copyright Copyright (c) 2016, Garth Zeglin.  All rights reserved. Licensed under the terms of the BSD 3-clause license.

#include "Joint.h"

void Joint::update(long interval)
{
  // Wait for the next sensor measurement time point.
  sensor_timer -= interval;
  if (sensor_timer <= 0) {
    sensor_timer += sensor_period;

    // Always process sensor input if available.
    if (position_pin >= 0) {
      raw_position = analogRead(position_pin);

      // Apply a linear calibration to convert from ADC units to degrees.
      float calibrated_raw_position = (raw_position - offset) * scale;

      // Apply a first-order low-pass filter to reduce the sampling noise.
      position += 0.5 * (calibrated_raw_position - position);
    }
  }

  // Wait for the next control cycle time point.
  control_timer -= interval;
  if (control_timer <= 0) {
    control_timer += control_period;

    // Compute
    switch (mode) {
    case IDLE:
      break;

    case OPENLOOP:
      if (push) {
	push->setDutyCycle(output_pwm);
      }
      if (pull) {
	pull->setDutyCycle(-output_pwm);
      }
      break;

    case POSITION:
      float position_error = desired_position - position;
      output_pwm = k_position * position_error;
      if (push) {
	push->setDutyCycle(output_pwm);
      }
      if (pull) {
	pull->setDutyCycle(-output_pwm);
      }
      break;
    }
  }
}
/****************************************************************/
void Joint::send_status(void)
{
  // only emit output if sensor data is available
  if (position_pin >= 0) {
    Serial.print(" ");
    Serial.print(raw_position);

    Serial.print(" ");
    Serial.print(position, 2);  // limit the output to two decimals
  }

  // always print output state
  Serial.print(" ");
  Serial.print(output_pwm, 2);  // limit the output to two decimals
}
/****************************************************************/