EMG-sensor Dc motor encoder. I'M TRYING TO CONTROL A DC MOTOR WITH ENCODER

Gravity: Analog EMG Sensor by OYMotion

SKU:SEN0240 Brand:DFRobot.

HEY. I WOULD LIKE HELP. I'M TRYING TO CONTROL A DC MOTOR WITH ENCODER. COULD SOMEONE HELP. PLEASE

#if defined(ARDUINO) && ARDUINO >= 100

#include "Arduino.h"

#else

#include "WProgram.h"

#endif

#include "EMGFilters.h"

#define SensorInputPin A1   //sensor input pin number   SensorInputPin

//int sensorPin1 = A1

unsigned long threshold = 600;  // threshold: Relaxed baseline values.(threshold=0:in the calibration process)

unsigned long EMG_num = 0;      // EMG_num: The number of statistical signals

bool bandera=false;

EMGFilters myFilter;

/*

  Set the input frequency.

  The filters work only with fixed sample frequency of

  `SAMPLE_FREQ_500HZ` or `SAMPLE_FREQ_1000HZ`.

  Inputs at other sample rates will bypass

*/

SAMPLE_FREQUENCY sampleRate = SAMPLE_FREQ_500HZ;

/*

  Set the frequency of power line hum to filter out.

  For countries with 60Hz power line, change to "NOTCH_FREQ_60HZ"

*/

NOTCH_FREQUENCY humFreq = NOTCH_FREQ_50HZ;

int encoder0PinA = 6;  //0

int encoder0PinB = 7;  //1

int encoder0Pos = 0;

int encoder0PinALast = LOW;

int n0 = LOW;

long pos0 = 0;

int encoder1PinA = 6;

int encoder1PinB = 7;

int encoder1Pos = 0;

int encoder1PinALast = LOW;

int n1 = LOW;

long pos1 = 0;

int encoderPinALastR = 0;

int encoderPosR = 0;

unsigned long serialData;

int inbyte;

int digitalState;

int pinNumber;

int analogRate;

int mot5p = 9;       // Motor 5

int mot5m = 8;

int mot6p = 3;       // Motor 6

int mot6m = 2;

int sensorValue1 = 0;  // variable to store the value coming from the sensor

int sensorValue2 = 0;

int sensorValue3 = 0;

int rotateVal4;

int rotateVal5;

void setup(){

  myFilter.init(sampleRate, humFreq, true, true, true);

  Serial.begin(115200);

  pinMode(encoder0PinA, INPUT);

  pinMode(encoder0PinB, INPUT);

  pinMode(encoder1PinA, INPUT);

  pinMode(encoder1PinB, INPUT);

}

void loop(){

  int data = analogRead(SensorInputPin);

  int dataAfterFilter = myFilter.update(data);  // filter processing

  int envelope = sq(dataAfterFilter);   //Get envelope by squaring the input

  envelope = (envelope > threshold) ? envelope : 0;    // The data set below the base value is set to 0, indicating that it is in a relaxed state

  /* if threshold=0,explain the status it is in the calibration process,the code bollow not run.

     if get EMG singal,number++ and print

  */

  if (threshold > 0)

  {

      {

        serialRead();

        rotateVal4 = serialData;

        if (pos0>rotateVal4){

          Stop(mot6p, mot6m);

          }

        else{

          forward(mot5p, mot5m);

          Read(n0, encoder0PinA, encoder0PinB, encoder0Pos);

        }

      }

      {

        serialRead();

        rotateVal5 = serialData;

        if (pos0>rotateVal5){

          Stop(mot5p, mot5m);

          }

        else{

          forward(mot6p, mot6m);

          Read(n1, encoder1PinA, encoder1PinB, encoder1Pos);

        }

      }

   }    

    Serial.print (encoderPosR);

    Serial.print ("\n");

}

long getSerial()

{

  serialData = 0;

  while (inbyte != '/')

  {

    inbyte = Serial.read();

    if (inbyte > 0 && inbyte != '/')

    {

      serialData = serialData * 10 + inbyte - '0';  

      Serial.println(serialData);

    }

  }

  inbyte = 0;

  return serialData;

}

long serialRead()

{

  serialData = 0;

  while (inbyte != '/')

  {

    inbyte = Serial.read();

    if (inbyte > 0 && inbyte != '/')

    {

      serialData = serialData * 10 + inbyte - '0';  

      Serial.println(serialData);

    }

  }

  inbyte = 0;

  return serialData;

}

void forward(int x, int y) {

  digitalWrite(x, LOW);

  //digitalWrite(AIN_2, HIGH);

  analogWrite(y, 93);

}

void reverse(int x, int y) {

  //digitalWrite(AIN_1, HIGH);

  analogWrite(x, 93);

  digitalWrite(y, LOW);

}

void Stop(int x, int y) {

  digitalWrite(x, LOW);

  digitalWrite(y, LOW);

}

void Read(int n, int encoderPinA, int encoderPinB, int encoderPos){

 // encoderPosR = encoderPos;

  n = digitalRead(encoderPinA);

  if ((encoderPinALastR == LOW) && (n == HIGH)) {

    if (digitalRead(encoderPinB) == LOW) {

      encoderPosR--;

    } else {

      encoderPosR++;

    }

    pos0 = encoderPosR;

    Serial.print (encoderPosR);

    Serial.print ("\n");

    encoderPos = encoderPosR;

  }

  encoderPinALastR = n;

  }

   /*if get EMG signal,return 1;

*/

int getEMGCount(int gforce_envelope)

{

  static long integralData = 0;

  static long integralDataEve = 0;

  static bool remainFlag = false;

  static unsigned long timeMillis = 0;

  static unsigned long timeBeginzero = 0;

  static long fistNum = 0;

  static int  TimeStandard = 200;

  /*

    The integral is processed to continuously add the signal value

    and compare the integral value of the previous sampling to determine whether the signal is continuous

   */

  integralDataEve = integralData;

  integralData += gforce_envelope;

  /*

    If the integral is constant, and it doesn't equal 0, then the time is recorded;

    If the value of the integral starts to change again, the remainflag is true, and the time record will be re-entered next time

  */

  if ((integralDataEve == integralData) && (integralDataEve != 0))

  {

    timeMillis = millis();

    if (remainFlag)

    {

      timeBeginzero = timeMillis;

      remainFlag = false;

      return 0;

    }

    /* If the integral value exceeds 200 ms, the integral value is clear 0,return that get EMG signal */

    if ((timeMillis - timeBeginzero) > TimeStandard)

    {

      integralDataEve = integralData = 0;

      return 1;

    }

    return 0;

  }

  else {

    remainFlag = true;

    return 0;

   }

}