Effetto Onda con Servomotori

International Italiano Software
5

Ho risolto in questo modo, usando la libreria VarSpeedServo trovata su GitHub:

#include <VarSpeedServo.h> // external library for variable speed servo

VarSpeedServo s1, s2, s3, s4, s5; // servo initialization

// INTESITY PARAMETER
int start[9][5] =  {         //columns=finger(0=thumb,4=littlefinger)
  {50,  35,  35, 145, 130},  //a1      {50,  35,  35, 145, 130}
  {50,  35,  35, 145, 130},  //a2      {100, 105, 55, -105, -120}
  {50,  35,  35, 145, 130},  //a3
  {50,  35,  35, 145, 130},  //p1
  {50,  35,  35, 145, 130},  //p2
  {50,  35,  35, 145, 130},  //p3
  {50,  35,  35, 145, 130},  //w1
  {50,  35,  35, 145, 130},  //w2
  {50,  35,  35, 145, 130}   //w3
};
int amplitude [9][5] = {
  {100, 105, 55, -105, -120},//a1
  {100, 105, 55, -105, -120},//a2
  {100, 105, 55, -105, -120},//a3
  {100, 105, 55, -105, -120},//p1
  {100, 105, 55, -105, -120},//p2
  {100, 105, 55, -105, -120},//p3
  {100, 105, 55, -105, -120},//w1
  {100, 105, 55, -105, -120},//w2
  {100, 105, 55, -105, -120},//w3
};

// wave exercise starting intervals
int interval[10] = {   0,    1000, 2000,  3000,  4000, // close
                       7000, 8000, 9000, 10000, 11000  // open
                   };

void(* software_reboot)(void) = 0; // reboot function

int servospeed = 20;               //speed of the servos
int rip;                           // number of repetitions
int i = 0;                         //repetitions counter
int j = 0;                         //intensity counter
char READ[20];                     //serial read array
unsigned long startMillis;         //time counter for wave exercise
unsigned long currentMillis;       //time counter for wave exercise

currentMillis = millis();
    // close
    if (currentMillis - startMillis > interval[0]) {
      s1.write(start[j][0] + amplitude[j][0], servospeed);
    }
    if (currentMillis - startMillis > interval[1]) {
      s2.write(start[j][1] + amplitude[j][1], servospeed);
    }
    if (currentMillis - startMillis > interval[2]) {
      s3.write(start[j][2] + amplitude[j][2], servospeed / 2);
    }
    if (currentMillis - startMillis > interval[3]) {
      s4.write(start[j][3] + amplitude[j][3], servospeed);
    }
    if (currentMillis - startMillis > interval[4]) {
      s5.write(start[j][4] + amplitude[j][4], servospeed);
      s1.wait(); s2.wait(); s3.wait(); s4.wait(); s5.wait();
    }
    // REBOOT
    reboot;
    // open
    if (currentMillis - startMillis > interval[5]) {
      s5.write(start[j][4], servospeed);
    }
    if (currentMillis - startMillis > interval[6]) {
      s4.write(start[j][3], servospeed);
    }
    if (currentMillis - startMillis > interval[7]) {
      s3.write(start[j][2], servospeed / 2);
    }
    if (currentMillis - startMillis > interval[8]) {
      s2.write(start[j][1], servospeed);
    }
    if (currentMillis - startMillis > interval[9]) {
      s1.write(start[j][0], servospeed);
      s1.wait(); s2.wait(); s3.wait(); s4.wait(); s5.wait();
    }
    // REBOOT
    reboot;