// ######################################################################
//
//  Useless Box Sketch v0.0 Beta
//
//  Released:  17/03/2017
//
//  Author: TechKnowTone
//
// ######################################################################
/*
    This software is furnished "as is", without technical support, and
    with no warranty, expressed or implied, as to its usefulness for any
    purpose.

    Elements of this software were taken from the public domain. It uses
    libraries which must be installed on your system.

    This program demonstrates the personality of a useless machine, which
    has two servo motors and a switch. It also reacts directly to
    basic servo commands sent over the serial interface for calibration.
    The code is written to allow for servo replacement and easy setup.
    Power saving, 1 second sleep, code is now included.
    
    Commands:
    !   - forces a 'soft' RESET
    Ann.    - perform action nn immediately
    SAnn.   - set servo angle in degrees 0 - 180
    SD.     - detach the active servo pin
    SLnn.   - set attached servo lower limit in microseconds
    SMnn.   - set servo angle in microseconds servoLL - servoUL
    SPnn.   - set the active servo pin number
    SUnn.   - set attached servo upper limit in microseconds
*/
// 
#define LEDPin 13 // pin used for onboard LED
#define servoFgrPin 4 // pin no. for finger servo
#define servoLidPin 5 // pin no. for lid servo
#define switchPin 12 // pin no. for push switch

// Declare and initialise global variables
#include <Servo.h>
#include "LowPower.h"

int actionTask; // action sub task
int Angle;  // start with servo in the centre pos
char cmdMode; // command mode
char cmdType; // command type
int cmdVal; // value associated with a cmdType
long FgrAng; // latest finger value in degrees
long FgrVal; // latest finger value in microseconds
unsigned long interval; // main loop interval in microseconds
char keyChar; // any keyboard character
int keyVal; // any keyboard value
long LidAng; // latest finger value in degrees
long LidVal; // latest finger value in microseconds
long mainCnt; // counts time in 10 ms steps
int mainTask; // main task pointer
long mainWake; // count at which to waken in 10 ms increments
unsigned long nextMicros; // main loop interval timer in microseconds
int pressCnt; // counts the number of switch presses recently
int runOnce = 0; // first POST sets it to 1
long servoFgrMax = 2105; //Full finger servo value
long servoFgrTch = 1952; //Touch finger servo value
long servoFgrTap = 1399; //Tap finger servo value
int servoLL = 544; // servo min time setting
long servoLidMax = 2543; //minimum lid servo value
long servoLidClr = 2021; //Clear lif servo value
long servoLidTap = 1643; //Tap lid servo value
int servoPin;  // servo output pin
int servoUL = 2400; // servo max time setting
int sleepEn; // sleep enable, 0 = disabled, 1 = enabled
int subTask = 0; // main sub task pointer
int switchState; // push switch state, -1 = Open, 0 = changing, 1 = closed
int timeDelay = 1; // delay in milliseconds
long wakeMinTime; // extended wake period
int wakeTask = 0; // wake task pointer
Servo servoMain; // Define our Servo
Servo servoFgr; // Finger servo
Servo servoLid; // Lid servo

void setup() {
  // put your setup code here, to run once:
  Serial.begin(19200); // high baud rate for calibration functions
  pinMode(LEDPin,OUTPUT);
  digitalWrite(LEDPin, LOW);
  pinMode(switchPin,INPUT_PULLUP);
  RESET();
  delay(1000);          // wait 1 second
}

void loop() {
  // put your main code here, to run repeatedly:
  readKey(); // empty Rx buffer
  if (micros() >= nextMicros) {
    // do these every 10ms
    nextMicros = micros() + interval;
    doMainTask();
  }
}

// ----------------------------------------------------------------------

void attachServos() {
  // attach both Finger and Lid servos
  servoFgr.attach(servoFgrPin); servoLid.attach(servoLidPin);
}

// ----------------------------------------------------------------------

void detachDel(int zDel) {
  // detach, wait zDel, then attach
  detachServos(); delay(zDel); attachServos();
}

// ----------------------------------------------------------------------

void detachServos() {
  // detach both Finger and Lid servos
  servoFgr.detach(); servoLid.detach();
}

// ----------------------------------------------------------------------

void doCmd() {
  // a '.' has been received so execute command if valid
  int zPrint = 1;
  switch (cmdMode) {
    case ' ': break;
    case 'A':
      // perform a specific action
      switch (cmdVal) {
        case 0: fingerAct00(); break;  
        case 1: fingerAct01(); break;  
        case 2: fingerAct02(); break;  
        case 3: fingerAct03(); break;  
        case 4: fingerAct04(); break;  
        case 50: fingerAct50(); break;  
        case 51: fingerAct51(); break;  
        case 52: fingerAct52(); break;  
        case 53: fingerAct53(); break;  
        case 90: fingerAct90(); break;  
        case 91: fingerAct91(); break;  
        case 92: fingerAct92(); break;  
        case 93: fingerAct93(); break;  
        case 94: fingerAct94(); break;  
      } 
      nextMicros = micros() + interval; // re-initialise loop timer
      break;
    case 'S':
      switch (cmdType) {
        case ' ': break;
        case 'A':
          Angle = cmdVal; servoMain.write(Angle);
          // report the angle in microseconds
          Serial.print("SM=");
          Serial.print(servoMain.readMicroseconds());
          Serial.println("."); zPrint = 0;
          break;
        case 'D': servoPin = -1; servoMain.detach(); break;
        case 'L':
          servoLL = cmdVal;
          if (servoPin > -1) {servoMain.attach(servoPin,servoLL,servoUL);}
          break;
        case 'M':
          Angle = cmdVal; servoMain.writeMicroseconds(Angle);
          // report the angle in degrees
          Serial.print("SA=");
          Serial.print(servoMain.read());
          Serial.println("."); zPrint = 0;
          break;
        case 'P':
          servoPin = cmdVal; servoMain.attach(servoPin,servoLL,servoUL);
          break;
        case 'U':
          servoUL = cmdVal;
          if (servoPin > -1) {servoMain.attach(servoPin,servoLL,servoUL);}
          break;
      } break;
  }
  if (zPrint > 0) {
    // is not a reporting function, echo the received command
    Serial.print(cmdMode); Serial.print(cmdType); Serial.print(cmdVal); Serial.println('.');
  }
  // now reset the variables
  cmdMode = ' '; cmdType = ' '; cmdVal = 0;
  mainCnt = 0; // zero task counter as something has happened
  mainWake = random(3500, 30000); sleepEn = 0; wakeTask= 0;
}

// ----------------------------------------------------------------------

void emptySerial() {
  // empty the serial input buffer
  keyVal = 0;
  while (keyVal != -1) {
    keyVal = Serial.read();
  }
}

// ----------------------------------------------------------------------

void extendCmdVal(int zVal) {
  // adds a new digit to the right-hand end of cmdVal
  cmdVal = (cmdVal * 10) + zVal;  
}

// ----------------------------------------------------------------------

void readSwitch() {
  // read the state of the push switch.
  // Two reads are needed to fully change the state from -1 to +1
  if (digitalRead(switchPin) == HIGH) {
    // switch is open
    if (switchState > -1) {
      switchState--;
      if (switchState < 0) {Serial.println("Switch Is Open.");}
    }
  } else {
    // switch is close
    if (switchState < 1) {
      switchState++;
      if (switchState > 0) {Serial.println("Switch Is Closed.");}
     }
  }
}

// ----------------------------------------------------------------------

void readKey() {
  // reads a key from the keyboard annd reacts accordingly
  keyVal = Serial.read();
  if (keyVal != -1) {
    keyChar = char(keyVal);
//      Serial.print("keyVal=");
//      Serial.print(keyChar);
//      Serial.print("   ASCII=");
//      Serial.println(keyVal);
    int zNF = 0;
    switch (keyChar) {
      case '!': RESET(); zNF = 1;break;
      case '0': extendCmdVal(0); zNF = 1;break;
      case '1': extendCmdVal(1); zNF = 1;break;
      case '2': extendCmdVal(2); zNF = 1;break;
      case '3': extendCmdVal(3); zNF = 1;break;
      case '4': extendCmdVal(4); zNF = 1;break;
      case '5': extendCmdVal(5); zNF = 1;break;
      case '6': extendCmdVal(6); zNF = 1;break;
      case '7': extendCmdVal(7); zNF = 1;break;
      case '8': extendCmdVal(8); zNF = 1;break;
      case '9': extendCmdVal(9); zNF = 1;break;
    }
    if (zNF == 0) {
      if (cmdMode == ' ') {
        // test for new Command Mode char?
        switch (keyChar) {
          case 'a': cmdMode = 'A'; break;
          case 'A': cmdMode = 'A'; break;
          case 's': cmdMode = 'S'; break;
          case 'S': cmdMode = 'S'; break;
        } cmdType = ' '; cmdVal = 0;
      } else {
        // test for Command Type char?
        switch (keyChar) {
          case 'a': cmdType = 'A'; break;
          case 'A': cmdType = 'A'; break;
          case 'd': cmdType = 'D'; break;
          case 'D': cmdType = 'D'; break;
          case 'l': cmdType = 'L'; break;
          case 'L': cmdType = 'L'; break;
          case 'm': cmdType = 'M'; break;
          case 'M': cmdType = 'M'; break;
          case 'p': cmdType = 'P'; break;
          case 'P': cmdType = 'P'; break;
          case 'u': cmdType = 'U'; break;
          case 'U': cmdType = 'U'; break;
          case '.': doCmd(); break;
        }
      }
    } else if (keyChar == '.') {doCmd();}
  }
}

// ----------------------------------------------------------------------

void RESET() {
  // perform a soft RESET
  servoMain.detach();
  servoFgr.detach();
  servoLid.detach();
  setDefaults();
  runPOST();
}

// ----------------------------------------------------------------------

void runPOST() {
  // called during start-up
  Serial.println("\n\n\n\nStarting POST...");
  delay(1000); emptySerial();
  if (runOnce < 1) {
    Serial.println("Testing Servos...");
    attachServos();
    SetFgrToRest(); SetLidToRest(); delay(200);
    for (int zP = 0; zP < 2; zP++) {
      SetFgrToTap(); SetLidToTap(); delay(200);
      SetFgrToRest(); SetLidToRest(); delay(200);
    } detachServos(); runOnce = 1;
  }
  mainCnt = 0; mainWake = 500; // set wake to tap lid after 5 seconds
  readSwitch(); readSwitch();
  if (switchState > 0) {
    // switch is in closed state so move finger to open it
    delay(5000); // allows user time to close the lid
    attachServos();
    SetLidVal(servoLidClr); delay(200);
    SetFgrVal(servoFgrMax); delay(400); // press switch OFF
    SetFgrToRest(); delay(100);
    SetLidToRest(); delayLidAng(45);
    mainWake = random(3000, 30000);
  }
  detachServos();
  Serial.println("POST complete!");
}

// ----------------------------------------------------------------------

void setDefaults() {
  // set defaults at start-up and in soft RESET
  Angle  = 90;  // start with servo in the centre pos
  cmdMode = ' '; // command mode
  cmdType = ' '; // command type
  cmdVal = 0; // value associated with a cmdType
  FgrAng = servoFgrTap; // set finger value to Tap angle
  interval = 10000; // main loop interval in microseconds
  mainCnt = 0; // counts time in 10 ms steps
  mainTask = 0; // main task pointer, starts = 0
  mainWake = random(30, 300); // waken between 30 sec - 5 min
  nextMicros = micros() + interval; // main loop interval in microseconds
  pressCnt = 0; // counts the number of switch presses recently
  servoPin = -1;  // servo output pin is undefined
  sleepEn; // sleep enable, 0 = disabled, 1 = enabled
  subTask = 0; // reset main sub task pointer
  switchState = 0; // set default to 0 = changing
  wakeMinTime = 0; // extended wake period
  wakeTask = 0; // wake task pointer
}

// ----------------------------------------------------------------------


