Arduino Code

#include <AFMotor.h>

AF_Stepper main_motor(200, 1);
AF_Stepper screw_motor(200, 2);

#define maxLength 30
//#define ENC_A 14
//#define ENC_B 15
//#define ENC_PORT PINC
#define encoder0PinA 2

#define encoder0PinB 9
#define ENC_A 2
#define ENC_B 13
#define ENC_PORT PINC

volatile unsigned int encoder0Pos = 100;


int ownID = 0; // Adjust according to module ID
boolean answer = false;

//int switchPin = 2;
int backSwitchPin = 2;
const int pingPin = 10;
int counter=1000;

String command = String(maxLength);
String echoCommand = String(maxLength);

boolean commandComplete = false;
int recID;

long stepsFromHome=0;


void setup()
{

//pinMode(switchPin, INPUT);
pinMode(encoder0PinA, INPUT);
digitalWrite(encoder0PinA, HIGH); // turn on pullup resistor
pinMode(encoder0PinB, INPUT);
digitalWrite(encoder0PinB, HIGH); // turn on pullup resistor

attachInterrupt(0, doEncoder, CHANGE);

Serial.begin(9600);
main_motor.setSpeed(10); // 100 rpm
screw_motor.setSpeed(50); // 100 rpm
command = "";
}

void loop()
{
if (commandComplete)
{
command=command+";";
if (command.equals("SINGLEROTATION;")) single_rotation(0);
if (command.equals("SENSORRIGHT;")) sensor_right();
if (command.equals("SENSORLEFT;")) sensor_left();
if (command.equals("3DSECTION;")) _3D_section(10);
if (command.equals("LINEAR3DSECTION;")) linear_3D_section();
if (command.equals("SHOWSENSORPOS;")) show_sensor_pos();
if (command.equals("SETSENSORPOS;")) set_sensor_pos();
if (command.substring(0,4).equals("SCAN")) scan(command);

command="";
commandComplete=false;
}
// else { Serial.println("no command"); }
// command="";
delay(20);
if(Serial.available() > 0) {
getIncomingChars();


}

}

void getIncomingChars()
{
char inChar = Serial.read();
int max=50;
int counter=0;

if(inChar == 59 || inChar == 10 || inChar == 13)
{
commandComplete = true;
}
else
{
command=command+inChar;
}



}

void _3D_section(int linearDistance)
{
// main_motor.setSpeed(10); // 100 rpm
// screw_motor.setSpeed(100); // 100 rpm
int k=0;
for(int j=0; j<linearDistance; j++)
{
for(int i=0; i<601; i++)
{
main_motor.step(1, FORWARD, DOUBLE);
k=j*1;
Serial.print(k); //ring
Serial.print(", ");
Serial.print(i); // degree
Serial.print(", ");
Serial.print(encoder0Pos);
Serial.print(";");
}
main_motor.release();
sensor_right_mm(1); // 2mm
}

}

void scan(String command)
{

String runSize = command.substring(4, command.indexOf(";"));

char runSize_as_char[]="277";
// runSize_as_char[0]='2';
// runSize_as_char[1]='7';
// runSize_as_char[2]='7';
// runSize_as_char[3]='�';
// runSize.toCharArray(runSize_as_char, runSize.length());
int i = int(runSize_as_char);

Serial.print("Run Size: ");
Serial.print(i);
Serial.print(";");
_3D_section(290);
}

void linear_3D_section()
{
main_motor.setSpeed(100); // 100 rpm
screw_motor.setSpeed(100); // 100 rpm
int direction=FORWARD;
for(int j=0; j<10; j++)
{
for(int i=0; i<601; i++)
{
screw_motor.step(20, direction, DOUBLE);
Serial.print(i); //ring
Serial.print(", ");
Serial.print(j); // degree
Serial.print(", ");
// Serial.print(getDistance());
Serial.print(";");
}
if (direction==FORWARD) direction=BACKWARD;
else direction=FORWARD;
main_axis_rotate(2);

}
main_motor.release();
screw_motor.release();

}


void single_rotation(int ring)
{
// main_motor.setSpeed(10); // 100 rpm
//screw_motor.setSpeed(100); // 100 rpm
//Serial.print("Stepping Forward 100;");
int8_t tmpdata;
String data="";

for(int i=0; i<601; i++)
{

// SINGLE single coil
// DOUBLE double coil (more torque)
// INTERLEAVE alternate single and double for 2x resolution (1/2 speed)
// MICROSTEP smooth
main_motor.step(1, BACKWARD, DOUBLE);

Serial.print(ring);
Serial.print(", ");
Serial.print(i);
Serial.print(", ");
Serial.print(encoder0Pos);
Serial.print(";");

}

main_motor.release();


}

void show_sensor_pos()
{
Serial.print("Sensor pos: ");
Serial.print(encoder0Pos);
Serial.print(";");

}

void set_sensor_pos()
{
encoder0Pos=152;
Serial.print("Sensor pos set to: ");
Serial.print(encoder0Pos);
Serial.print(";");

}

void sensor_right()
{
//main_motor.setSpeed(10); // 100 rpm
// screw_motor.setSpeed(50); // 100 rpm
/*
Serial.print("Encoder start pos: ");
Serial.print(encoder0Pos);
Serial.print(";");
for(int i=1; i<500; i++)
{
screw_motor.step(3, BACKWARD, DOUBLE);
}
Serial.print("Encoder end pos: ");
Serial.print(encoder0Pos);
Serial.print(";");
screw_motor.release();
*/
sensor_right_mm(30);
}

void sensor_right_mm(int numberMm)
{
int dist=numberMm*48;
for(int i=1; i<dist; i++)
{
screw_motor.step(1, BACKWARD, DOUBLE);
}
screw_motor.release();
}

void sensor_left_mm(int numberMm)
{

int dist=numberMm*48;
for(int i=1; i<dist; i++)
{
screw_motor.step(1, FORWARD, DOUBLE);
}
screw_motor.release();
}

void main_axis_rotate(int number)
{

for(int i=0; i<number; i++)
{
main_motor.step(1, BACKWARD, DOUBLE);
}
main_motor.release();
}

void sensor_left()
{
/*
//main_motor.setSpeed(10); // 100 rpm
//screw_motor.setSpeed(50); // 100 rpm
Serial.print("Encoder start pos: ");
Serial.print(encoder0Pos);
Serial.print(";");
for(int i=1; i<1410; i++)
{
screw_motor.step(3, FORWARD, DOUBLE);
}
Serial.print("Encoder end pos: ");
Serial.print(encoder0Pos);
Serial.print(";");
screw_motor.release();
*/
sensor_left_mm(30);
}


void doEncoder() {
/* If pinA and pinB are both high or both low, it is spinning
* forward. If they're different, it's going backward.
*
* For more information on speeding up this process, see
* [Reference/PortManipulation], specifically the PIND register.
*/

if (digitalRead(encoder0PinA) == digitalRead(encoder0PinB)) {
encoder0Pos--;
} else {
encoder0Pos++;
}

//Serial.print (encoder0Pos, DEC);
//Serial.print (";");
}