//PWM-Duty-Cycle:
// 0Grad  = 0,55 ms
// 5Grad  = 0,6 ms
// 10Grad = 0,65 ms
// 15Grad = 0,7 ms
// 20Grad = 0,75 ms
// 25Grad = 0,8 ms
// 30Grad = 0,85 ms
// 35Grad = 0,9 ms
// 40 Grad = 0,95 ms
// 45 grad = 1,00 ms
// 50 Grad = 1,05 ms
// 55 Grad = 1,10 ms
// 60 Grad = 1,15 ms
// 65 Grad = 1,20 ms
// 70 Grad = 1,25 ms
// 75 Grad = 1,30 ms
// 80 Grad = 1,35 ms
// 85 Grad = 1,40 ms
// 90 Grad = 1,45 ms
// 180 Grad = 2,35ms 



//Define sensors and servos

// servo.h instanziert Servo-Objekte und konfiguriert Ausgänge als PWM (bis zu 2ms Duty-Cycle bei 50Hz)
#include <Servo.h> //Includes servo library

Servo finger1, finger2, finger3, finger4, finger5;

int servoPin1 = 5;   //gelb, kleiner Finger
int servoPin2 = 6;   //gruen, Ringfinger
int servoPin3 = 9;   //blau. Mittelfinger
int servoPin4 = 10;  //weiss, Zeigefinger
int servoPin5 = 3;   //braun, Daumen

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);
  
  /* Servo-Kontakte als Ausgang */
  pinMode(servoPin1, OUTPUT);
  pinMode(servoPin2, OUTPUT);
  pinMode(servoPin3, OUTPUT);
  pinMode(servoPin4, OUTPUT);
  pinMode(servoPin5, OUTPUT);

}

int winkel_1 = 145;  // Winkel bei kleinen Sensorwerten
int winkel_2 =10;     // Winkel bei grossen Sensorwerten
int speed1 = 800;


void loop()
{
int pos1;
int pos2;
int pos3;
int pos4;
int pos5;
  
  pos1 = winkel_2; 
  finger1.write(pos1); //Daumen
  delay (speed1);
  pos2 = winkel_2;
  finger2.write(pos2); //Zeigefinger 
  delay (speed1);
  pos3 = winkel_2; 
  finger3.write(pos3); //Mittelfinger
  delay (speed1);
  pos4 = winkel_2; 
  finger4.write(pos4); //Ringfinger 
  delay (speed1);
  pos5 = winkel_2; 
  finger5.write(pos5); //kleiner Finger (Sensor: weiss)
  delay (speed1);
  pos1 = winkel_1; 
  finger1.write(pos1); //Daumen
  delay (speed1);
  pos2 = winkel_1; 
  finger2.write(pos2); //Zeigefinger
  delay (speed1);
  pos3 = winkel_1; 
  finger3.write(pos3); //Mittelfinger
  delay (speed1);
  pos4 = winkel_1; 
  finger4.write(pos4); //Ringfinger 
  delay (speed1);
  pos5 = winkel_1; 
  finger5.write(pos5); //kleiner Finger (Sensor: weiss)
  delay (speed1); 
  
  // 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");
*/
}