136 lines
3.0 KiB
C++
136 lines
3.0 KiB
C++
#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 =250;
|
|
int max1 =560;
|
|
int pos1 = position(offset1, flex1, max1);
|
|
|
|
//Zeigefinger
|
|
int offset2 = 150;
|
|
int max2 = 620;
|
|
int pos2 = position(offset2, flex2, max2);
|
|
|
|
//Mittelfinger
|
|
int offset3 = 440;
|
|
int max3 = 650;
|
|
int pos3 = position(offset3, flex3, max3);
|
|
|
|
//Ringfinger
|
|
int offset4 = 250;
|
|
int max4 = 500;
|
|
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);
|
|
}
|
|
|