{ Raspberry Pi LPC1114 I/O Processor Expansion Board SPI Agent Firmware }
{ Pulse Width Modulated output services                                 }

{ Copyright (C)2014-2018, Philip Munts, President, Munts AM Corp.             }
{                                                                             }
{ Redistribution and use in source and binary forms, with or without          }
{ modification, are permitted provided that the following conditions are met: }
{                                                                             }
{ * Redistributions of source code must retain the above copyright notice,    }
{   this list of conditions and the following disclaimer.                     }
{                                                                             }
{ THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" }
{ AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE   }
{ IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE  }
{ ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE   }
{ LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR         }
{ CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF        }
{ SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS    }
{ INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN     }
{ CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)     }
{ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE  }
{ POSSIBILITY OF SUCH DAMAGE.                                                 }

UNIT spi_agent_pwm;

INTERFACE

  USES
    lpc11xx,
    spi_agent_commands,
    spi_agent_exceptions,
    spi_agent_gpio,
    spi_agent_messages,
    spi_agent_pins,
    spi_agent_transport;

  CONST
    PWM_MIN_DUTYCYCLE      = 0.0;	{ Percent }
    PWM_MAX_DUTYCYCLE      = 100.0;	{ Percent }
    SERVO_MIN_POSITION     = -1.0;	{ Normalized deflection from null position }
    SERVO_NEUTRAL_POSITION = 0; 	{ Midpoint/neutral/null/zero position }
    SERVO_MAX_POSITION     = 1.0;	{ Normalized deflection from null position }
    MOTOR_MIN_SPEED        = -1.0;	{ Normalized full speed reverse }
    MOTOR_MAX_SPEED        = 1.0;	{ Normalized full speed forward }
    MOTOR_STOP             = 0.0;

  TYPE
    PWM_FREQUENCY_t = 50 .. 50000;	{ Hertz }
    PWM_CHANNEL_t   = 1 .. 4;

    PWM_OUTPUT_t =
      CLASS
        CONSTRUCTOR create(pwm_pin : GPIO_PIN_t;
          freq : PWM_FREQUENCY_t = 100);

        CONSTRUCTOR create(pwm_channel : PWM_CHANNEL_t;
          freq : PWM_FREQUENCY_t = 100);

        PROCEDURE put(dutycycle : real);

      PRIVATE
        pin : GPIO_PIN_t;
      END;

    SERVO_OUTPUT_t =
      CLASS(PWM_OUTPUT_t)
        CONSTRUCTOR create(pwm_pin : GPIO_PIN_t;
          freq : PWM_FREQUENCY_t = 50);

        CONSTRUCTOR create(pwm_channel : PWM_CHANNEL_t;
          freq : PWM_FREQUENCY_t = 50);

        PROCEDURE put(position : real);

      PRIVATE
        frequency : PWM_FREQUENCY_t;
      END;

    MOTOR_OUTPUT_t =
      CLASS
        CONSTRUCTOR create(pwm_pin : GPIO_PIN_t; dir_pin : GPIO_PIN_t;
          freq : PWM_FREQUENCY_t = 100);

        PROCEDURE put(speed : real);

      PRIVATE
        pwmout : PWM_OUTPUT_t;
        dirout : GPIO_t;
      END;

IMPLEMENTATION

  CONST
    ChannelToPin : ARRAY [pwm_channel_t] OF GPIO_PIN_t =
      (LPC1114_PWM1, LPC1114_PWM2, LPC1114_PWM3, LPC1114_PWM4);

  CONSTRUCTOR PWM_OUTPUT_t.create(pwm_pin : GPIO_PIN_t; freq : PWM_FREQUENCY_t);

  VAR
    cmd    : SPIAGENT_COMMAND_MSG_t;
    resp   : SPIAGENT_RESPONSE_MSG_t;
    error  : integer;
    errbuf : string;

  BEGIN

    { Build the command structure }

    cmd.command := ord(SPIAGENT_CMD_CONFIGURE_PWM_OUTPUT);
    cmd.pin := ord(pwm_pin);
    cmd.data := freq;

    { Issue command to the SPI Agent Firmware }

    spiagent_command(cmd, resp, error);

    { Process errors }

    IF error <> 0 THEN
      BEGIN
        Str(error, errbuf);
        RAISE SpiAgentError.create('ERROR: spiagent_command() failed, error=' +
          errbuf);
      END;

    IF resp.error <> 0 THEN
      BEGIN
        Str(resp.error, errbuf);
        RAISE SpiAgentError.create('ERROR: SPI Agent Firmware returned error=' +
          errbuf);
      END;

    INHERITED create;

    pin := pwm_pin;
  END;

  CONSTRUCTOR PWM_OUTPUT_t.create(pwm_channel: pwm_channel_t;
    freq : PWM_FREQUENCY_t);

  BEGIN
    Create(ChannelToPin[pwm_channel], freq);
  END;

  PROCEDURE PWM_OUTPUT_t.put(dutycycle: real);

  VAR
    cmd    : SPIAGENT_COMMAND_MSG_t;
    resp   : SPIAGENT_RESPONSE_MSG_t;
    error  : integer;
    errbuf : string;

  BEGIN
    IF (dutycycle < PWM_MIN_DUTYCYCLE) OR (dutycycle > PWM_MAX_DUTYCYCLE) THEN
      BEGIN
        RAISE SpiAgentError.create('ERROR: Invalid duty cycle value');
      END;

    { Build the command structure }

    cmd.command := ord(SPIAGENT_CMD_PUT_PWM);
    cmd.pin := ord(pin);
    cmd.data := Round(655.35*dutycycle);

    { Issue command to the SPI Agent Firmware }

    spiagent_command(cmd, resp, error);

    { Process errors }

    IF error <> 0 THEN
      BEGIN
        Str(error, errbuf);
        RAISE SpiAgentError.create('ERROR: spiagent_command() failed, error=' +
          errbuf);
      END;

    IF resp.error <> 0 THEN
      BEGIN
        Str(resp.error, errbuf);
        RAISE SpiAgentError.create('ERROR: SPI Agent Firmware returned error=' +
          errbuf);
      END;
  END;

  CONSTRUCTOR SERVO_OUTPUT_t.create(pwm_pin : GPIO_PIN_t;
    freq : PWM_FREQUENCY_t);

  BEGIN
    IF freq > 400 THEN
      BEGIN
        RAISE SpiAgentError.create('ERROR: PWM frequency is too high for servos');
      END;

    INHERITED create(pwm_pin, freq);

    Self.frequency := freq;
  END;

  CONSTRUCTOR SERVO_OUTPUT_t.create(pwm_channel : pwm_channel_t;
    freq : PWM_FREQUENCY_t);

  BEGIN
    Create(ChannelToPin[pwm_channel], freq);
  END;

  PROCEDURE SERVO_OUTPUT_t.put(position: real);

  BEGIN
    IF (position < SERVO_MIN_POSITION) OR (position > SERVO_MAX_POSITION) THEN
      BEGIN
        RAISE SpiAgentError.create('ERROR: Invalid servo position value');
      END;

    INHERITED put((0.5E-1*position + 1.5E-1)*Self.frequency);
  END;

  CONSTRUCTOR MOTOR_OUTPUT_t.create(pwm_pin : GPIO_PIN_t; dir_pin : GPIO_PIN_t;
    freq : PWM_FREQUENCY_t);

  BEGIN
    IF pwm_pin = dir_pin THEN
      BEGIN
        RAISE SpiAgentError.create('ERROR: PWM and direction pins cannot be the same');
      END;

    INHERITED create;

    pwmout := PWM_OUTPUT_t.create(pwm_pin, freq);
    dirout := GPIO_t.create(dir_pin, GPIO_MODE_OUTPUT);
  END;

  PROCEDURE MOTOR_OUTPUT_t.put(speed : real);

  BEGIN
    IF (speed < MOTOR_MIN_SPEED) OR (speed > MOTOR_MAX_SPEED) THEN
      BEGIN
        RAISE SpiAgentError.create('ERROR: Invalid servo position value');
      END;

    IF speed < 0 THEN
      BEGIN
        dirout.put(False);
        pwmout.put(-100*speed);
      END
    ELSE
      BEGIN
        dirout.put(True);
        pwmout.put(100*speed);
      END
  END;
END.