#include <Servo.h>

//Funktion zur Berechnung der Servo-Position
//Offset: nicht benötigter Bereich der Widerstandswerte
//flex: gemesener Widerstandswert
//Maximalwinkel: 150°; Minimalwinkel: -10°
//Rückgabe: Position des Servos
int position(int offset, int flex, int max)
{
	int pos;
	int div = (max - offset) / 10;
	pos = (flex - offset) / div * 16;
	if(pos < -10) pos = -30;
	else if(pos > 150) pos = 140;
	
	return pos;
	
}

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);
	//Die Pins und die entsprechenden Servos zusammenführen
	
	//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(servoPin5, OUTPUT);
	pinMode(servoPin4, OUTPUT);
	
	//Sensor-Kontakte als Eingang
	pinMode(flexPin1, INPUT);
	pinMode(flexPin2, INPUT);
	pinMode(flexPin3, INPUT);
	pinMode(flexPin5, INPUT);
	pinMode(flexPin4, INPUT);

}

void loop()
{
	//Auslesen der Sensorwiderstände
	int flex1 = analogRead(flexPin1);
	int flex2 = analogRead(flexPin2);
	int flex3 = analogRead(flexPin3);
	int flex4 = analogRead(flexPin4);
	int flex5 = analogRead(flexPin5);

	//Daumen
	int offset1 =100;
	int max1 =600;
	int pos1 = position(offset1, flex1, max1);
	
	//Zeigefinger
	int offset2 = 360;
	int max2 = 640;
	int pos2 = position(offset2, flex2, max2);
	
	//Mittelfinger
	int offset3 = 440;
	int max3 = 650;
	int pos3 = position(offset3, flex3, max3);
	
	//Ringfinger
	int offset4 = 470;
	int max4 = 600;
	int pos4 = position(offset4, flex4, max4);
	
	//Kleiner Finger
	int offset5 = 290;
	int max5 = 530;
	int pos5 = position(offset5, flex5, max5);
	
	//Kontrollausgabe der Werte über den 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("/// KleinerFinger");
	Serial.print(pos5);
	Serial.print(" - ");
	Serial.print(flex5);
	Serial.print("\n");
	
	//Gib dem Servo anhand von pos an wie weit er sich bewegen muss.
	finger1.write(pos1);
	finger2.write(pos2);
	finger3.write(pos3);
	finger4.write(pos4);
	finger5.write(pos5);
}