//Slave Arduino Code:

#include<SPI.h>     //Library for SPI 
#include "Servo.h"  //Library for Servo 

#define LENGTH 6
//byte holdingArray[6];
volatile bool msgTransferInProcess = false;
volatile bool received = false;
volatile byte j = 0;
volatile byte c;
byte jointPositions [LENGTH];

Servo left_servo;
Servo right_servo;
Servo rotate_servo;
Servo grip_rot_servo;
Servo gripper_servo;


void setup (void)
{
    left_servo.attach(4);  
    right_servo.attach(3);
    rotate_servo.attach(2);
    grip_rot_servo.attach(5);
    gripper_servo.attach(6);   //define Pin for each servomotor
    
  Serial.begin (9600);
   // have to send on master in, *slave out*
  pinMode(MISO, OUTPUT);

                     // turn on SPI in slave mode
  SPCR |= _BV(SPE);
                     // turn on interrupts
  SPCR |= _BV(SPIE);
}                    // end of setup

void loop (void)
{
  byte n = 1;
  
  if (received== true)    
  {
   for ( byte k = 1 ; k < LENGTH; k++){
     Serial.println("");
     Serial.print(jointPositions[k]);    
    
   }
   
      j = 0;
      for (byte h = 1; h <LENGTH; h++)
        {
        rotate_servo.write(jointPositions[1]);
        right_servo.write(jointPositions[2]);
        left_servo.write(jointPositions[3]);
        grip_rot_servo.write(jointPositions[4]);
        gripper_servo.write(jointPositions[5]);
        }
     
    Serial.println();
    Serial.println("=================");
    
    msgTransferInProcess  = false;     
    received = false;
    }
  }

ISR (SPI_STC_vect)
{
  c = SPDR;
  if (msgTransferInProcess == false)
  {
    if (c == 0xCD)
    {
      SPDR = 0xEF;
      msgTransferInProcess  = true;
    }
  }
  else
  {
    jointPositions[j] = c;
    j++;
    if (j == LENGTH-1)
    {
      received = true;
    }
  }
}
