#include <Servo.h>
Servo myservoR;
Servo myservoL;
const int ledPin = 13;
int Adir = A0;
int Bdir = A2;
int Cdir = A1;
int Ddir = A5;
int Edir = A4;
int Fdir = A3;
int Apulse = 10;
int Bpulse = 11;
int Cpulse = 12;
int Dpulse = 3;
int Epulse = 4;
int Fpulse = 5;
//stepper intagers
int acel = 850;
int dcel = 2800;
String stringOne = "";
int incomingByte;
int Acmd = 0;
float Apos = 0;
float Anewpos = 0;
float Alastpos = 0;
int Asteps = 0;
int Aspd = 1500;
int Aacel = 1000;
int Adecl = 1000;
int Bcmd = 0;
float Bpos = 0;
float Bnewpos = 0;
float Blastpos = 0;
int Bsteps = 0;
int Bspd = 1000;
int Bacel = 1000;
int Bdecl = 1000;
int Ccmd = 0;
float Cpos = 0;
float Cnewpos = 0;
float Clastpos = 0;
int Csteps = 0;
int Cspd = 1000;
int Cacel = 1000;
int Cdecl = 1000;
int Dcmd = 0;
float Dpos = 0;
float Dnewpos = 0;
float Dlastpos = 0;
int Dsteps = 0;
int Dspd = 1000;
int Dacel = 1000;
int Ddecl = 1000;
int Ecmd = 0;
float Epos = 0;
float Enewpos = 0;
float Elastpos = 0;
int Esteps = 0;
int Espd = 1000;
int Eacel = 1000;
int Edecl = 1000;
int Fcmd = 0;
float Fpos = 0;
float Fnewpos = 0;
float Flastpos = 0;
int Fsteps = 0;
int Fspd = 1000;
int Facel = 1000;
int Fdecl = 1000;
int Gcmd = 0;
int Rval = 0;
int Lval = 0;
int tempAcmd = 0;
int n = LOW;
int ledPinST = 0;
// communacations value stores
byte inByte = 0;// incoming serial byte
String inString = "";
float z1 = 9.999;
float s = 0;
float gotoByte = 0;
float gotoabs = 0;
long tempsteps = 1200;
long resteps = 0;
long steps = 100;
long SPD = 950;
long tempSPD = 700;
long aspeed = 820;
long dspeed = 900;
long z2 = 0;
byte estop;
int checkindex = 0;
int count = 100;
void setup() {
myservoR.attach(9);
myservoL.attach(6);
Serial.begin (9600);
checkindex = 1;
pinMode(ledPin, OUTPUT);
pinMode (Adir, OUTPUT);
pinMode (Bdir, OUTPUT);
pinMode (Cdir, OUTPUT);
pinMode (Ddir, OUTPUT);
pinMode (Edir, OUTPUT);
pinMode (Fdir, OUTPUT);
pinMode (12, OUTPUT);
pinMode (11, OUTPUT);
pinMode (10, OUTPUT);
pinMode (5, OUTPUT);
pinMode (4, OUTPUT);
pinMode (3, OUTPUT);
Anewpos = 180;
Alastpos = 180;
Bnewpos = 180;
Blastpos = 180;
Cnewpos = 180;
Clastpos = 180;
Dnewpos = 180;
Dlastpos = 180;
Enewpos = 180;
Elastpos = 180;
Fnewpos = 180;
Flastpos = 180;
Gcmd = 120;
Serial.println(Gcmd);
myservoR.write(107);
delay(300);
myservoL.write(128);
delay(300);
Serial.println("x");
delay(300);
}
void loop() {
if (count < 1){
Serial.println("x");
count = 100;
}
count--;
delay(1);
// ...........................................
if (Serial.available()) {
byte inByte = Serial.read();
switch (inByte) {
case 'A':// SETTING ACELLRATION FROM REMOTE SERIAL
Acmd = Serial.parseInt();
if (Acmd > Alastpos) {
//Serial.println(Acmd);
// Serial.println(Alastpos);
int x = (Acmd - Alastpos);
// Serial.println(x);
Alastpos = Acmd;
x = map(x, 0, 360, 0, 5200); // switch to deg
// Serial.println(x);
Apos = x;
Aposmov();
break;
}
if (Acmd < Alastpos) {
// Serial.println(Acmd);
// Serial.println(Alastpos);
int x = (Acmd - Alastpos);
// Serial.println(x);
Alastpos = Acmd;
x = map(x, 0, 360, 0, 5200); // switch to deg
// Serial.println(x);
Apos = x;
Anegmov();
}
break;
case 'B':// SETTING ACELLRATION FROM REMOTE SERIAL
Bcmd = Serial.parseInt();
if (Bcmd > Blastpos) {
// Serial.println(Bcmd);
// Serial.println(Blastpos);
int x = (Bcmd - Blastpos);
// Serial.println(x);
Blastpos = Bcmd;
x = map(x, 0, 360, 0, 5200); // switch to deg
// Serial.println(x);
Bpos = x;
Bposmov();
break;
}
if (Bcmd < Blastpos) {
// Serial.println(Bcmd);
// Serial.println(Blastpos);
int x = (Bcmd - Blastpos);
// Serial.println(x);
Blastpos = Bcmd;
x = map(x, 0, 360, 0, 5200); // switch to deg
// Serial.println(x);
Bpos = x;
Bnegmov();
}
break;
case 'C':// SETTING ACELLRATION FROM REMOTE SERIAL
Ccmd = Serial.parseInt();
if (Ccmd > Clastpos) {
// Serial.println(Ccmd);
// Serial.println(Clastpos);
int x = (Ccmd - Clastpos);
// Serial.println(x);
Clastpos = Ccmd;
x = map(x, 0, 360, 0, 5200); // switch to deg
// Serial.println(x);
Cpos = x;
Cposmov();
break;
}
if (Ccmd < Clastpos) {
// Serial.println(Ccmd);
// Serial.println(Clastpos);
int x = (Ccmd - Clastpos);
// Serial.println(x);
Clastpos = Ccmd;
x = map(x, 0, 360, 0, 5200); // switch to deg
// Serial.println(x);
Cpos = x;
Cnegmov();
}
break;
case 'D':// SETTING ACELLRATION FROM REMOTE SERIAL
Dcmd = Serial.parseInt();
if (Dcmd > Dlastpos) {
// Serial.println(Dcmd);
// Serial.println(Dlastpos);
int x = (Dcmd - Dlastpos);
// Serial.println(x);
Dlastpos = Dcmd;
x = map(x, 0, 360, 0, 5200); // switch to deg
// Serial.println(x);
Dpos = x;
Dposmov();
break;
}
if (Dcmd < Dlastpos) {
// Serial.println(Dcmd);
// Serial.println(Dlastpos);
int x = (Dcmd - Dlastpos);
// Serial.println(x);
Dlastpos = Dcmd;
x = map(x, 0, 360, 0, 5200); // switch to deg
// Serial.println(x);
Dpos = x;
Dnegmov();
}
break;
case 'E':// SETTING ACELLRATION FROM REMOTE SERIAL
Ecmd = Serial.parseInt();
if (Ecmd > Elastpos) {
// Serial.println(Ecmd);
// Serial.println(Elastpos);
int x = (Ecmd - Elastpos);
// Serial.println(x);
Elastpos = Ecmd;
x = map(x, 0, 360, 0, 5200); // switch to deg
// Serial.println(x);
Epos = x;
Eposmov();
break;
}
if (Ecmd < Elastpos) {
// Serial.println(Ecmd);
// Serial.println(Elastpos);
int x = (Ecmd - Elastpos);
// Serial.println(x);
Elastpos = Ecmd;
x = map(x, 0, 360, 0, 5200); // switch to deg
// Serial.println(x);
Epos = x;
Enegmov();
}
break;
case 'F':// SETTING ACELLRATION FROM REMOTE SERIAL
Fcmd = Serial.parseInt();
if (Fcmd > Flastpos) {
// Serial.println(Fcmd);
// Serial.println(Flastpos);
int x = (Fcmd - Flastpos);
// Serial.println(x);
Flastpos = Fcmd;
x = map(x, 0, 360, 0, 5200); // switch to deg
// Serial.println(x);
Fpos = x;
Fposmov();
break;
}
if (Fcmd < Flastpos) {
// Serial.println(Fcmd);
// Serial.println(Flastpos);
int x = (Fcmd - Flastpos);
// Serial.println(x);
Flastpos = Fcmd;
x = map(x, 0, 360, 0, 5200); // switch to deg
// Serial.println(x);
Fpos = x;
Fnegmov();
}
break;
case 'G':// SETTING ACELLRATION FROM REMOTE SERIAL
Gcmd = Serial.parseInt();
Gcmd = constrain(Gcmd, 0, 110);
// Serial.println(Gcmd);
Rval = map(Gcmd, 0, 100, 102, 132);
Lval = map(Gcmd, 100, 0, 102, 132);
// Serial.println(Rval);
// Serial.println(Lval);
myservoR.write(Rval);
myservoL.write(Lval);
delay(500);
Serial.println("x");
break;
}
}
}
void Aposmov() {
// Serial.println("x");
Acmd = Apos;
Aacel = Aspd + 3000;
int a = 0;
while (Acmd >= 1) {
//============== accelrate plus set the max speed
if (Aacel > 500 && a == 0) {
Aacel = Aacel / 1.004;
}
if (Acmd < 500 && Aacel < 1600) {// never hit max slow down any way
a = 1; // set bit to one to avoid acel conflict
}
if (a == 1 && Acmd < 500 && Aacel < 2999)// when were aproching last few steps
{
Aacel = Aacel * 1.003 ; // decel from short move
}
if (a == 1 || a == 0 && Aacel < 599 && Acmd < 300) {//<<<<< trigger max speed hit not much time to stop
a = 2;
}
if (a == 2 && Acmd < 500 && Aacel < 2999) {
Aacel = Aacel * 1.003 ; // decel from fast long move
}
digitalWrite(Adir, LOW);
digitalWrite(12, HIGH);
delayMicroseconds(Aacel);
digitalWrite(12, LOW);
delayMicroseconds(Aacel);
Acmd--;
}
a = 0;
}
void Anegmov() {
Acmd = Apos * -1;
Aacel = Aspd + 3000;
int a = 0;
while (Acmd >= 1) {
//============== accelrate plus set the max speed
if (Aacel > 500 && a == 0) {
Aacel = Aacel / 1.004;
}
if (Acmd < 500 && Aacel < 1600) {// never hit max slow down any way
a = 1; // set bit to one to avoid acel conflict
}
if (a == 1 && Acmd < 500 && Aacel < 2999)// when were aproching last few steps
{
Aacel = Aacel * 1.003 ; // decel from short move
}
if (a == 1 || a == 0 && Aacel < 599 && Acmd < 300) {//<<<<< trigger max speed hit not much time to stop
a = 2;
}
if (a == 2 && Acmd < 500 && Aacel < 2999) {
Aacel = Aacel * 1.003 ; // decel from fast long move
}
digitalWrite(Adir, HIGH);
digitalWrite (12, HIGH);
delayMicroseconds(Aacel);
digitalWrite(12, LOW);
delayMicroseconds(Aacel);
Acmd--;
}
}
void Bposmov() {
// Serial.println("x");
Bcmd = Bpos;
Bacel = Bspd + 3000;
int a = 0;
while (Bcmd >= 1) {
//============== accelrate plus set the max speed
if (Bacel > 600 && a == 0) {// top speed here
Bacel = Bacel / 1.006;
}
if (Bcmd < 600 && Bacel < 1600) {// never hit max slow down any way
a = 1; // set bit to one to avoid acel conflict
}
if (a == 1 && Bcmd < 600 && Bacel < 2999)// when were aproching last few steps
{
Bacel = Bacel * 1.006 ; // decel from short move
}
if (a == 1 || a == 0 && Bacel < 599 && Bcmd < 300) {//<<<<< trigger max speed hit not much time to stop
a = 2;
}
if (a == 2 && Bcmd < 500 && Bacel < 2999) {
Bacel = Bacel * 1.006 ; // decel from fast long move
}
digitalWrite(Bdir, HIGH);
digitalWrite(10, HIGH);
delayMicroseconds(Bacel);
digitalWrite(10, LOW);
delayMicroseconds(Bacel);
Bcmd--;
}
a = 0;
}
void Bnegmov() {
Bcmd = Bpos * -1;
Bacel = Bspd + 3000;
int a = 0;
while (Bcmd >= 1) {
//============== accelrate plus set the max speed
if (Bacel > 600 && a == 0) {// top speed here
Bacel = Bacel / 1.006;
}
if (Bcmd < 600 && Bacel < 1600) {// never hit max slow down any way
a = 1; // set bit to one to avoid acel conflict
}
if (a == 1 && Bcmd < 600 && Bacel < 2999)// when were aproching last few steps
{
Bacel = Bacel * 1.006 ; // decel from short move
}
if (a == 1 || a == 0 && Bacel < 599 && Bcmd < 300) {//<<<<< trigger max speed hit not much time to stop
a = 2;
}
if (a == 2 && Bcmd < 500 && Bacel < 2999) {
Bacel = Bacel * 1.006 ; // decel from fast long move
}
digitalWrite(Bdir, LOW);
digitalWrite(10, HIGH);
delayMicroseconds(Bacel);
digitalWrite(10, LOW);
delayMicroseconds(Bacel);
Bcmd--;
}
}
void Cposmov() {
// Serial.println("x");
Ccmd = Cpos;
Cacel = Cspd + 3000;
int a = 0;
while (Ccmd >= 1) {
//============== accelrate plus set the max speed
if (Cacel > 600 && a == 0) {// top speed here
Cacel = Cacel / 1.004;
}
if (Ccmd < 600 && Cacel < 1600) {// never hit max slow down any way
a = 1; // set bit to one to avoid acel conflict
}
if (a == 1 && Ccmd < 600 && Cacel < 2999)// when were aproching last few steps
{
Cacel = Cacel * 1.003 ; // decel from short move
}
if (a == 1 || a == 0 && Cacel < 599 && Ccmd < 300) {//<<<<< trigger max speed hit not much time to stop
a = 2;
}
if (a == 2 && Ccmd < 500 && Cacel < 2999) {
Cacel = Cacel * 1.003 ; // decel from fast long move
}
digitalWrite(Cdir, HIGH);
digitalWrite(11, HIGH);
delayMicroseconds(Cacel);
digitalWrite(11, LOW);
delayMicroseconds(Cacel);
Ccmd--;
}
}
void Cnegmov() {
Ccmd = Cpos * -1;
Cacel = Cspd + 3000;
int a = 0;
while (Ccmd >= 1) {
//============== accelrate plus set the max speed
if (Cacel > 600 && a == 0) {// top speed here
Cacel = Cacel / 1.004;
}
if (Ccmd < 600 && Cacel < 1600) {// never hit max slow down any way
a = 1; // set bit to one to avoid acel conflict
}
if (a == 1 && Ccmd < 600 && Cacel < 2999)// when were aproching last few steps
{
Cacel = Cacel * 1.003 ; // decel from short move
}
if (a == 1 || a == 0 && Cacel < 599 && Ccmd < 300) {//<<<<< trigger max speed hit not much time to stop
a = 2;
}
if (a == 2 && Ccmd < 500 && Cacel < 2999) {
Cacel = Cacel * 1.003 ; // decel from fast long move
}
digitalWrite(Cdir, LOW);
digitalWrite(11, HIGH);
delayMicroseconds(Cacel);
digitalWrite(11, LOW);
delayMicroseconds(Cacel);
Ccmd--;
}
}
void Dposmov() {
// Serial.println("x");
Dcmd = Dpos;
Dacel = Dspd + 2100;
int a = 0;
while (Dcmd >= 1) {
//============== accelrate plus set the max speed
if (Dacel > 300 && a == 0) {// top speed here
Dacel = Dacel / 1.003;
}
if (Dcmd < 600 && Dacel < 1600) {// never hit max slow down any way
a = 1; // set bit to one to avoid acel conflict
}
if (a == 1 && Dcmd < 600 && Dacel < 2099)// when were aproching last few steps
{
Dacel = Dacel * 1.002 ; // decel from short move
}
if (a == 1 || a == 0 && Dacel < 599 && Dcmd < 300) {//<<<<< trigger max speed hit not much time to stop
a = 2;
}
if (a == 2 && Dcmd < 500 && Dacel < 2099) {
Dacel = Dacel * 1.002 ; // decel from fast long move
}
digitalWrite(Ddir, LOW);
digitalWrite(5, HIGH);
delayMicroseconds(Dacel);
digitalWrite(5, LOW);
delayMicroseconds(Dacel);
Dcmd--;
}
}
void Dnegmov() {
Dcmd = Dpos * -1;
Dacel = Dspd + 2100;
int a = 0;
while (Dcmd >= 1) {
//============== accelrate plus set the max speed
if (Dacel > 300 && a == 0) {// top speed here
Dacel = Dacel / 1.002;
}
if (Dcmd < 600 && Dacel < 1600) {// never hit max slow down any way
a = 1; // set bit to one to avoid acel conflict
}
if (a == 1 && Dcmd < 600 && Dacel < 2099)// when were aproching last few steps
{
Dacel = Dacel * 1.002 ; // decel from short move
}
if (a == 1 || a == 0 && Dacel < 599 && Dcmd < 300) {//<<<<< trigger max speed hit not much time to stop
a = 2;
}
if (a == 2 && Dcmd < 500 && Dacel < 2099) {
Dacel = Dacel * 1.002 ; // decel from fast long move
}
digitalWrite(Ddir, HIGH);
digitalWrite(5, HIGH);
delayMicroseconds(Dacel);
digitalWrite(5, LOW);
delayMicroseconds(Dacel);
Dcmd--;
}
}
void Eposmov() {
// Serial.println("x");
Ecmd = Epos;
Eacel = Espd + 3000;
int a = 0;
while (Ecmd >= 1) {
//============== accelrate plus set the max speed
if (Eacel > 300 && a == 0) {// top speed here
Eacel = Eacel / 1.004;
}
if (Ecmd < 600 && Eacel < 1600) {// never hit max slow down any way
a = 1; // set bit to one to avoid acel conflict
}
if (a == 1 && Ecmd < 600 && Eacel < 2999)// when were aproching last few steps
{
Eacel = Eacel * 1.003 ; // decel from short move
}
if (a == 1 || a == 0 && Eacel < 599 && Ecmd < 300) {//<<<<< trigger max speed hit not much time to stop
a = 2;
}
if (a == 2 && Ecmd < 500 && Eacel < 2999) {
Eacel = Eacel * 1.003 ; // decel from fast long move
}
digitalWrite(Edir, LOW);
digitalWrite(4, HIGH);
delayMicroseconds(Eacel);
digitalWrite(4, LOW);
delayMicroseconds(Eacel);
Ecmd--;
}
}
void Enegmov() {
Ecmd = Epos * -1;
Eacel = Espd + 3000;
int a = 0;
while (Ecmd >= 1) {
//============== accelrate plus set the max speed
if (Eacel > 300 && a == 0) {// top speed here
Eacel = Eacel / 1.004;
}
if (Ecmd < 600 && Eacel < 1600) {// never hit max slow down any way
a = 1; // set bit to one to avoid acel conflict
}
if (a == 1 && Ecmd < 600 && Eacel < 2999)// when were aproching last few steps
{
Eacel = Eacel * 1.003 ; // decel from short move
}
if (a == 1 || a == 0 && Eacel < 599 && Ecmd < 300) {//<<<<< trigger max speed hit not much time to stop
a = 2;
}
if (a == 2 && Ecmd < 500 && Eacel < 2999) {
Eacel = Eacel * 1.003 ; // decel from fast long move
}
digitalWrite(Edir, HIGH);
digitalWrite(4, HIGH);
delayMicroseconds(Eacel);
digitalWrite(4, LOW);
delayMicroseconds(Eacel);
Ecmd--;
}
}
void Fposmov() {
// Serial.println("x");
Fcmd = Fpos;
Facel = Fspd + 3000;
int a = 0;
while (Fcmd >= 1) {
//============== accelrate plus set the max speed
if (Facel > 300 && a == 0) {// top speed here
Facel = Facel / 1.004;
}
if (Fcmd < 600 && Facel < 1600) {// never hit max slow down any way
a = 1; // set bit to one to avoid acel conflict
}
if (a == 1 && Fcmd < 600 && Facel < 2999)// when were aproching last few steps
{
Facel = Facel * 1.003 ; // decel from short move
}
if (a == 1 || a == 0 && Facel < 599 && Fcmd < 300) {//<<<<< trigger max speed hit not much time to stop
a = 2;
}
if (a == 2 && Fcmd < 500 && Facel < 2999) {
Facel = Facel * 1.003 ; // decel from fast long move
}
digitalWrite(Fdir, LOW);
digitalWrite(3, HIGH);
delayMicroseconds(Facel);
digitalWrite(3, LOW);
delayMicroseconds(Facel);
Fcmd--;
}
;
}
void Fnegmov() {
Fcmd = Fpos * -1;
Facel = Fspd + 3000;
int a = 0;
while (Fcmd >= 1) {
//============== accelrate plus set the max speed
if (Facel > 300 && a == 0) {// top speed here
Facel = Facel / 1.004;
}
if (Fcmd < 600 && Facel < 1600) {// never hit max slow down any way
a = 1; // set bit to one to avoid acel conflict
}
if (a == 1 && Fcmd < 600 && Facel < 2999)// when were aproching last few steps
{
Facel = Facel * 1.003 ; // decel from short move
}
if (a == 1 || a == 0 && Facel < 599 && Fcmd < 300) {//<<<<< trigger max speed hit not much time to stop
a = 2;
}
if (a == 2 && Fcmd < 500 && Facel < 2999) {
Facel = Facel * 1.003 ; // decel from fast long move
}
digitalWrite(Fdir, HIGH);
digitalWrite(3, HIGH);
delayMicroseconds(Facel);
digitalWrite(3, LOW);
delayMicroseconds(Facel);
Fcmd--;
}
}