for andrew

#include <Servo.h>
#include <SoftwareSerial.h>


SoftwareSerial armSerial(2,4); //2 is rx, 4 is tx
const int avg_len = 10;
const int num_fingers = 3;
const int fingerAdd[num_fingers] = {A0,A1,A2};
const int servoAdd[num_fingers] = {11,10,9};
int move_speed = 5;
const int accel = 1;
const int accel_end = 30;
int avg_value[num_fingers][avg_len];
int avg_avg[num_fingers] =  {0,0,0};
int servo_pos[num_fingers] =  {0,0,0};

int input_pot = 0;

int act_pos[num_fingers] = {0,0,0};

Servo finger_servo[num_fingers];


int max_val[num_fingers] = {0,0,0}; //= 265;
int min_val[num_fingers] = {0,0,0};//= 160;


void setup() {
  // put your setup code here, to run once:
   Serial.begin(57600);
   armSerial.begin(57600);


 Serial.println("Initializing Gauntlet");
  delay(1000);
  Serial.print("Squeeze in 3");
  delay(1000);
  Serial.print(" 2");
  delay(1000);
  Serial.print(" 1");
  delay(1000);


  for(int x = 0; x < num_fingers; x++)
  {
  finger_servo[x].attach(servoAdd[x]);

  min_val[x] = readFingers(x);
  Serial.println(min_val[x]);
  }

  delay(1000);
  Serial.print("Extend fingers in 3");
  delay(1000);
  Serial.print(" 2");
  delay(1000);
  Serial.print(" 1");
  delay(1000);


  for(int x = 0; x < num_fingers; x++)
  {
    max_val[x] = readFingers(x);
    Serial.println(max_val[x]);
  for(int i = 0; i < avg_len; i++) //clear array
  {
    avg_value[x][i] = 0;
  }

  }







   //initialization phase will be required with a glove, or use a flush timer but this solution would limit the ammount of time to perform operations

  }



void loop() {
  // put your main code here, to run repeatedly:

 // move_speed = map(analogRead(A1), 0,1023,1,10);
  //Serial.println(move_speed);
  for(int x = 0; x < num_fingers; x++)
  {

    //delay(50);
      //shift array to the right


      for(int i = 1; i < avg_len; i++)
      {
         avg_value[x][i] = avg_value[x][i-1];
      }

      avg_value[x][0] = readFingers(x); //FIX THIS NEXT TIME
    //  Serial.print((String)x + " : " + (String)avg_value[x][0] +" ");

      if(avg_value[x][0]> max_val[x])
     {
        avg_value[x][0] = (max_val[x]-min_val[x])/2;
      }

      if(avg_value[x][0] < min_val[x])
      {
       avg_value[x][0] =  (max_val[x]-min_val[x])/2;
      }

      avg_avg[x] = 0;

  for(int i = 0; i < avg_len; i++)
  {
    avg_avg[x] += avg_value[x][i];
  }

  avg_avg[x] = avg_avg[x]/avg_len;

  //Serial.println(avg_avg);

  if(map(avg_avg[x], min_val[x], max_val[x], 0, 180) >= 0)
  {
    servo_pos[x] = map(avg_avg[x], min_val[x], max_val[x], 0, 180); //map the avg_val of finger to a servo
  }

  int temp_move = move_speed;

  if(act_pos[x]+ accel_end < servo_pos[x])
  {
    temp_move += move_speed;
  }
  else if(act_pos[x] - accel_end > servo_pos[x])
  {
    temp_move += move_speed;
  }

  for(int i = temp_move; i >= 0; i--)
  {
    if((act_pos[x] + i) < servo_pos[x]) //adding move_speed keeps servo from overshooting
    {
      act_pos[x] += i;
     // Serial.print(i);
      break;
    }
    else if (act_pos[x] - i > servo_pos[x])
    {
      act_pos[x] -= i;
      //Serial.print(i);
      break;
    }
  }


   //Serial.println(servo_pos);
  //Serial.println(move_speed);//act_pos);

  //Telemetry


 Serial.print((String)x + ": " + (String)servo_pos[x] + " ");



   finger_servo[x].write(act_pos[x]);

 delay(1);
  }


Serial.println();


}

int readFingers(int i)
{
  int16_t adc[]={0,0,0,0}; // we read from the ADC, we have a sixteen bit integer as a result
  armSerial.flush();
 // adc[0] = (int16_t)((armSerial.readStringUntil('\t')).toInt());
  adc[0] = (int16_t)((armSerial.readStringUntil('\t')).toInt());
  adc[1] = (int16_t)((armSerial.readStringUntil('\t')).toInt());
  adc[2] = (int16_t)((armSerial.readStringUntil('\t')).toInt());
  adc[3] = (int16_t)((armSerial.readStringUntil('\r')).toInt());

  return adc[i]; //map((adc[i] * 0.1875)/1000,0,3.3,0,255);
}