Design for a Cause 2021 - Robotic Hand (Blog #8) - Code update

I wanted to post the updated code I am now using for the robotic hand. This code works 100% better, it calibrates the signal for each finger for more precise movement.

#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif

#include "EMGFilters.h"

#include <Servo.h>

#define TIMING_DEBUG 1

#define SensorPin A0 // input pin number

EMGFilters myFilter;

SAMPLE_FREQUENCY sampleRate = SAMPLE_FREQ_500HZ;

NOTCH_FREQUENCY humFreq = NOTCH_FREQ_60HZ;

Servo myservo1;
Servo myservo2;
Servo myservo3;
Servo myservo4;
Servo myservo5;

static int Threshold = 50;
const int stepsPer = 200;
int envlope = 0;
const int ledPin = 13;


unsigned long timeStamp;
unsigned long timeBudget;

int sensorValue = 0;         // the sensor value
int fingerValue = 0;
int a = 400;
int b = 0;
int x = 0;
int y = 0;
int calB = 0;
int calR = 0;
int calM = 0;
int calI = 0;
int calT = 0;
float calMod = 1.1; //Add a 10% calibration modifier
float calModMinus = 0.9; //Subtract 10% calibration Modifier

void setup(){
  
  myFilter.init(sampleRate, humFreq, true, true, true);
  Serial.begin(115200);
  timeStamp = micros();

  myservo1.attach(2);
  myservo1.attach(3);
  myservo1.attach(4);
  myservo1.attach(5);
  myservo1.attach(6);

  b = 0;
  x = 0;

  Serial.println("Calibrate fingers in 2 seconds");
  delay(1000);
  Serial.println("Calibrate Baby finger");
  delay(250);
  digitalWrite(ledPin, HIGH);
  delay(250);
  digitalWrite(ledPin, LOW);
  delay(250);
  digitalWrite(ledPin, HIGH);
  delay(250);
  
    while (millis() < 15000) { //Calibrate Baby Finger
    timeStamp = micros();
    int Value = analogRead(SensorPin);

    // filter processing
    int DataAfterFilter = myFilter.update(Value);
    int envlope = (sq(DataAfterFilter));
    
    x = envlope;
    calB = calB + x;
    b = b + 1;  

 }

  calB = calB / b; //Average Baby Finger Readings

  Serial.println(calB); //Show calibration Reading
  //
  digitalWrite(ledPin, LOW);
  delay(250);
  digitalWrite(ledPin, HIGH);
  delay(250);
  digitalWrite(ledPin, LOW);
  delay(250);
  Serial.println("Calibrate Ring finger");
  delay(250);
  digitalWrite(ledPin, HIGH);
  delay(250);
  digitalWrite(ledPin, LOW);
  delay(250);
  digitalWrite(ledPin, HIGH);
  delay(250);
  
    b = 0;
    
   while (millis() < 15000) { //Calibrate Ring Finger
    timeStamp = micros();
    int Value = analogRead(SensorPin);

    // filter processing
    int DataAfterFilter = myFilter.update(Value);

    int envlope = (sq(DataAfterFilter));
    
    x = envlope;
    calR = calR + x;
    b = b + 1;  

 }

  calR = calR / b; //Average Ring Finger Readings
  Serial.println(calR); //Show calibration Reading
  //
  digitalWrite(ledPin, LOW);
  delay(250);
  digitalWrite(ledPin, HIGH);
  delay(250);
  digitalWrite(ledPin, LOW);
  delay(250);
  Serial.println("Calibrate Middle finger");
  delay(250);
  digitalWrite(ledPin, HIGH);
  delay(250);
  digitalWrite(ledPin, LOW);
  delay(250);
  digitalWrite(ledPin, HIGH);
  delay(250);

   b = 0;
   
   while (millis() < 15000) { //Calibrate Middle Finger
    timeStamp = micros();
    int Value = analogRead(SensorPin);

    // filter processing
    int DataAfterFilter = myFilter.update(Value);

    int envlope = (sq(DataAfterFilter));
    
    x = envlope;
    calM = calM + x;
    b = b + 1;  

 }

  calM = calM / b; //Average Middle Finger Readings
  Serial.println(calM); //Show calibration Reading
  //
  digitalWrite(ledPin, LOW);
  delay(250);
  digitalWrite(ledPin, HIGH);
  delay(250);
  digitalWrite(ledPin, LOW);
  delay(250);
  Serial.println("Calibrate Index finger");
  delay(250);
  digitalWrite(ledPin, HIGH);
  delay(250);
  digitalWrite(ledPin, LOW);
  delay(250);
  digitalWrite(ledPin, HIGH);
  delay(250);
  
    b = 0;
    
    while (millis() < 15000) { //Calibrate Index Finger
    timeStamp = micros();
    int Value = analogRead(SensorPin);

    // filter processing
    int DataAfterFilter = myFilter.update(Value);

    int envlope = (sq(DataAfterFilter));
    
    x = envlope;
    calI = calI + x;
    b = b + 1; 
     

 }

  calI = calI / b; //Average Index Finger Readings
  Serial.println(calI); //Show calibration Reading
  //
  digitalWrite(ledPin, LOW);
  delay(250);
  digitalWrite(ledPin, HIGH);
  delay(250);
  digitalWrite(ledPin, LOW);
  delay(250);
  Serial.println("Calibrate Thumb finger");
  delay(250);
  digitalWrite(ledPin, HIGH);
  delay(250);
  digitalWrite(ledPin, LOW);
  delay(250);
  digitalWrite(ledPin, HIGH);
  delay(250);
  
    b = 0;
    while (millis() < 15000) {  //Calibrate Thumb
    timeStamp = micros();
    int Value = analogRead(SensorPin);

    // filter processing
    int DataAfterFilter = myFilter.update(Value);

    int envlope = (sq(DataAfterFilter));
    
    x = envlope;
    calT = calT + x;
    b = b + 1;  

 }
  calT = calT / b; //Average Thumb Readings
  Serial.println(calT); //Show calibration Reading

}



void loop() {
    timeStamp = micros();
      
    int Value = analogRead(SensorPin);

    // filter processing
    int DataAfterFilter = myFilter.update(Value);

    int envlope = (sq(DataAfterFilter));
                       
       if (calB > envlope * calModMinus && calB < envlope * calMod){ //Baby finger
        
       servo1(); //Goto function servo1
       Serial.println("Small");
       Serial.println(envlope);
       delay(100);
       
      }
       
       if (calR > envlope * calModMinus && calR < envlope * calMod){ //Ring Finger
       servo2(); //Goto function servo2
       Serial.println("Ring");
       Serial.println(envlope);
       delay(100);
       
      }

      
       if (calM > envlope * calModMinus && calM < envlope * calMod){ //Middle Finger
       servo3(); //Goto function servo3
       Serial.println("Middle");
       Serial.println(envlope);
       delay(100);
       
      }
       
       if (calI > envlope * calModMinus && calI < envlope * calMod){ //Index Finger
       servo4(); //Goto function servo4
       Serial.println("Index");
       Serial.println(envlope);
       delay(100);
       
      }

       if (calT > envlope * calModMinus && calT < envlope * calMod){ //Thumb
       servo5(); //Goto function servo5
       Serial.println("Thumb");
       Serial.println(envlope);
       delay(100);
       
      }
     
     delay(100);

  }


void servo1(){
  myservo1.attach(2); //Power to servo
  delay(15); //Short delay
  myservo1.write(180); //Turn servo 180°
  delay(1000); //Delay 1 second
  myservo1.write(0); //Return servo to 0°
  delay(1000); //Delay 1 second
  myservo1.detach(); //Power off to servo
}

void servo2(){
  myservo2.attach(3);
  delay(15);
  myservo2.write(180);
  delay(1000);
  myservo2.write(0);
  delay(1000);
  myservo2.detach();
}

void servo3(){
  myservo3.attach(4);
  delay(15);
  myservo3.write(180);
  delay(1000);
  myservo3.write(0);
  delay(1000);
  myservo3.detach();
}

void servo4(){
  myservo4.attach(5);
  delay(15);
  myservo4.write(180);
  delay(1000);
  myservo4.write(0);
  delay(1000);
  myservo4.detach();
}

void servo5(){
  myservo5.attach(6);
  delay(15);
  myservo5.write(180);
  delay(1000);
  myservo5.write(0);
  delay(1000);
  myservo5.detach();
}

Thank you for following!!

Dale Winhold