#define encoder0PinA 2 #define encoder0PinB 4 #define encoder1PinA 3 #define encoder1PinB 5 volatile int encoder0Pos = 0; volatile int encoder1Pos = 0; int WR=100; // angular velocity of right wheel int WL=100; // angular velocity of right wheel long newposition; long oldposition = 0; unsigned long newtime; unsigned long oldtime = 0; long vel; long newposition1; long oldposition1 = 0; unsigned long newtime1; unsigned long oldtime1 = 0; long vel1; int ENA=8; // SpeedPinA connected to Arduino's port 8 int ENB=9; // SpeedPinB connected to Arduino's port 9 int IN1=48; // RightMotorWire1 connected to Arduino's port 48 int IN2=49; // RightMotorWire2 connected to Arduino's port 49 int IN3=50; // RightMotorWire1 connected to Arduino's port 48 int IN4=51; // RightMotorWire2 connected to Arduino's port 49 void setup() { pinMode(ENA,OUTPUT); pinMode(ENB,OUTPUT); pinMode(IN1,OUTPUT); pinMode(IN2,OUTPUT); pinMode(IN3,OUTPUT); pinMode(IN4,OUTPUT); digitalWrite(ENA,HIGH); //enable motorA digitalWrite(ENB,HIGH); //enable motorB pinMode(encoder0PinA, INPUT); pinMode(encoder0PinB, INPUT); pinMode(encoder1PinA, INPUT); pinMode(encoder1PinB, INPUT); // encoder pin on interrupt 0 (pin 2) attachInterrupt(0, doEncoderA, CHANGE); // encoder pin on interrupt 1 (pin 3) attachInterrupt(1, doEncoderB, CHANGE); Serial.begin (9600); } void loop(){ int rightPWM; if (WR > 0) { //forward digitalWrite(IN1,LOW); digitalWrite(IN2,HIGH); } else if (WR < 0){ //reverse digitalWrite(IN1,HIGH); digitalWrite(IN2,LOW); } if (WR == 0) { rightPWM = 0; analogWrite(ENA, rightPWM); } else { rightPWM = map(abs(WR), 1, 100, 1, 255); analogWrite(ENA, rightPWM); } int leftPWM; if (WL > 0) { //forward digitalWrite(IN3,LOW); digitalWrite(IN4,HIGH); } else if (WL < 0) { //reverse digitalWrite(IN3,HIGH); digitalWrite(IN4,LOW);} if (WL == 0) { leftPWM = 0; analogWrite(ENB, leftPWM); } else { leftPWM = map(abs(WL), 1, 100, 1, 255); analogWrite(ENB, leftPWM); } // to determine the speed of motors by encoders newposition = encoder0Pos; newtime = millis(); vel = (newposition-oldposition) * 1000 /(long)(newtime-oldtime); oldposition = newposition; oldtime = newtime; newposition1 = encoder1Pos; newtime1 = millis(); vel1 = (newposition1-oldposition1) * 1000 /(long)(newtime1-oldtime1); oldposition1 = newposition1; oldtime1 = newtime1; Serial.print (vel); Serial.print ("\t"); Serial.print (vel1); Serial.print ("\t"); Serial.print (encoder0Pos*-1); Serial.print("\t"); Serial.print (encoder1Pos*-1); Serial.print("\t"); Serial.println ((encoder0Pos*-1) -( encoder1Pos*-1)); } // 1 encoder counts void doEncoderA(){ // look for a low-to-high on channel A if (digitalRead(encoder0PinA) == HIGH) { // check channel B to see which way encoder is turning if (digitalRead(encoder0PinB) == LOW) { encoder0Pos = encoder0Pos + 1; // CW } else { encoder0Pos = encoder0Pos - 1; // CCW } } else // must be a high-to-low edge on channel A { // check channel B to see which way encoder is turning if (digitalRead(encoder0PinB) == HIGH) { encoder0Pos = encoder0Pos + 1; // CW } else { encoder0Pos = encoder0Pos - 1; // CCW } } } // 2 encoder counts void doEncoderB(){ // look for a low-to-high on channel B if (digitalRead(encoder1PinB) == HIGH) { // check channel A to see which way encoder is turning if (digitalRead(encoder1PinA) == HIGH) { encoder1Pos = encoder1Pos + 1; // CW } else { encoder1Pos = encoder1Pos - 1; // CCW } } // Look for a high-to-low on channel B else { // check channel B to see which way encoder is turning if (digitalRead(encoder1PinA) == LOW) { encoder1Pos = encoder1Pos + 1; // CW } else { encoder1Pos = encoder1Pos - 1; // CCW } } }