// LM: March 2022 - Servo control of Sunfounder Robot Arm for Connect Four game
//                - Servo motor concepts based on Rollarm.ino (citation below)

/* -----------------------------------------------------------------------------
  Author             : Allen
  Check              : Amy
  Version            : V2.0
  Date               : 12/09/2019
  Description        : Rollarm control program
  Company website    : http://www.sunfounder.com
*/

#include <Servo.h>

#define SERIAL_BAUD 9600

#define YELLOW_BUTTON 3

// Auxiliary inputs
#define BIT0 8
#define BIT1 9
#define BIT2 10
#define DATA_READY 11

// Time
const long ONESEC = 1000;
const long TWOSEC = 2000;
const long JIFFY = 20;                                  // Software debounce

// Alias array subscripts by servo functions
const int ROTOR = 0;
const int LOWER = 1;
const int UPPER = 2;
const int GRIPPER = 3;
String servoName[4] = {"ROTOR", "LOWER ARM", "UPPER ARM", "GRIPPER"};

// Application
const int NUM_OF_SERVOS = 4;                            // Rotator, upper arm, lower arm, gripper
const int COL = 7;
Servo myServo[NUM_OF_SERVOS];
int analogInput[NUM_OF_SERVOS];
int lastAngle[NUM_OF_SERVOS];

// Gripper
const int OPEN = 160;
const int CLOSED = 171;
// Key positions - Determined by physical layout of game components
//                 Numbers are angles in degrees, or so they say..
int raHome[NUM_OF_SERVOS] = {95, 90, 95, 145};
// Hover position, vertically above pickup tray
const int aboveTrayOpen[NUM_OF_SERVOS] = {30, 90, 100, 160};
const int aboveTrayClosed[NUM_OF_SERVOS] = {30, 90, 100, 170};
// On pickup tray                    
const int onTrayOpen[NUM_OF_SERVOS] = {30, 135, 35, 155};
const int onTrayClosed[NUM_OF_SERVOS] = {30, 135, 35, 170};
// Close to but clear of game array columns                 
const int aboveGameOpen[NUM_OF_SERVOS] = {95, 90, 95, 145};
const int aboveGameClosed[NUM_OF_SERVOS] = {95, 90, 95, 170};
// At opening to column COL, gripper open                     
const int onGameOpen[COL][NUM_OF_SERVOS] = {{136, 126, 98, 165},
                                            {122, 116, 84, 165},
                                            {112, 113, 76, 165},
                                            {99, 113, 71, 165},
                                            {87, 111, 75, 165},
                                            {76, 118, 81, 165},
                                            {65, 124, 100, 164}};
// At opening to column COL, gripper closed             
const int onGameClosed[COL][NUM_OF_SERVOS] {{134, 129, 101, 170},
                                            {123, 123, 92, 170},
                                            {110, 118, 84, 170},
                                            {98, 116, 77, 170},
                                            {85, 110, 71, 170},
                                            {75, 118, 82, 170},
                                            {62, 124, 89, 170}};
             
void setup() {
  Serial.begin(SERIAL_BAUD);
  Serial.println("RobotArm-LM.ino started...");
  Serial.println();

  for (int i=0; i<NUM_OF_SERVOS; i++)                   // DIO pins 4, 5, 6, 7
    myServo[i].attach(4+i);

  pinMode(YELLOW_BUTTON, INPUT);
  pinMode(BIT0, INPUT);
  pinMode(BIT1, INPUT);
  pinMode(BIT2, INPUT);
  pinMode(DATA_READY, INPUT);
  
  for (int i=0; i<NUM_OF_SERVOS; i++)                   // Read startup state of servos
    lastAngle[i] = myServo[i].read();

  multAdjustServo(raHome);                              // Initialize to intermediate angles                           
  delay(ONESEC);
//test();
/*
  while(true)                                           // Optionally wait here (test context)
    ;
*/
}

void loop() {
  int testInput;
  if ((testInput = getInput()) >= 0) {
    Serial.println("Test input: " + String(testInput));
    if (testInput > 0) {
//    makePlay(testInput);
      makePlay(mirror(testInput));
    }
    delay(ONESEC);
  }

}

// The following two functions are based on ReadPot() and mapping0() in Record.ino

void getRaw() {                                         // 0 to 1023
  analogInput[0] = analogRead(A0);
  analogInput[1] = analogRead(A1);
  analogInput[2] = analogRead(A2);
  analogInput[3] = analogRead(A3);
}

void rawToAngle() {
  analogInput[0] = map(analogInput[0], 0, 1023, 10, 170);
  analogInput[1] = map(analogInput[1], 0, 1023, 10, 170);
  analogInput[2] = map(analogInput[2], 0, 1023, 10, 170);
  analogInput[3] = map(analogInput[3], 0, 1023, 100, 180);
}

// The following function adjusts one servo from its last stored angle to the specified angle

void adjustServo(int servo_number, int target_angle) {
  Serial.print("Adjust servo ");
  Serial.print(servoName[servo_number]);
  Serial.println(" to " + String(target_angle) + " degrees.");
  int current_angle = myServo[servo_number].read();
  while (current_angle != target_angle) {
    if (current_angle < target_angle)
      current_angle++;
    else
      current_angle--;
    myServo[servo_number].write(current_angle);
    delay(5);
  }
  lastAngle[servo_number] = current_angle;
}

void adjustServo(int servo_number, int target_angle, long reciprocalSpeed) {
  Serial.println("Adjust " + servoName[servo_number] + " to " + String(target_angle) + " degrees.");
  int current_angle = myServo[servo_number].read();
  while (current_angle != target_angle) {
    if (current_angle < target_angle)
      current_angle++;
    else
      current_angle--;
    myServo[servo_number].write(current_angle);
    delay(reciprocalSpeed);
  }
  lastAngle[servo_number] = current_angle;
}

void multAdjustServo(int target_array[NUM_OF_SERVOS]) {
  // Adjust servos to target angles in interleaved fashion.
  Serial.print("multAdjustServo() to target array {");
  for (int i=0; i<NUM_OF_SERVOS; i++) {
    Serial.print(String(target_array[i]));
    if (i < (NUM_OF_SERVOS-1))
      Serial.print(", ");
  }
  Serial.println("}");
  boolean allDone = false;
  int current_angle;
  while (!allDone) {
    allDone = true;
    current_angle =myServo[ROTOR].read();
    if (current_angle != target_array[ROTOR]) {
      if (current_angle < target_array[ROTOR]) {
        current_angle++;
        allDone = false;   
      }
      else if (current_angle > target_array[ROTOR]) {
        current_angle--;
        allDone = false;
      }
      myServo[ROTOR].write(current_angle);
      lastAngle[ROTOR] = current_angle;
      delay(2);
    }
    current_angle =myServo[LOWER].read();
    if (current_angle != target_array[LOWER]) {
      if (current_angle < target_array[LOWER]) {
        current_angle++;
        allDone = false;   
      }
      else if (current_angle > target_array[LOWER]) {
        current_angle--;
        allDone = false;
      }
      myServo[LOWER].write(current_angle);
      lastAngle[LOWER] = current_angle;
      delay(2);
    }
    current_angle =myServo[UPPER].read();
    if (current_angle != target_array[UPPER]) {
      if (current_angle < target_array[UPPER]) {
        current_angle++;
        allDone = false;   
      }
      else if (current_angle > target_array[UPPER]) {
        current_angle--;
        allDone = false;
      }
      myServo[UPPER].write(current_angle);
      lastAngle[UPPER] = current_angle;
      delay(2);
    }
    current_angle =myServo[GRIPPER].read();
    if (current_angle != target_array[GRIPPER]) {
      if (current_angle < target_array[GRIPPER]) {
        current_angle++;
        allDone = false;   
      }
      else if (current_angle > target_array[GRIPPER]) {
        current_angle--;
        allDone = false;
      }
      myServo[GRIPPER].write(current_angle);
      lastAngle[GRIPPER] = current_angle;
      delay(2);
    }
  }
}

void multAdjustServo(int target_array[NUM_OF_SERVOS], long reciprocalSpeed) {
  // Adjust servos to target angles in interleaved fashion.
  Serial.print("multAdjustServo() to target array {");
  for (int i=0; i<NUM_OF_SERVOS; i++) {
    Serial.print(String(target_array[i]));
    if (i < (NUM_OF_SERVOS-1))
      Serial.print(", ");
  }
  Serial.println("}");
  boolean allDone = false;
  int current_angle;
  while (!allDone) {
    allDone = true;
    current_angle =myServo[ROTOR].read();
    if (current_angle != target_array[ROTOR]) {
      if (current_angle < target_array[ROTOR]) {
        current_angle++;
        allDone = false;   
      }
      else if (current_angle > target_array[ROTOR]) {
        current_angle--;
        allDone = false;
      }
      myServo[ROTOR].write(current_angle);
      lastAngle[ROTOR] = current_angle;
      delay(reciprocalSpeed);
    }
    current_angle =myServo[LOWER].read();
    if (current_angle != target_array[LOWER]) {
      if (current_angle < target_array[LOWER]) {
        current_angle++;
        allDone = false;   
      }
      else if (current_angle > target_array[LOWER]) {
        current_angle--;
        allDone = false;
      }
      myServo[LOWER].write(current_angle);
      lastAngle[LOWER] = current_angle;
      delay(reciprocalSpeed);
    }
    current_angle =myServo[UPPER].read();
    if (current_angle != target_array[UPPER]) {
      if (current_angle < target_array[UPPER]) {
        current_angle++;
        allDone = false;   
      }
      else if (current_angle > target_array[UPPER]) {
        current_angle--;
        allDone = false;
      }
      myServo[UPPER].write(current_angle);
      lastAngle[UPPER] = current_angle;
      delay(reciprocalSpeed);
    }
    current_angle =myServo[GRIPPER].read();
    if (current_angle != target_array[GRIPPER]) {
      if (current_angle < target_array[GRIPPER]) {
        current_angle++;
        allDone = false;   
      }
      else if (current_angle > target_array[GRIPPER]) {
        current_angle--;
        allDone = false;
      }
      myServo[GRIPPER].write(current_angle);
      lastAngle[GRIPPER] = current_angle;
      delay(reciprocalSpeed);
    }
  }
}

void copyServoArray(int to[NUM_OF_SERVOS], int fm[NUM_OF_SERVOS]) {
  for (int i=0; i<NUM_OF_SERVOS; i++)
    to[i] = fm[i];
  return;
}

// Compound game play movements

const long NORMAL = 2;
const long SLOW = 6;
const long VERY_SLOW = 10;
const long SINGLE_SERVO_SLOW = 10;
const long SINGLE_SERVO_VERY_SLOW = 25;
const long SHORT_PAUSE = 200;
const long LONG_PAUSE = 500;

void goToPickupTray() {
  multAdjustServo(aboveTrayOpen, NORMAL);
  delay(SHORT_PAUSE);
  multAdjustServo(onTrayOpen, SLOW);
  return;
}

void seizePiece() {
  adjustServo(GRIPPER, CLOSED, SINGLE_SERVO_SLOW);
  return;
}

void clearPickupTray() {
  // Lift gently
  // To do: Check on naming. Are upper and lower arm names reversed?
  int start = myServo[UPPER].read();
  adjustServo(LOWER, start+20, 10);
}

void bringPieceToGameArray(int col) {
  multAdjustServo(aboveTrayClosed, SLOW);
  delay(SHORT_PAUSE);
  multAdjustServo(aboveGameClosed, SLOW);
  delay(SHORT_PAUSE);
  multAdjustServo(onGameClosed[col-1], VERY_SLOW);
  return;
}

void releasePiece() {
  adjustServo(GRIPPER, OPEN, SINGLE_SERVO_VERY_SLOW);
}

void returnToStagingArea() {
  multAdjustServo(aboveGameOpen, SLOW);
  delay(SHORT_PAUSE);
  multAdjustServo(raHome, NORMAL);
}

void makePlay(int col) {
  Serial.println("Go to pickup tray.");
  goToPickupTray();
  delay(SHORT_PAUSE);
  Serial.println("Seize piece.");
  seizePiece();
  delay(SHORT_PAUSE);
  Serial.println("Clear pickup tray");
  clearPickupTray();
  delay(SHORT_PAUSE);
  Serial.println("Bring piece to column " + String(col) + " of game array.");
  bringPieceToGameArray(col);
  delay(SHORT_PAUSE);
  Serial.println("Release piece.");
  releasePiece();
  delay(LONG_PAUSE);
  Serial.println("Return to staging area.");
  returnToStagingArea();
}


// Teensy interface

int getInput() {
  if (digitalRead(DATA_READY) == HIGH) {
    debouncePB();
    return binToDec();
  }
  return -1;  
}

void debouncePB() {
  delay(JIFFY);
  while (digitalRead(DATA_READY) == HIGH)
    ;                                           // Debounce  
}

int binToDec() {
  int result = 0;
  if (digitalRead(BIT0) == HIGH)
    result ++;
  if (digitalRead(BIT1) == HIGH)
    result += 2;
  if (digitalRead(BIT2) == HIGH)
    result += 4;
  return result;
}

int mirror(int column) {
  return COL + 1 - column;                      // Game array is upside down to robota arm
}

// Test / Debug - Modify function to test various

void test() {
  Serial.println("In function test()...");
  if (false) {
    int restore = lastAngle[ROTOR];
    Serial.println("Last rotor angle " + String(restore) + " degrees.");
    delay(ONESEC);
    adjustServo(ROTOR, 125);                            // CCW from above (looking down on robot arm)
    delay(ONESEC);
    adjustServo(ROTOR, 75);                             // CW from above
    delay(ONESEC);
    adjustServo(ROTOR, restore);
  }
  else if (false) {
    int restore = lastAngle[LOWER];
    Serial.println("Last lower arm angle " + String(restore) + " degrees.");
    adjustServo(LOWER, 50);                             // Up
    delay(ONESEC);
    adjustServo(LOWER, 130);                            // Down
    delay(ONESEC);
    adjustServo(LOWER, restore);        
  }
  else if (false) {
    int restore = lastAngle[UPPER];
    Serial.println("Last lower arm angle " + String(restore) + " degrees.");
    adjustServo(UPPER, 50);                             // Down
    delay(ONESEC);
    adjustServo(UPPER, 130);                            // Up
    delay(ONESEC);
    adjustServo(UPPER, restore);        
  }
  else if (false) {
    int restore = lastAngle[GRIPPER];
    Serial.println("Last gripper angle " + String(restore) + " degrees.");
    adjustServo(GRIPPER, 120);                          // Open wider (out)
    delay(ONESEC);
    adjustServo(GRIPPER, 170);                          // Close narrower (in)
    delay(ONESEC);
    adjustServo(GRIPPER, restore);    
  }
  else if (false) {
    int target[NUM_OF_SERVOS];
    target[ROTOR] = 125;                                // CCW
    target[LOWER] = 50;                                 // Up
    target[UPPER] = 130;                                // Up
    target[GRIPPER] = lastAngle[GRIPPER];               // No change
    multAdjustServo(target);
    delay(ONESEC);
    target[ROTOR] = 75;                                 // CCW
    target[LOWER] = 130;                                // Down
    target[UPPER] = 50;                                 // Down
    target[GRIPPER] = lastAngle[GRIPPER];               // No change
    multAdjustServo(target);
    delay(ONESEC);
    multAdjustServo(raHome);
  }
  else if (true)
    roar();
  else
    Serial.println("No servo test specified.");
}

// Whimsy

void roar() {
  // Imitate head movement of lion roar
  // Sound effect: MGM lion roar
  const int roarUp[NUM_OF_SERVOS] = {125, 50, 130, 120};
  const int roarDown[NUM_OF_SERVOS]= {65, 130, 50, 160};
  long upDelay = 3, downDelay = 2;
  long pauseAtTop = ONESEC, pauseAtBottom = ONESEC;
  delay(TWOSEC);
  adjustServo(GRIPPER, 170, 20);                        // Slowly close gripper
  delay(TWOSEC);
  multAdjustServo(roarUp, upDelay);
  delay(pauseAtTop);
  multAdjustServo(roarDown, downDelay);
  delay(pauseAtBottom);
  multAdjustServo(raHome, 4);
}