Arduino.ru

#include <VarSpeedServo.h>
#define FSR_PIN A0
#define ALPHA 0.2
int angle;

int button = 10; //кнопка сжатия пальцев
int button2 = 11; //кнопка разжатия пальцев
int button3 = 12; //пока не использован нигде
int val;
int val_filter;
int U_out;
float force;
VarSpeedServo servo1;
VarSpeedServo servo2;
VarSpeedServo servo3;
VarSpeedServo servo4;
VarSpeedServo servo5;
//VarSpeedServo servo1;
const int s1 = 160;  //константные углы для каждого серво
const int s2 = 80;
const int s3 = 0;
const int s4 = 45;
const int s5 = 180;
int angle1;
int angle2;

void setup() {
  Serial.begin(115200);
  servo1.attach(5);
  servo2.attach(7);
  servo3.attach(6);
  servo4.attach(4);
  servo5.attach(3);
  servo1.write(s1);
  servo2.write(s2);
  servo3.write(s3);
  servo4.write(s4);
  servo5.write(s5);
  //pinMode(led, OUTPUT);
  pinMode(button, INPUT);
  pinMode(button2, INPUT);
  pinMode(button3, INPUT);
}

void loop() {
  // put your main code here, to run repeatedly:
  if (digitalRead(button) == HIGH) {
    for (angle1 = 160, angle2 = 80; angle1 >= 80 && angle2 <= 150;) {  //тут я работаю только с двумя серво
      servo1.write(angle1);
      servo2.write(angle2);
      if (ruka()) {
        angle1--;
        angle2++;
      }
      delay(150);
    }
  }
  if (digitalRead(button2) == HIGH) {
    for (angle1 = 80, angle2 = 150; angle1 <= 160 && angle2 >= 80;) {
      servo1.write(angle1);
      servo2.write(angle2);
      if (ruka()) {
        angle1++;
        angle2--;
      }
      delay(150);
    }
  }
}
bool ruka() {
  float force = 7.5211 * exp(0.005 * val);
  force = force / 9.81;
  val = analogRead(FSR_PIN);
  val_filter = ALPHA * val + (1 - ALPHA) * val_filter;



  if (force >= 10) {
    servo1.detach();
    servo2.detach();
    Serial.print("Bytes");
    Serial.print("\t");
    Serial.println(val);
    Serial.print("Force");
    Serial.print("\t");
    Serial.println(force);
    Serial.print("Angle");
    Serial.print("\t");
    Serial.println(angle);
    return false;
  }
  else {
    servo1.attach(7);
    servo2.attach(5);
    Serial.print("Bytes");
    Serial.print("\t");
    Serial.println(val);
    Serial.print("Force");
    Serial.print("\t");
    Serial.println(force);
    Serial.print("Angle");
    Serial.print("\t");
    Serial.println(angle);
    return true;
  }
}