//Define sensors and servos

#include <Servo.h> //Includes servo library

Servo finger1, finger2, finger3, finger4, finger5;

int servoPin1 = 5;   //gelb
int servoPin2 = 6;   //gruen
int servoPin3 = 9;   //blau
int servoPin4 = 10;  //weiss
int servoPin5 = 3;   //braun

int flexPin1 = A0;   //Daumen (braun)
int flexPin2 = A1;   //Zeigefinger (gelb)
int flexPin3 = A2;   //Mittelfinger (gruen)
int flexPin4 = A3;   //Ringfinger (blau)
int flexPin5 = A4;   //kleiner Finger (weiss)
                     //A5 = NC

void setup()
{
  Serial.begin(9600);
  //Attach the servo objects to their respective pins
  //Daumen (Sensor: braun, Servo: gelb)
  finger1.attach(servoPin1);
  
  //Zeigefinger (Sensor: gelb, Servo: gruen)
  finger2.attach(servoPin2);
  
  //Mittelfinger (Sensor: gruen, Servo: blau)
  finger3.attach(servoPin3);
  
  //Ringfinger (Sensor: blau, Servo: weiss)
  finger4.attach(servoPin4);
  
  //kleiner Finger (Sensor: weiss, Servo: braun)
  finger5.attach(servoPin5);
  
  /* set each servo pin to output; I'm not acutally sure if this is
  even necessary, but I did just in case it is */
  pinMode(servoPin1, OUTPUT);
  pinMode(servoPin2, OUTPUT);
  pinMode(servoPin3, OUTPUT);
  pinMode(servoPin4, OUTPUT);
  pinMode(servoPin5, OUTPUT);
  
  //Set each flex sensor pin to input: this is necessary
  pinMode(flexPin1, INPUT);
  pinMode(flexPin2, INPUT);
  pinMode(flexPin3, INPUT);
  pinMode(flexPin4, INPUT);
  pinMode(flexPin5, INPUT);
}

int winkel = 145;
// Parameter der Sensoren
int daumen = 600;
int zfinger = 400;
int mfinger = 750;
int rfinger = 700;
int klfinger = 500;

void loop()
{
 /* delay (80);*/
  //Defines analog input variables
  //Auslesen der Sensor-Widerstände
  int flex1 = analogRead(flexPin1);
  int flex2 = analogRead(flexPin2);
  int flex3 = analogRead(flexPin3);
  int flex4 = analogRead(flexPin4);
  int flex5 = analogRead(flexPin5);
  
  /* Defines "pos" variables as being proportional to the flex inputs.
  The 400 to 700 value range seemed adequate for my sensors, but you can change
  yours accordingly. */
  //Daumen
/*  int pos1 = map(flex1, 649, 650, 0, 1023);
  pos1 = constrain(pos1, 0, 145); */

  int pos1;
  if (flex1 < daumen)
    {
    pos1 = 0;  
    }
  else
    {
    pos1 = winkel;    
    }  

  //Zeigefinger
/*  int pos2 = map(flex2, 449, 450, 0, 1023);
  pos2 = constrain(pos2, 0, 145); */
  int pos2;
  if (flex2 < zfinger)
    {
    pos2 = 0;  
    }
  else
    {
    pos2 = winkel;    
    }

  //Mittelfinger
/*  int pos3 = map(flex3, 949, 950, 0, 1023);
  pos3 = constrain(pos3, 0, 145);*/
    int pos3;
  if (flex3 < mfinger)
    {
    pos3 = 0;  
    }
  else
    {
    pos3 = winkel;    
    }
  
  //Ringfinger
/*  int pos4 = map(flex4, 599, 600, 0, 1023); 
  pos4 = constrain(pos4, 0, 145);*/
  int pos4;
  if (flex4 < rfinger)
    {
    pos4 = 0;  
    }
  else
    {
    pos4 = winkel;    
    }  

  //kleiner Finger
/*  int pos5 = map(flex5, 599, 600, 0, 1023);
  pos5 = constrain(pos5, 0, 145); */
  int pos5;
  if (flex5 < klfinger)
    {
    pos5 = 0;  
    }
  else
    {
    pos5 = winkel;    
    }  
  
  // Kontrollausgabe der Werte über Serial-Monitor
  Serial.print(" Daumen ");
  Serial.print(pos1);
  Serial.print(" - "); 
  Serial.print(flex1);
  Serial.print("*** Zeigefinger ");   
  Serial.print(pos2);
  Serial.print(" - ");
  Serial.print(flex2);
  Serial.print("*** Mittelfinger ");
  Serial.print(pos3);
  Serial.print(" - "); 
  Serial.print(flex3);
  Serial.print("*** Ringfinger ");   
  Serial.print(pos4);
  Serial.print(" - ");
  Serial.print(flex4);  
  Serial.print("*** kleiner Finger ");   
  Serial.print(pos5);
  Serial.print(" - ");
  Serial.print(flex5);  
  Serial.print("\n");

    //Tells servos to move by the amount specified in the "pos" variables
  finger1.write(pos1);
  finger2.write(pos2);
  finger3.write(pos3);
  finger4.write(pos4);
  finger5.write(pos5); //kleiner Finger (Sensor: weiss)
}