// including libraries for pinchange interrupt
#include <PinChangeInt.h>
#include <PinChangeIntConfig.h>
//assigning pins for encoders
define encoder1PinA 5
define encoder1PinB 6
define encoder2PinA 9
define encoder2PinB 10
// X outputs
define out1PinA 4
define out1PinB 3
// Y outputs
define out2PinA 11
define out2PinB 12
//VARIABLES*****************
volatile long encoder1Pos = 0;//raw encoder count for encoder 1
volatile long encoder2Pos = 0;//raw encoder count for encoder 2
volatile float Apos = 0;//encoder 1 reading in thou
volatile float Bpos = 0;//encoder 2 reading in thou
volatile float Cpos = 14000;//distance between encoders 1&2 in thou
volatile long x = 0;//X position in thou
volatile long y = 0;//Y position in thou
volatile long XposOld = 0;//X position in thou
volatile long YposOld = 0;//Y position in thou
//previous pin positions
volatile long encoder1PinALast = LOW;
volatile long encoder1PinBLast = LOW;
volatile long encoder2PinALast = LOW;
volatile long encoder2PinBLast = LOW;
//temp data holders
volatile long n1 = LOW;
volatile long m1 = LOW;
volatile long n2 = LOW;
volatile long m2 = LOW;
long valNew1 = 0;
long valOld1 = 0;
long valNew2 = 0;
long valOld2 = 0;
long outcount=1;
long outcountY=1;
volatile int Xsmall=0;
volatile int Ysmall=0;
//************************** SET UP **********************************
void setup()
{
pinMode (encoder1PinA,INPUT);
pinMode (encoder1PinB,INPUT);
pinMode (encoder2PinA,INPUT);
pinMode (encoder2PinB,INPUT);
pinMode (out2PinA,OUTPUT);
pinMode (out2PinB,OUTPUT);
pinMode(out1PinA, OUTPUT);
pinMode(out1PinB, OUTPUT);
Serial.begin (9600);
PCintPort::attachInterrupt(encoder1PinA, changeA1,CHANGE);
PCintPort::attachInterrupt(encoder1PinB, changeB1,CHANGE);
PCintPort::attachInterrupt(encoder2PinA, changeA2,CHANGE);
PCintPort::attachInterrupt(encoder2PinB, changeB2,CHANGE);
}
//************************** LOOP **********************************
void loop()
{
encoder1PinALast = n1;
encoder2PinALast = n2;
valNew1 = encoder1Pos;
valNew2 = encoder2Pos;
XposOld = Xsmall;
YposOld = Ysmall;
Apos=(encoder1Pos*-0.984)+4600+2365;//compensating for intial postion of the probe and the extension of the wire cushions
Bpos=(encoder2Pos*1.4925)+6772+475;//compensating for intial postion of the probe and the extension of the wire cushions
Cpos=14000;
if (valNew1 != valOld1 || valNew2 != valOld2)
{
x=( ((sq(Bpos)) + (sq(Cpos)) - (sq(Apos)) ) /(2*Cpos)); //a=Bpos b=Apos
y= sqrt(abs((sq(Bpos))-(sq(x))));
Xsmall=x/10;
Ysmall=y/10;
valOld1 = valNew1;
valOld2 = valNew2;
if (XposOld != Xsmall)
{
switch(outcount)
{
case 1:
{
if (XposOld > Xsmall)
{
digitalWrite(out1PinA,HIGH);
digitalWrite(out1PinB,LOW);
}
else
{
digitalWrite(out1PinA,HIGH);
digitalWrite(out1PinB,LOW);
}
outcount=2;
break;
}
case 2:
{
if (XposOld > Xsmall)
{
digitalWrite(out1PinA,HIGH);
digitalWrite(out1PinB,HIGH);
}
else
{
digitalWrite(out1PinA,LOW);
digitalWrite(out1PinB,LOW);
}
outcount=3;
break;
}
case 3:
{
if (XposOld > Xsmall)
{
digitalWrite(out1PinA,LOW);
digitalWrite(out1PinB,HIGH);
}
else
{
digitalWrite(out1PinA,LOW);
digitalWrite(out1PinB,HIGH);
}
outcount=4;
break;
}
default:
{
if (XposOld > Xsmall)
{
digitalWrite(out1PinA,LOW);
digitalWrite(out1PinB,LOW);
}
else
{
digitalWrite(out1PinA,HIGH);
digitalWrite(out1PinB,HIGH);
}
outcount=1;
break;
}
}
}
if (YposOld != Ysmall)
{
switch(outcountY)
{
case 1:
{
if (YposOld > Ysmall)
{
digitalWrite(out2PinA,HIGH);
digitalWrite(out2PinB,LOW);
}
else
{
digitalWrite(out2PinA,HIGH);
digitalWrite(out2PinB,LOW);
}
outcountY=2;
break;
}
case 2:
{
if (YposOld > Ysmall)
{
digitalWrite(out2PinA,HIGH);
digitalWrite(out2PinB,HIGH);
}
else
{
digitalWrite(out2PinA,LOW);
digitalWrite(out2PinB,LOW);
}
outcountY=3;
break;
}
case 3:
{
if (XposOld > Xsmall)
{
digitalWrite(out2PinA,LOW);
digitalWrite(out2PinB,HIGH);
}
else
{
digitalWrite(out2PinA,LOW);
digitalWrite(out2PinB,HIGH);
}
outcountY=4;
break;
}
default:
{
if (XposOld > Xsmall)
{
digitalWrite(out2PinA,LOW);
digitalWrite(out2PinB,LOW);
}
else
{
digitalWrite(out2PinA,HIGH);
digitalWrite(out2PinB,HIGH);
}
outcountY=1;
break;
}//default
}//switch case
}// if YoldPos!= Ysmall
}//if (valNew1 != valOld1 || valNew2 != valOld2)
}//forever loop
//************************** ENCODER 1 **********************************
void changeA1()
{
n1 = digitalRead(encoder1PinA);
m1 = digitalRead(encoder1PinB);
if (encoder1PinBLast ^ n1 ==1)
{
encoder1Pos--;
//encoder1Pos++;
}
else
{
encoder1Pos++;
}
encoder1PinBLast = m1;
}
void changeB1()
{
m1 = digitalRead(encoder1PinB);
n1 = digitalRead(encoder1PinA);
if (encoder1PinBLast ^ n1 ==1)
{
encoder1Pos--;
//encoder1Pos++;
}
else
{
encoder1Pos++;
}
encoder1PinBLast = m1;
}
//************************** ENCODER 2 **********************************
void changeA2()
{
n2 = digitalRead(encoder2PinA);
m2 = digitalRead(encoder2PinB);
if (encoder2PinBLast ^ n2 ==1)
{
//encoder2Pos++;
encoder2Pos--;
}
else
{
encoder2Pos++;
}
encoder2PinBLast = m2;
}
void changeB2()
{
m2 = digitalRead(encoder2PinB);
n2 = digitalRead(encoder2PinA);
if (encoder2PinBLast ^ n2 ==1)
{
//encoder2Pos++;
encoder2Pos--;
}
else
{
encoder2Pos++;
}
encoder2PinBLast = m2;
}