// Robot hand
// Include the servo library to add servo-control functions:
#include <Servo.h>
// Create a servo "object", called servo#. Each servo object
// controls one servo
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;
Servo servo5;
// Define the analog input pin to measure flex sensor position:
const int flexpin0 = 0;
const int flexpin1 = 1;
const int flexpin2 = 2;
const int flexpin3 = 3;
const int flexpin4 = 4;
void setup()
{
// Enable control of a servo on pin 9-13:
servo1.attach(9);
servo2.attach(10);
servo3.attach(11);
servo4.attach(12);
servo5.attach(13);
}
void loop()
{
int flexposition0; // Input value from the analog pin.
int servoposition0; // Output value to the servo.
int flexposition1; // Input value from the analog pin.
int servoposition1; // Output value to the servo.
int flexposition2; // Input value from the analog pin.
int servoposition2; // Output value to the servo.
int flexposition3; // Input value from the analog pin.
int servoposition3; // Output value to the servo.
int flexposition4; // Input value from the analog pin.
int servoposition4; // Output value to the servo.
// Read the position of the flex sensor (0 to 1023):
flexposition0 = analogRead(flexpin0);
flexposition1 = analogRead(flexpin1);
flexposition2 = analogRead(flexpin2);
flexposition3 = analogRead(flexpin3);
flexposition4 = analogRead(flexpin4);
// map analogread's range to servo range
servoposition0 = map(flexposition0, 600, 900, 0, 180);
servoposition0 = constrain(servoposition0, 0, 180);
servoposition1 = map(flexposition1, 600, 900, 0, 180);
servoposition1 = constrain(servoposition1, 0, 180);
servoposition2 = map(flexposition2, 600, 900, 0, 180);
servoposition2 = constrain(servoposition2, 0, 180);
servoposition3 = map(flexposition3, 600, 900, 0, 180);
servoposition3 = constrain(servoposition3, 0, 180);
servoposition4 = map(flexposition4, 600, 900, 0, 180);
servoposition4 = constrain(servoposition4, 0, 180);
// Commands servo to move to position determined by flexor
servo1.write(servoposition0);
servo2.write(servoposition1);
servo3.write(servoposition2);
servo4.write(servoposition3);
servo5.write(servoposition4);
}