#include <smartlieducation.h>

#define MAX_SPEED 150
#define MIN_SPEED 100
#define MOVE_DELAY 2000
#define IDLE_PAUSE 1000
#define OPEN_ANGLE 100
#define CLOSE_ANGLE 20
#define TO_OPEN 1
#define TO_CLOSE -1

uint16_t GetAngle(SensorsClass * rot);
uint8_t DriveTo(char direction = 1);
uint8_t wait(uint16_t period);

uint8_t speed = MAX_SPEED;
uint8_t 
  angle1 = 0,
  angle2 = 0,
  angle3 = 0,
  angle4 = 0;

uint16_t watchTimer = 4000;
uint16_t isDriving = 0;

uint8_t buttonPressed = 0;
uint8_t startDrive = 0;

void setup() {
  Serial.begin(9600);
  // put your setup code here, to run once:
  Finger1.Stop();
  Finger2.Stop();
  Finger3.Stop();
  Finger4.Stop();

  ROT1.SetCoeffs(100, 700); // тут числа от балды - нало калибровать
  ROT2.SetCoeffs(100, 700); 
  ROT3.SetCoeffs(100, 700); 
  ROT4.SetCoeffs(100, 700); 
}

void loop() {
  // Finger1.Drive(-50);
  // delay(2000);
  // Serial.println(ROT1.GetRaw());
  // while(1);
  
  // ВАРИАНТ 1: САМАЯ минимальная реализация - движение с паузами между сжатиием и разжатием
  // Finger1.DriveRaw(speed);
  // Finger2.DriveRaw(speed);
  // Finger3.DriveRaw(speed);
  // Finger4.DriveRaw(speed);
  // delay(MOVE_DELAY);
  // Finger1.Stop();
  // Finger2.Stop();
  // Finger3.Stop();
  // Finger4.Stop();
  // delay(IDLE_PAUSE);

  // Finger1.DriveRaw((-1) * speed);
  // Finger2.DriveRaw((-1) * speed);
  // Finger3.DriveRaw((-1) * speed);
  // Finger4.DriveRaw((-1) * speed);
  // delay(MOVE_DELAY);
  // Finger1.Stop();
  // Finger2.Stop();
  // Finger3.Stop();
  // Finger4.Stop();
  // delay(IDLE_PAUSE);

  // ------------------------------------------------------------------------------------------

  // ВАРИАНТ 2: реализация чуть сложнее - тут учитываеются углы
  // DriveTo(TO_OPEN);
  // delay(IDLE_PAUSE);
  // DriveTo(TO_CLOSE);
  // delay(IDLE_PAUSE);
  
  // ------------------------------------------------------------------------------------------

  // ВАРИАНТ 3: кнопка с умным ожиданием
  if (Smartli.SwitchState(50)) {
    // если кнопка была нажата один раз - выставляем флаг, что кнопка нажата
    buttonPressed = 1;
  }

  if (buttonPressed) {
    // если прилетело, что кнопка нажата - меняем состояние работы на противоположеное
    startDrive = 1;
    delay(200);
  }

  if (startDrive) {
    // если отработал флаг начала движения - работаем
    if (DriveTo(TO_OPEN)) {
      // если в процессе отработки DriveTo() будет нажата кнопка - останавливаем движение и дальше программа не идёт
      startDrive = 0;
      buttonPressed = 0;
    } else {
      // иначе, еесли DriveTo() успешно завершилось - идём на отработку паузы между движениями
      if (wait(IDLE_PAUSE)) {
        // если во время паузы нажали кнопку - прерывает работу
        startDrive = 0;
        buttonPressed = 0;
      } else {
        // если пауза отработала идём дальше
        if (DriveTo(TO_CLOSE)) {
          startDrive = 0;
          buttonPressed = 0;
        } else {
          if (wait(IDLE_PAUSE)) {
            startDrive = 0;
            buttonPressed = 0;
          }
        }
      }
    }

    if (startDrive == 0 && buttonPressed == 0) {
      // если остановили в ходе работы - сигналоизируем
      Smartli.Vibrate(200, 3);
    }
  }
}

/**
* Метод запускает движение пальцев и висит в себе до тех порка не случится 1 из 2
* 1. Все пальцы не придут к нужному углу
* 2. Не сработает таймаут
*/
uint8_t DriveTo(char direction = 1) {
  Finger1.DriveRaw(direction * speed);
  Finger2.DriveRaw(direction * speed);
  Finger3.DriveRaw(direction * speed);
  Finger4.DriveRaw(direction * speed);
  isDriving = abs(Finger1.GetSpeed()) + abs(Finger2.GetSpeed()) + abs(Finger3.GetSpeed()) + abs(Finger4.GetSpeed());
  watchTimer = 4000;

  while (isDriving && watchTimer) {
    if (GetAngle(&ROT1) < OPEN_ANGLE) {
      Finger1.DriveRaw(direction * speed);
    } else {
      Finger1.Break();
    }

    if (GetAngle(&ROT2) < OPEN_ANGLE) {
      Finger2.DriveRaw(direction * speed);
    } else {
      Finger2.Break();
    }

    if (GetAngle(&ROT3) < OPEN_ANGLE) {
      Finger3.DriveRaw(direction * speed);
    } else {
      Finger3.Break();
    }

    if (GetAngle(&ROT4) < OPEN_ANGLE) {
      Finger4.DriveRaw(direction * speed);
    } else {
      Finger4.Break();
    }

    watchTimer--;
    if (Smartli.SwitchState(50) == 2) {
      watchTimer -= 50;
    }

    if (Smartli.SwitchState(50) == 1) {
      return 1;
    }

    if (wait(1)) {
      return 1;
    }
  }

  return 0;
}

uint16_t GetAngle(SensorsClass * rot) {
  return map(rot->GetValue(), 0, 1023, 0, 359);
}

/**
* return 0 - ожидание просто отработало
* return 1 - ожидание прервано по кнопке
*/
uint8_t wait(uint16_t period) {
  while(period) {
    period--;
    delay(1);

    if (Smartli.SwitchState(50) == 2) {
      period -= 50;
    }

    if (Smartli.SwitchState(50) == 1) {
      return 1;
    }
  }

  return 0;




}

Модуль Е

#include <smartlieducation.h>

#define MAX_SPEED 150
#define MIN_SPEED 100
#define MOVE_DELAY 2000
#define IDLE_PAUSE 1000
#define OPEN_ANGLE 100
#define CLOSE_ANGLE 20
#define TO_OPEN 1
#define TO_CLOSE -1

uint16_t GetAngle(SensorsClass * rot);
uint8_t DriveTo(char direction = 1);
uint8_t wait(uint16_t period);

uint8_t speed = MAX_SPEED;
uint8_t 
  angle1 = 0,
  angle2 = 0,
  angle3 = 0,
  angle4 = 0;

uint16_t watchTimer = 4000;
uint16_t isDriving = 0;

uint8_t mode = 0; // 0 - reabilitation | 1 - emg
uint8_t state = 0; // 0 - close | 1 - open

void setup() {
  Serial.begin(9600);
  // put your setup code here, to run once:
  Finger1.Stop();
  Finger2.Stop();
  Finger3.Stop();
  Finger4.Stop();

  ROT1.SetCoeffs(100, 700); // тут числа от балды - нало калибровать
  ROT2.SetCoeffs(100, 700); 
  ROT3.SetCoeffs(100, 700); 
  ROT4.SetCoeffs(100, 700); 
  
  EMG1.SetCoeffs(200, 500);
  EMG2.SetCoeffs(200, 500);
}

void loop() {
  uint16_t buttonTime = Smartli.SwitchPressedTime(50);
  
  if (buttonTime > 6000) {
    mode = !mode ? 1 : 0;
  } else if (buttonTime > 500) {
    if (state == 0) {
      if (!DriveTo(TO_OPEN)) {
        state = 1;
      }      
    } else {
      if (!DriveTo(TO_OPEN)) {
        state = 0;
      } 
    }    
  }
  
  if (mode) {
    // если работаем по ЭМГ
    uint8_t open = EMG1.GetValue() > 30;
    uint8_t close = EMG2.GetValue() > 30;
    if (open && !close) {
      DriveTo(TO_OPEN);
    } else if (!open && close) {
      DriveTo(TO_CLOSE);
    } else {
      Finger1.Stop();
      Finger2.Stop();
      Finger3.Stop();
      Finger4.Stop();
    }
  } else {
    DriveTo(TO_OPEN);
    delay(IDLE_PAUSE);
    DriveTo(TO_CLOSE);
    delay(IDLE_PAUSE);
  }
}

/**
* Метод запускает движение пальцев и висит в себе до тех порка не случится 1 из 2
* 1. Все пальцы не придут к нужному углу
* 2. Не сработает таймаут
*/
uint8_t DriveTo(char direction = 1) {
  Finger1.DriveRaw(direction * speed);
  Finger2.DriveRaw(direction * speed);
  Finger3.DriveRaw(direction * speed);
  Finger4.DriveRaw(direction * speed);
  isDriving = abs(Finger1.GetSpeed()) + abs(Finger2.GetSpeed()) + abs(Finger3.GetSpeed()) + abs(Finger4.GetSpeed());
  watchTimer = 4000;

  while (isDriving && watchTimer) {
    if (GetAngle(&ROT1) < OPEN_ANGLE) {
      Finger1.DriveRaw(direction * speed);
    } else {
      Finger1.Break();
    }

    if (GetAngle(&ROT2) < OPEN_ANGLE) {
      Finger2.DriveRaw(direction * speed);
    } else {
      Finger2.Break();
    }

    if (GetAngle(&ROT3) < OPEN_ANGLE) {
      Finger3.DriveRaw(direction * speed);
    } else {
      Finger3.Break();
    }

    if (GetAngle(&ROT4) < OPEN_ANGLE) {
      Finger4.DriveRaw(direction * speed);
    } else {
      Finger4.Break();
    }

    watchTimer--;
    if (Smartli.SwitchState(50) == 2) {
      watchTimer -= 50;
    }

    if (Smartli.SwitchState(50) == 1) {
      return 1;
    }

    if (wait(1)) {
      return 1;
    }
  }

  return 0;
}

uint16_t GetAngle(SensorsClass * rot) {
  return map(rot->GetValue(), 0, 1023, 0, 359);
}

/**
* return 0 - ожидание просто отработало
* return 1 - ожидание прервано по кнопке
*/
uint8_t wait(uint16_t period) {
  while(period) {
    period--;
    delay(1);

    if (Smartli.SwitchState(50) == 2) {
      period -= 50;
    }

    if (Smartli.SwitchState(50) == 1) {
      return 1;
    }
  }

  return 0;
}