/// \file ValveControl.ino
///
/// \brief Hardware I/O driver for controlling pneumatic valves using a simple message protocol.
///
/// \copyright Copyright (c) 2014-2016, Garth Zeglin.  All rights
///            reserved. Licensed under the terms of the BSD 3-clause license.
///
/// \details This example is intended as a starting point for creating custom
///          firmware for driving one or more pneumatic actuators. It includes a
///          simple ASCII protocol for sending motion commands to ease
///          connecting to dynamic code (e.g. Max or Python) running on a laptop
///          or Raspberry Pi.

#include "Pneumatic.h"
#include "Joint.h"

/****************************************************************/
/**** ASCII messaging scheme ************************************/
/****************************************************************/

// The message protocol is based on commands encoded as a sequence of string
// tokens and integers in a line of text.  One line is one message.  All the
// input message formats begin with a string naming a specific command or
// destination followed by one or two argument integers.  The output formats are
// similar but include more general debugging output with a variable number of
// tokens.

// The following message formats are recognized by this program.

// Command	Arguments		Meaning
// ping                                 query whether the controller is running
// stop					set all valve channels to a neutral state
// led		<value>			controls the built-in LED, value is 0 or non-zero
// speed	<axis> <value>		sets the PWM rate and direction; axis is an integer from 1 to N, value ranges over -100 to 100
// pos          <axis> <value>		sets the joint position target; axis is an integer from 1 to N, value is in degrees

// This program generates the following messages:

// Command	Arguments		Meaning
// awake                                initialization has completed or ping was received
// led		<value>			reply with current LED state

/****************************************************************/
/**** Global variables and constants ****************************/
/****************************************************************/

// The baud rate is the number of bits per second transmitted over the serial port.
#define BAUD_RATE 115200

// Some versions of the Arduino IDE don't correctly define this symbol for an
// Arduino Uno.  Note that this pin definition is potentially shared with
// SPINDLE_DIR_PIN.
#ifndef LED_BUILTIN
#define LED_BUILTIN 13
#endif

/****************************************************************/
/// Property specification for each pneumatic axis.  This is used to define an
/// initialization table for the specific connected hardware.
struct axis_config_t {
  int8_t fill, empty;
  enum Pneumatic::pneumatic_config_t config;
};

#if 1

// Declare the hardware configuration for a valve card in the valve stack rack.
static struct axis_config_t config_table[] = {
  { 2, 3, Pneumatic::FILL_EMPTY },
  { 4, 5, Pneumatic::FILL_EMPTY },
  { 6, 7, Pneumatic::FILL_EMPTY },
  { 8, 9, Pneumatic::FILL_EMPTY }
};
#endif

#if 0
// Declare the hardware configuration for the 'instructor station'.
static struct axis_config_t config_table[] = {
  // { 3, 5, Pneumatic::PROP_FILL_PROP_EMPTY },
  // { 6, 9, Pneumatic::PROP_FILL_PROP_EMPTY },

  { 3, 5, Pneumatic::FILL_EMPTY },
  { 6, 9, Pneumatic::FILL_EMPTY },
  
  { 2, 4, Pneumatic::FILL_EMPTY },
  { 7, -1, Pneumatic::FILL_THREEWAY },
  { 8, -1, Pneumatic::FILL_THREEWAY }
};
#endif

/// Compute the number of entries in the axis table in use.
#define NUM_CHANNELS (sizeof(config_table) / sizeof(struct axis_config_t))

/// Control object for each pneumatic channel. The declaration
/// statically initializes the global state objects for the
/// channels.  Note that this does not initialize the hardware;
/// that is performed in setup().
static Pneumatic channel[NUM_CHANNELS];

/// Define the number of physical axes to control.
#define NUM_JOINTS 2

/// Control object for each closed-loop joint controller.  These objects manage
/// the sensor input for each joint and can generate control signals to one or
/// more Pneumatic objects.  The declaration statically initializes the global
/// state objects for the axes.  Note that this does not initialize the
/// hardware; that is performed in setup().
static Joint controller[NUM_JOINTS];

/****************************************************************/
/****************************************************************/
/// Process an input message received from the USB port.
/// Unrecognized commands are silently ignored.

/// \param argc		number of argument tokens
/// \param argv		array of pointers to strings, one per token

static void vc_parse_input_message(int argc, char *argv[])
{
  // Interpret the first token as a command symbol.
  char *command = argv[0];

  /* -- process zero-argument commands --------------------------- */
  if (argc == 1) {
    if ( string_equal( command, "ping" )) {
      send_message("awake");

    } else if (string_equal(command, "stop")) {
      // stop all feedback control and close off all manifolds
      for (int i = 0; i < NUM_JOINTS; i++)   controller[i].setMode(Joint::IDLE);
      for (int i = 0; i < NUM_CHANNELS; i++) channel[i].setDutyCycle(0.0);

    } else if (string_equal(command, "empty")) {
      // stop all feedback control and empty all manifolds
      for (int i = 0; i < NUM_JOINTS; i++)   controller[i].setMode(Joint::IDLE);
      for (int i = 0; i < NUM_CHANNELS; i++) channel[i].setDutyCycle(-1.0);
    }
  }

  /* -- process one-argument commands --------------------------- */
  else if (argc == 2) {
    long value = atol(argv[1] );

    // Process the 'led' command.
    if ( string_equal( command, "led" )) {
#ifdef LED_BUILTIN
      // turn on the LED if that value is true, then echo it back as a handshake
      digitalWrite(LED_BUILTIN, (value != 0) ? HIGH : LOW);
#endif
      send_message( "led", value );
    }
  }

  /* -- process two-argument commands --------------------------- */
  else if (argc == 3) {
    
    if ( string_equal( command, "speed" )) {
      int axis = atoi(argv[1]);
      int value = atoi(argv[2]);
      // takes an integer value between -100 and 100
      if (axis > 0 && axis <= NUM_CHANNELS) channel[axis-1].setDutyCycle( 0.01 * value );
    }
    else if ( string_equal( command, "pos" )) {
      int axis = atoi(argv[1]); // joint number starting with 1
      int value = atoi(argv[2]);  // in degrees
      if (axis > 0 && axis <= NUM_JOINTS) {
	controller[axis-1].setMode(Joint::POSITION);
	controller[axis-1].setTargetPosition((float) value);
      }
    }
    else if ( string_equal( command, "offset" )) {
      int axis = atoi(argv[1]); // joint number starting with 1
      int value = atoi(argv[2]);  // in raw ADC units
      if (axis > 0 && axis <= NUM_JOINTS) controller[axis-1].setOffset(value);
    }

    else if ( string_equal( command, "scale" )) {
      int axis = atoi(argv[1]); // joint number starting with 1
      float value = atof(argv[2]);  // in degree/raw ADC units
      if (axis > 0 && axis <= NUM_JOINTS) controller[axis-1].setScale(value);
    }
  }
}

/****************************************************************/
/// Polling function to update all background control tasks.

static void vc_hardware_poll(long interval)
{
  for (int i = 0; i < NUM_JOINTS; i++) {
    controller[i].update(interval);
  }
  for (int i = 0; i < NUM_CHANNELS; i++) {
    channel[i].update(interval);
  }
}

/****************************************************************/
/**** Standard entry points for Arduino system ******************/
/****************************************************************/

/// Standard Arduino initialization function to configure the system.  This code
/// will need to be modified to initialize the channel[] array with Pneumatic
/// objects matching the specific valve and manifold configuration, and the
/// controller[] array with the physical axis specifications.
void setup(void)
{
  // Initialize the array of axis control objects and valve I/O as soon as possible.
  for (int i = 0; i < NUM_CHANNELS; i++) {
    channel[i] = Pneumatic(config_table[i].fill, config_table[i].empty, config_table[i].config);
    channel[i].begin();
  }

  // Initialize a closed loop controller. The scale and offset were calculated empirically and
  // will need to be adjusted for every individual joint.

  // single-ended test:
  // controller[0] = Joint(Joint::SINGLE_FILL_EMPTY, &channel[0], NULL, A0, -0.338, 608);

  // Our default joints are double-ended:
  controller[0] = Joint(Joint::DUAL_FILL_EMPTY, &channel[0], &channel[1], A0, 0.338, 512);
  controller[1] = Joint(Joint::DUAL_FILL_EMPTY, &channel[2], &channel[3], A1, 0.338, 512);
  
#ifdef LED_BUILTIN
  pinMode( LED_BUILTIN, OUTPUT );
#endif

  // initialize the Serial port
  Serial.begin( BAUD_RATE );

  // additional hardware configuration can go here

  // send a wakeup message
  send_message("awake");
}

/****************************************************************/
/// Standard Arduino polling function to handle all I/O and periodic processing.
/// This function is called repeatedly as fast as possible from within the
/// built-in library to poll program events.  This loop should never be allowed
/// to stall or block so that all tasks can be constantly serviced.
void loop()
{
  // The timestamp in microseconds for the last polling cycle, used to compute
  // the exact interval between output updates.
  static unsigned long last_update_clock = 0;
  
  // Read the microsecond clock.
  unsigned long now = micros();

  // Compute the time elapsed since the last poll.  This will correctly handle wrapround of
  // the 32-bit long time value given the properties of twos-complement arithmetic.
  unsigned long interval = now - last_update_clock;
  last_update_clock = now;

  // Begin the polling cycle.
  vc_hardware_poll(interval);
  vc_serial_input_poll(interval);
  vc_status_output_poll(interval);
  
  // other polled tasks can go here
}

/****************************************************************/
void vc_status_output_poll(long interval)
{
  static long timer = 0;
  const long period = 100000;  // 10 Hz
  
  // Wait for the next status output event time point.
  timer -= interval;
  if (timer <= 0) {
    timer += period;
    controller[0].send_status();
    Serial.print(" ");
    controller[1].send_status();
    Serial.println();
  }
}
/****************************************************************/

/// \file Joint.h
/// \brief Pneumatic axis controller for one joint.
/// \copyright Copyright (c) 2015-2016, Garth Zeglin.  All rights reserved. Licensed under the terms of the BSD 3-clause license.
/// \details Pneumatic axis controller for one joint, using separate fill and empty valves.

#ifndef __JOINT_H_INCLUDED__
#define __JOINT_H_INCLUDED__

#include <Arduino.h>
#include <stdint.h>

#include "Pneumatic.h"

/// An instance of this class manages generation of fill and empty signals for
/// one joint axis.
class Joint {

public:
  /// Valve configuration for the axis.
  enum joint_config_t { SINGLE_FILL_EMPTY,        ///< single set of fill/empty valves, with gravity or spring return
			DUAL_FILL_EMPTY,          ///< double set of fill/empty valves to actively push and pull
			NUM_JOINT_CONFIGURATIONS  ///< the total number of defined valve configuration modes
  };

  /// Control modes for the axis.
  enum joint_state_t { IDLE,             ///< no control computed, no outputs updated
		       POSITION,         ///< proportional position control
		       NUM_JOINT_MODES   ///< the total number of axis control modes
  };
  
private:
  /****************************************************************/
  // The following instance variables may only be modified from a non-interrupt
  // context, i.e., not within poll().

  /// The underlying Pneumatic objects manage generate valve switching signals.
  Pneumatic *push, *pull;
  
  /// The I/O pins for this channel designated using the Arduino convention.
  int8_t position_pin;  ///< pin designator for the analog position input (e.g. A0)

  /// Configuration of the valve set for this axis.
  enum joint_config_t config;
  
  /// Countdown for the sensor measurement period, in microseconds.
  long sensor_timer;

  /// Duration of the sensor measurement cycle, in microseconds.
  long sensor_period;

  /// Countdown for the control period, in microseconds.
  long control_timer;

  /// Duration of the control cycle, in microseconds.
  long control_period;

  /// Raw position signal, in ADC units.
  int raw_position;

  /// Filtered position signal, in degrees.
  float position;

  /// Scaling between ADC units and degrees, units are degree/ADC unit.
  float scale;

  /// Offset subtracted from raw ADC signal prior to scaling, in ADC units.
  int offset;

  /// Current position target.
  float desired_position;

  /// Proportional position gain, units are 1/degree.
  float k_position;

  /// Current output PWM rate.
  float output_pwm;
  
  /// Current control mode for single axis.
  enum joint_state_t mode;

  /****************************************************************/
  
public:
			     
  /// Default constructor called during array initialization.  Note that this is
  /// a no-op, since each object needs to be properly initalized with unique pin
  /// numbers later.
  Joint() { }
      
  /// Main constructor.  Note: this does not initialize the underlying hardware.
  Joint( enum joint_config_t _config, Pneumatic *_push, Pneumatic *_pull = NULL, int8_t _position_pin = -1, float _scale = 0.32, float _offset = 512 ) {
    push             = _push;
    pull             = _pull;
    position_pin     = _position_pin;
    config    	     = _config;
    control_timer    = 0;
    sensor_timer     = 0;
    sensor_period    = 5000; // 200Hz
    control_period   = 50000; // 20Hz
    raw_position     = 0;
    position         = 0.0;
    scale            = _scale;
    offset           = _offset;
    mode             = IDLE;
    desired_position = 0.0;
    k_position       = 1.0 / 30.0;   // full-on PWM at 30 degrees error
    output_pwm       = 0.0;
  }

  /// Hardware initialization.  This class follows the Arduino convention of
  /// initializing instance data in the constructor but not hardware.
  void begin(void) {
    // Read the analog input once to guarantee the ADC mode and initialize the filter.
    if (position_pin >= 0) {
      raw_position = analogRead(position_pin);
      position = (raw_position - offset) * scale;
    }
  }

  /// Main polling function to be called as often as possible.  The interval
  /// argument is the duration in microseconds since the last call.
  void update(long interval);

  /// Send a status output message to the serial port.  This should not be called too often to avoid stalling.
  void send_status(void);

  /// Set the position target.
  void setTargetPosition(float _degrees) {
    desired_position = _degrees;
  }

  /// Set the control mode.
  void setMode(Joint::joint_state_t _mode) {
    mode = _mode;
  }

  /// Update the position sensor gain.
  void setScale(float _scale) {
    scale = _scale;
  }

  /// Update the position sensor offset.
  void setOffset(int _offset) {
    offset = _offset;
  }
    
};

#endif //__JOINT_H_INCLUDED__

/// \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 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)
{
  Serial.print(raw_position);

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

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

/// \file Pneumatic.h
/// \brief Pneumatic channel controller for one valve set.
/// \copyright Copyright (c) 2015-2016, Garth Zeglin.  All rights reserved. Licensed under the terms of the BSD 3-clause license.
/// \details Pneumatic axis controller for one valve set.  Different modes are available, including using separate fill and empty valves.

#ifndef __PNEUMATIC_H_INCLUDED__
#define __PNEUMATIC_H_INCLUDED__

#include <Arduino.h>
#include <stdint.h>

/// An instance of this class manages generation of fill and empty signals for
/// one pneumatic channel.
class Pneumatic {

public:
  /// Valve configuration for the channel.
  enum pneumatic_config_t { FILL_EMPTY,            ///< pair of two-way valves: one fill, one empty
			    PROP_FILL_PROP_EMPTY,  ///< pair of proportional two-way valves: one fill one empty
			    FILL_THREEWAY          ///< single three-way valve: fills when active, empties otherwise
  };

private:
  /****************************************************************/
  // The following instance variables may only be modified from a non-interrupt
  // context, i.e., not within poll().

  /// The I/O pins for this channel designated using the Arduino convention.
  int8_t fill_pin, empty_pin;

  /// Configuration of the valve set for this axis.
  enum pneumatic_config_t config;
  
  /// Current actuator state.
  bool fill, empty;
  
  /// Countdown for the current actuator signal phase, in microseconds.
  long timer;

  /// Duration of the full cycle, in microseconds.
  long period;

  /// Duration of the active period.  Positive values fill, negative values empty.
  long active;
  
  /// Current duty cycle fraction (-1 to 1).
  float duty;
  
  /// Current control mode for single axis.
  enum pneumatic_state_t { IDLE,       ///< no air flow, commands ignored
			   OPEN_LOOP,  ///< open-loop control with valve speed control via PWM
			   NUM_PNEUMATIC_MODES   ///< the total number of axis modes
  } mode;

  /// Utility function to set valve state and flags.
  void set_valves( bool _fill, bool _empty) {
    fill = _fill;
    empty = _empty;

    switch (config) {
    case FILL_EMPTY:
      digitalWrite( fill_pin,  (fill) ? (HIGH) : (LOW));
      digitalWrite( empty_pin, (empty) ? (HIGH) : (LOW));
      break;
      
    case PROP_FILL_PROP_EMPTY:
      // bypass the soft PWM if the valves are proportional
      if (duty > 0.0) analogWrite(fill_pin, (int) (duty * 255));
      else analogWrite(fill_pin, 0);

      if (duty < 0.0) analogWrite(empty_pin, (int) (-duty * 255));
      else analogWrite(empty_pin, 0);
      break;

    case FILL_THREEWAY:
      digitalWrite( fill_pin,  (fill) ? (HIGH) : (LOW));
      break;
    }
  }
  /****************************************************************/
  
public:
			     
  /// Default constructor called during array initialization.  Note that this is
  /// a no-op, since each object needs to be properly initalized with unique pin
  /// numbers later.
  Pneumatic() { }
      
  /// Main constructor.  The arguments are the pin numbers for the step and
  /// direction outputs. Note: this does not initialize the underlying hardware.
  /// Unused outputs should supply a pin number of -1.
  Pneumatic( int8_t _fill_pin, int8_t _empty_pin, enum pneumatic_config_t _config) {
    fill_pin  = _fill_pin;
    empty_pin = _empty_pin;
    config    = _config;
    fill      = false;
    empty     = false;
    timer     = 0;
    period    = 50000;
    active    = 0;
    duty      = 0.0;
    mode      = IDLE;
  }

  /// Hardware initialization.  This class follows the Arduino convention of
  /// initializing instance data in the constructor but not hardware.
  void begin(void) {
    if (fill_pin >= 0){
      pinMode( fill_pin, OUTPUT );
      digitalWrite( fill_pin, LOW );
    }
    if (empty_pin >= 0) {
      pinMode( empty_pin, OUTPUT );
      digitalWrite( empty_pin, LOW );
    }
  }

  /// Set the valve duty cycle and enable open-loop output mode.
  /// The duty cycle is specified from -1.0 to 1.0; positive values fill, negative values empty.
  void setDutyCycle(float _duty) {
    duty   = constrain(_duty, -1.0, 1.0);
    mode   = OPEN_LOOP;
    active = duty * period;
  }

  /// Main polling function to be called as often as possible.  The interval
  /// argument is the duration in microseconds since the last call.
  void update(long interval);

};

#endif //__PNEUMATIC_H_INCLUDED__

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

#include "Pneumatic.h"

void Pneumatic::update(long interval)
{
  switch (mode) {
  case IDLE:
    break;

  case OPEN_LOOP:

    // Wait for the next event time point.
    timer -= interval;

    if (timer <= 0) {
      
      if (active == 0) {  // no air flow
	set_valves( false, false ); // fill, empty
	timer += period;

      } else if (active > 0) {  	// filling
	if (fill) {
	  set_valves( false, false ); // fill, empty
	  timer += (period - active);

	} else {
	  set_valves( true, false); // fill, empty
	  timer += active;
	}
      } else { // emptying
	if (empty) {
	  set_valves( false, false ); // fill, empty
	  timer += (period - (-active));

	} else {
	  set_valves( false, true);  // fill, empty
	  timer += (-active);
	}
      }
    }
    break;
  }
}