/*
 Remote Controlled Robotic Arm (MeArm)
 pfodDevice for Arduino
 see www.pfod.com.au for more details

 (c)2012 Forward Computing and Control Pty. Ltd.
 This code may be freely used for both private and commerical use.
 Provide this copyright is maintained.
 */

#include <Servo.h>
#include <EEPROM.h>
#include "pfodParser.h"
Servo myservoA;
Servo myservoB;
Servo myservoC;
Servo myservoD;
int startingPoint = DEFAULT_PULSE_WIDTH;
long pfodServoCurrentA = startingPoint;
long pfodServoRtnA = startingPoint;
long pfodServoCurrentB = startingPoint;
long pfodServoRtnB = startingPoint;
long pfodServoCurrentC = startingPoint;
long pfodServoRtnC = startingPoint;
long pfodServoCurrentD = startingPoint;
long pfodServoRtnD = startingPoint;

unsigned long slewRateTimer = 0;
unsigned long SLEW_RATE_TIME = 20; // 2secs to get there


void extraSetup() {
  myservoA.attach(6);
  myservoA.writeMicroseconds(pfodServoRtnA);
  myservoB.attach(9);
  myservoB.writeMicroseconds(pfodServoRtnB);
  myservoC.attach(10);
  myservoC.writeMicroseconds(pfodServoRtnC);
  myservoD.attach(11);
  myservoD.writeMicroseconds(pfodServoRtnD);
  slewRateTimer = millis();
  pfodServoRtnD = 1900;  // <<<<<<< set this starting position once you have calibrated your gripper
}

/* ===== pfod for Robotic Arm ====
// Using Serial and 9600 for send and receive
// Serial D0 (RX) and D1 (TX) on Arduino Uno, Micro, ProMicro, Due, Mega, Mini, Nano, Pro and Ethernet
// This code uses Serial so remove shield when programming the board
/* Code generated by pfodDesigner V1.2.565
 * (c)2014-2015 Forward Computing and Control Pty. Ltd.
 * NSW Australia, www.forward.com.au
 * This generated code may be freely used for both private and commerical use
 */

pfodParser parser; // create a parser to handle the pfod messages

// the setup routine runs once on reset:
void setup() {
  Serial.begin(9600);
  for (int i = 3; i > 0; i--) {
    // wait a few secs to see if we are being programmed
    delay(1000);
  }
  parser.connect(&Serial); // connect the parser to the i/o stream
  // <<<<<<<<< Your extra setup code goes here
  extraSetup();
}

// the loop routine runs over and over again forever:
void loop() {
  unsigned long ms = millis();
  if ((ms - slewRateTimer) > SLEW_RATE_TIME) {
    slewRateTimer += SLEW_RATE_TIME;
    long diff = (pfodServoRtnA - pfodServoCurrentA);
    int maxDiff = 4;
    if (diff > maxDiff) {
      diff = maxDiff;
    }
    if (diff < -maxDiff) {
      diff = -maxDiff;
    }
    pfodServoCurrentA += diff;
    myservoA.writeMicroseconds(pfodServoCurrentA);

    if (pfodServoCurrentB > pfodServoRtnB) {
      pfodServoCurrentB -= 1;
    } else if (pfodServoCurrentB < pfodServoRtnB) {
      pfodServoCurrentB += 1;
    } else {
    }
    myservoB.writeMicroseconds(pfodServoCurrentB);

    if (pfodServoCurrentC > pfodServoRtnC) {
      pfodServoCurrentC -= 1;
    } else if (pfodServoCurrentC < pfodServoRtnC) {
      pfodServoCurrentC += 1;
    } else {
    }
    myservoC.writeMicroseconds(pfodServoCurrentC);

    if (pfodServoCurrentD > pfodServoRtnD) {
      pfodServoCurrentD -= 1;
    } else if (pfodServoCurrentD < pfodServoRtnD) {
      pfodServoCurrentD += 1;
    } else {
    }
    myservoD.writeMicroseconds(pfodServoCurrentD);
  }

  byte cmd = parser.parse(); // pass it to the parser
  // parser returns non-zero when a pfod command is fully parsed
  if (cmd != 0) { // have parsed a complete msg { to }
    byte* pfodFirstArg = parser.getFirstArg(); // may point to \0 if no arguments in this msg.
    long pfodLongRtn; // used for parsing long return arguments, if any
    if ('.' == cmd) {
      // pfodApp has connected and sent {.} , it is asking for the main menu
      // send back the menu designed
      sendMainMenu();

      // now handle commands returned from button/sliders
    } else if ('A' == cmd) { // user moved slider -- 'A '
      parser.parseLong(pfodFirstArg, &pfodLongRtn); // parse first arg as a long
      pfodServoRtnA = pfodLongRtn;
      send_updateA();

    } else if ('B' == cmd) { // user moved slider -- 'B '
      parser.parseLong(pfodFirstArg, &pfodLongRtn); // parse first arg as a long
      pfodServoRtnB = pfodLongRtn;
      send_updateB();

    } else if ('C' == cmd) { // user moved slider -- 'C '
      parser.parseLong(pfodFirstArg, &pfodLongRtn); // parse first arg as a long
      pfodServoRtnC = pfodLongRtn;
      send_updateC();

    } else if ('D' == cmd) { // user moved slider -- 'D '
      parser.parseLong(pfodFirstArg, &pfodLongRtn); // parse first arg as a long
      pfodServoRtnD = pfodLongRtn;
      send_updateD();

    } else if ('!' == cmd) {
      // CloseConnection command
      closeConnection(parser.getPfodAppStream());
    } else {
      // unknown command
      parser.print(F("{}")); // always send back a pfod msg otherwise pfodApp will disconnect.
    }
  }

  //  <<<<<<<<<<<  Your other loop() code goes here

}

void closeConnection(Stream *io) {
  // add any special code here to force connection to be dropped
}

void sendMainMenu() {
  parser.print(F("{."));  // start a Menu screen pfod message
  send_menuContents(); // send the menu contents
  parser.print(F("}"));  // close pfod message
}

void sendMainMenuUpdate() {
  parser.print(F("{:"));  // start an Update Menu pfod message
  send_menuContents(); // send the menu contents
  parser.print(F("}"));  // close pfod message
}

// modify this method if you need to update the menu to reflect state changes
void send_menuContents() {
  // send menu prompt
  parser.print(F("<w><b>Robotic Arm Control"));
  // send menu items
  parser.print(F("|A~Rotation\n`"));
  parser.print(pfodServoRtnA);
  parser.print(F("`2400`544~\302\260~0.09698276~-544"));
  parser.print(F("|B~In/Out\n`"));
  parser.print(pfodServoRtnB);
  parser.print(F("`2400`544~%~0.05388~-544"));
  parser.print(F("|C~Up/Down\n`"));
  parser.print(pfodServoRtnC);
  parser.print(F("`2400`544~%~0.05388~-544"));
  parser.print(F("|D~Gripper\n`"));
  parser.print(pfodServoRtnD);
  parser.print(F("`2080`1884~% Closed~0.510204~-1884")); // adjust this for our gripper
  // ============ end of menu item ===========
}

void send_updateA() {
  parser.print(F("{:"));
  parser.print(F("|A~Rotation\n`"));
  parser.print(pfodServoRtnA);
  parser.print('}');
}
void send_updateB() {
  parser.print(F("{:"));
  parser.print(F("|B~In/Out\n`"));
  parser.print(pfodServoRtnB);
  parser.print('}');
}
void send_updateC() {
  parser.print(F("{:"));
  parser.print(F("|C~Up/Down\n`"));
  parser.print(pfodServoRtnC);
  parser.print('}');
}
void send_updateD() {
  parser.print(F("{:"));
  parser.print(F("|D~Gripper\n`"));
  parser.print(pfodServoRtnD);
  parser.print('}');
}