174 lines
3.8 KiB
C++
174 lines
3.8 KiB
C++
//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)
|
|
}
|
|
|