Arduino Servo

CaDucT
Offline
Зарегистрирован: 04.05.2019
 
Всем привет, можете, пожалуйста, посоветовать решение одной проблемы? Работаю над рукой робота и сенсором давления. Рабочий код уже есть, только есть одна проблема. При сжатии руки, срабатывет оператор условия силы и сервы останавливаются. Но мне нужно, чтобы при нажатии другой кнопки, он должен разжать руку, но так как там я написал серво детач, они не хотят слушаться. Посоветуете что-нибудь? Буду очень благодарен)
 
Вот подробный код:
 
#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;
  }
}

 

 

mykaida
mykaida аватар
Offline
Зарегистрирован: 12.07.2018

[quote=CaDucT]

Всем привет, можете, пожалуйста, посоветовать решение одной проблемы? 

[/quot]

Мы работаем над этим. В следующем обновлении форума эта проблема будет решенаю

[quote=CaDucT]

При сжатии руки, срабатывет оператор условия силы и сервы останавливаются. Но мне нужно, чтобы при нажатии другой кнопки, он должен разжать руку, но так как там 

[/quot]

А откуда МЫ знаем, что Вы руку сжимаете? Вы хоть пукните на этом моменте (как в том анекдоте).

CaDucT
Offline
Зарегистрирован: 04.05.2019

CaDucT пишет:

  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);
    }
  }
}

 

button отвечает за сжатие, button2 за разжатие. Но при нажатии button2 и когда работает условие силы, но рука не разжимается.

sadman41
Offline
Зарегистрирован: 19.10.2016

Натыкайте Serial.print() в циклы и посмотрите как меняются углы. Меняются ли они вообще....

mykaida
mykaida аватар
Offline
Зарегистрирован: 12.07.2018

Сдается мне что тут проблема в п/п ruka()

CaDucT
Offline
Зарегистрирован: 04.05.2019

sadman41 пишет:

Натыкайте Serial.print() в циклы и посмотрите как меняются углы. Меняются ли они вообще....

Да, углы двигаются, но мотор стоит.

sadman41
Offline
Зарегистрирован: 19.10.2016

Хотя, мне кажется, что логику менять надо и силу учитывать только при сжатии. Иначе ruka() будет  всегда детачить сервы в крайнем сжатом положении. В руку() передавать дирекшн и к нему привязываться.

CaDucT
Offline
Зарегистрирован: 04.05.2019

mykaida пишет:

Сдается мне что тут проблема в п/п ruka()

в методе ruka() только attach() и detach(), если обойтись без кнопки разжатия, то этот метод работает отлично. А что по циклу и условиям скажете? Мне кажется что-то неправильно там, но знаю где именно(

CaDucT
Offline
Зарегистрирован: 04.05.2019

sadman41 пишет:

Хотя, мне кажется, что логику менять надо и силу учитывать только при сжатии. Иначе ruka() будет  всегда детачить сервы в крайнем сжатом положении. В руку() передавать дирекшн и к нему привязываться.

Вы правы, он у меня не может выйти из цикла for так как, постоянно привязывается к методу ruka() и детачится. Спасибо) Только вот я заметил, что мне полностью придется убрать детач, так как рука при сжатии остается в цикле навечно.

sadman41
Offline
Зарегистрирован: 19.10.2016

Не обязательно. Надо подумать, подправить алгоритм и ограничивать движение только при сжатии. Тип действия передавать параметром в функцию рука(). Аттач делать не в ней, а сразу после обнаружения нажатия на кнопку.

CaDucT
Offline
Зарегистрирован: 04.05.2019

sadman41 пишет:

Не обязательно. Надо подумать, подправить алгоритм и ограничивать движение только при сжатии. Тип действия передавать параметром в функцию рука(). Аттач делать не в ней, а сразу после обнаружения нажатия на кнопку.

Пробовал перенести кусок кода для attach из метода ruka, но выходит тоже самое. Пытался переделать алгоритм, использовал break чтобы выйти из цикла, вообще не работает. Сейчас получается так, что при нажатии button1 рука сжимается, и пока рука сжимается. даже при нажатии button2, пальцы не двигаются в обратную сторону. Даже после break цикл продолжает работать дальше. Хотя это странно учитывая, что break должен выйти из цикла.