// LPC1114 I/O Processor Servo Output Test

// Copyright (C)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.

// Hardware setup:
//
// Arduino Uno R3
// Seeed Studios Arduino Base Shield V2, switched to 3.3V
// Munts Technologies LPC1114 I/O Processor Grove Module, plugged into I2C
// Parallax Standard Servo

#include <LPC1114_IOP.h>

LPC1114_IOP::Transport_Class iop;
LPC1114_IOP::Servo Servo1;
LPC1114_IOP::Servo Servo2;
LPC1114_IOP::Servo Servo3;
LPC1114_IOP::Servo Servo4;

double position = LPC1114_SERVO_NEUTRAL;
double step = 0.01;

void setup()
{
  Serial.begin(115200);
  Serial.print("LPC1114 I/O Processor Servo Output Test\r\n\n");

  Servo1.Init(&iop, LPC1114_PWM1);
  Servo2.Init(&iop, LPC1114_PWM2);
  Servo3.Init(&iop, LPC1114_PWM3);
  Servo4.Init(&iop, LPC1114_PWM4);
}

// Sweep servos back and forth

void loop()
{
  Serial.print("Servo position: ");
  Serial.println(position, 2);

  Servo1 = position;
  Servo2 = position;
  Servo3 = position;
  Servo4 = position;

  position += step;

  if (position > LPC1114_SERVO_MAX)
  {
    position = LPC1114_SERVO_MAX;
    step = -step;
  }

  if (position < LPC1114_SERVO_MIN)
  {
    position = LPC1114_SERVO_MIN;
    step = -step;
  }
}