Test motor dc encoder 25GA370

Test using arduino mega 2560 and adafruit v1 motor shield this is my code

#include <AFMotor.h>

AF_DCMotor motor(1);
AF_DCMotor motor2(3);
volatile long int encoder_pos = 0;
volatile long int encoder_pos2 = 0;
//The sample code for driving one way motor encoder
const byte encoder0pinA = 18;//A pin -> the interrupt pin 0
const byte encoder0pinB = 31;//B pin -> the digital pin 3


const byte encoder1pinA = 19;//A pin -> the interrupt pin 0
const byte encoder1pinB = 33;//B pin -> the digital pin 3


byte encoder0PinALast;
byte encoder1PinALast;
int duration;//the number of the pulses
boolean Direction;//the rotation direction


void setup()
{
  Serial.begin(115200);//Initialize the serial port
//  EncoderInit();//Initialize the module

  attachInterrupt(digitalPinToInterrupt(encoder0pinA), encoder, RISING);

  attachInterrupt(digitalPinToInterrupt(encoder1pinA), encoder2, RISING);

}

void encoder(){

  if(digitalRead(encoder0pinB) == HIGH){
    encoder_pos++;
  }else{
    encoder_pos--;
  }
}

void encoder2(){

  if(digitalRead(encoder1pinB) == HIGH){
    encoder_pos2++;
  }else{
    encoder_pos2--;
  }
}

void loop()
{
//  motor.run(BACKWARD);
//  motor.setSpeed(40);
////  delay(200);
//
////  motor.run(BACKWARD);
////  motor.setSpeed(150);
////  delay(200);
//
//  motor2.run(FORWARD);
//  motor2.setSpeed(50);
//  delay(200);

//  motor2.run(FORWARD);
//  motor2.setSpeed(150);
//  delay(200);
  
  
  Serial.print("Pulse:");
  Serial.println(duration);
  Serial.print("Encoder pos1:");
  Serial.println(encoder_pos);
  Serial.print("Encoder pos2:");
   Serial.println(encoder_pos2);
  duration = 0;
  delay(1000);

  
  
}

void EncoderInit()
{
  Direction = true;//default -> Forward
  pinMode(encoder0pinB,INPUT);
  attachInterrupt(0, wheelSpeed, CHANGE);
}

void wheelSpeed()
{
  int Lstate = digitalRead(encoder0pinA);
  if((encoder0PinALast == LOW) && Lstate==HIGH)
  {
    int val = digitalRead(encoder0pinB);
    if(val == LOW && Direction)
    {
      Direction = false; //Reverse
    }
    else if(val == HIGH && !Direction)
    {
      Direction = true;  //Forward
    }
  }
  encoder0PinALast = Lstate;

  if(!Direction)  duration++;
  else  duration--;
}

Advertisement