162 lines
3.6 KiB
C++
162 lines
3.6 KiB
C++
|
|
//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
|
|
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);
|
|
|
|
/* Servo-Kontakte als Ausgang */
|
|
pinMode(servoPin1, OUTPUT);
|
|
pinMode(servoPin2, OUTPUT);
|
|
pinMode(servoPin3, OUTPUT);
|
|
pinMode(servoPin4, OUTPUT);
|
|
pinMode(servoPin5, OUTPUT);
|
|
|
|
//Sensor-kontakte al EINGANG
|
|
pinMode(flexPin1, INPUT);
|
|
pinMode(flexPin2, INPUT);
|
|
pinMode(flexPin3, INPUT);
|
|
pinMode(flexPin4, INPUT);
|
|
pinMode(flexPin5, INPUT);
|
|
}
|
|
|
|
int winkel_0 = -10; // Winkel bei grossen Sensorwerten
|
|
int winkel_1 =150; // Winkel bei kleinen Sensorwerten
|
|
// Parameter der Sensoren
|
|
int daumen = 600; // sensor: Violett/Grau-1
|
|
int zfinger = 600; // Sensor: Blau/Grün
|
|
int mfinger = 600; // Sensor: Rot/Braun
|
|
int rfinger = 600; // Sensor: Violett/Grau-2
|
|
int klfinger = 600; // Sensor: Orange/Gelb
|
|
|
|
void loop()
|
|
{
|
|
// bei Bedarf: Entschleunigung der Durchläufe mit Delay in Millisekunden
|
|
/* delay (80);*/
|
|
//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);
|
|
|
|
//Daumen
|
|
int pos1;
|
|
if (flex1 < daumen)
|
|
{
|
|
pos1 = winkel_0;
|
|
}
|
|
else
|
|
{
|
|
pos1 = winkel_1;
|
|
}
|
|
|
|
//Zeigefinger
|
|
int pos2;
|
|
if (flex2 < zfinger)
|
|
{
|
|
pos2 = winkel_0;
|
|
}
|
|
else
|
|
{
|
|
pos2 = winkel_1;
|
|
}
|
|
|
|
//Mittelfinger
|
|
int pos3;
|
|
if (flex3 < mfinger)
|
|
{
|
|
pos3 = winkel_0;
|
|
}
|
|
else
|
|
{
|
|
pos3 = winkel_1;
|
|
}
|
|
|
|
//Ringfinger
|
|
int pos4;
|
|
if (flex4 < rfinger)
|
|
{
|
|
pos4 = winkel_0;
|
|
}
|
|
else
|
|
{
|
|
pos4 = winkel_1;
|
|
}
|
|
|
|
//kleiner Finger
|
|
int pos5;
|
|
if (flex5 < klfinger)
|
|
{
|
|
pos5 = winkel_0;
|
|
}
|
|
else
|
|
{
|
|
pos5 = winkel_1;
|
|
}
|
|
|
|
// 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); //Daumen
|
|
finger2.write(pos2); //Zeigefinger
|
|
finger3.write(pos3); //Mittelfinger
|
|
finger4.write(pos4); //Ringfinger
|
|
finger5.write(pos5); //kleiner Finger (Sensor: weiss)
|
|
}
|
|
|