So this is what im working with right now:
int E1 = 5; //Right Motor Power
int E2 = 6; //Left Motor Power
int M1 = 4; //Right Motor Direction
int M2 = 7; //Left Motor Direction
const byte encoder0pinA = 2; //Interrupt 0
const byte encoder0pinB = 12;
const byte encoder1pinA = 3; //Interrupt 1
const byte encoder1pinB = 13;
byte encoder0PinALast;
byte encoder1PinALast;
int duration0;//Encoder0 number of pulses
int duration1;//Encoder1 number of pulses
boolean Direction0;
boolean Direction1;
void setup()
{
Serial.begin(9600);//Initialize the serial port
EncoderInit();//Initialize the module
}
void loop()
{
Serial.print("Left Pulse:");
Serial.println(duration0);
Serial.print("Right Pulse:");
Serial.println(duration1);
duration0 = 0;
duration1 = 0;
analogWrite (E1,100);
digitalWrite(M1,LOW);
analogWrite (E2,107.9);
digitalWrite(M2,LOW);
delay(500);
}
void EncoderInit()
{
Direction0 = true;
Direction1 = true;
pinMode(encoder0pinB,INPUT);
attachInterrupt(0, wheelSpeed0, CHANGE);
pinMode(encoder1pinB,INPUT);
attachInterrupt(1, wheelSpeed1, CHANGE);
}
void wheelSpeed0()
{
//Encoder 0 Code
int Lstate = digitalRead(encoder0pinA);
if((encoder0PinALast == LOW) && Lstate==HIGH)
{
int val0 = digitalRead(encoder0pinB);
if(val0 == LOW && Direction0)
{
Direction0 = false; //Reverse
}
else if(val0 == HIGH && !Direction0)
{
Direction0 = true; //Forward
}
}
encoder0PinALast = Lstate;
if(!Direction0) duration0++;
else duration0--;
}
void wheelSpeed1()
{
//Encoder 1 Code
int Lstate1 = digitalRead(encoder1pinA);
if((encoder1PinALast == LOW) && Lstate1==HIGH)
{
int val1 = digitalRead(encoder1pinB);
if(val1 == LOW && Direction1)
{
Direction1 = false; //Reverse
}
else if(val1 == HIGH && !Direction1)
{
Direction1 = true; //Forward
}
}
encoder1PinALast = Lstate1;
if(!Direction1) duration1++;
else duration1--;
}
This is the serial output:
Left Pulse:1
Right Pulse:-1
Left Pulse:780
Right Pulse:708
Left Pulse:1437
Right Pulse:1321
Left Pulse:1618
Right Pulse:1491
Left Pulse:1675
Right Pulse:1549
Left Pulse:1697
Right Pulse:1571
Left Pulse:1704
Right Pulse:1579
Left Pulse:1707
Right Pulse:1589
Left Pulse:1709
Right Pulse:1589
Left Pulse:1712
Right Pulse:1591
Left Pulse:1714
Right Pulse:1594
Left Pulse:1716
Right Pulse:1591
Left Pulse:1717
Right Pulse:1592
Left Pulse:1719
Right Pulse:1593
Left Pulse:1722
Right Pulse:1592
Left Pulse:1723
Right Pulse:1597
Left Pulse:1722
Right Pulse:1597
Left Pulse:1724
Right Pulse:1600
Left Pulse:1725
Right Pulse:1604
At this speed it takes the Right wheel about 1.2s so right wheel average of 1596/1200 milliseconds = 1.33 more or less the speed that i want..