#include <Wire.h>
#include <AccelStepper.h>
//-----------------------------------------------------------------------------
#define dirPin1 6
#define stepPin1 7
#define motorInterfaceType1 1
AccelStepper stepper = AccelStepper(motorInterfaceType1, stepPin1, dirPin1);
//-----------------------------------------------------------------------------
#define dirPin2 9
#define stepPin2 10
#define motorInterfaceType2 1
AccelStepper stepper1 = AccelStepper(motorInterfaceType2, stepPin2, dirPin2);
//variabili -------------------------------------------------------------------
//Pulsanti INVERSIONE ---------------------------------------------------------
int varsw1 = HIGH; // HIGH - LOW inverte lettura asse 0 scarico (2)
//-------------------
int varsw2 = HIGH; // HIGH - LOW inverte lettura asse 0 scarico (3)
int vartub = HIGH; // HIGH - LOW inverte lettura tubo scarico (3)
//-------------------
int varsw3 = HIGH; // HIGH - LOW inverte lettura asse 0 scarico (4)
int vartub1 = HIGH; // HIGH - LOW inverte lettura tubo scarico (4)
//-------------------
int ResPuls = HIGH; // HIGH - LOW reset pulsante
int reseton = 1; // 0 - 1 Reset Pulsante on - off
//-----------------------------------------------------------------------------
//reg. giri enc.- motore
int varpot = 1200; // 1200 impulsi = 1 giro encoder
int varstep = 750; // 400 impulsi = 1 giro motore
//reg. giri enc.- motore
//-----------------------------------------------------------------------------
//Scarico (3)
int maxspeed = 10000; //velocita di movimento(0 - 10000)
int acceleration = 10000; //velocita accelerazione iniziale(0 - 10000)
//-------------------
int maxspeedres = 500; //velocita di movimento nel reset(0 - 10000)
int accelerationres = 500; //velocita accelerazione iniziale nel reset(0 - 10000)
//Scarico (3)
//-----------------------------------------------------------------------------
//Scarico (4)
int scarico4 = 0; //set 0 disable - 1 enable
//-------------------
int maxspeed1 = 8000; //velocita di movimento(0 - 10000)
int acceleration1 = 8000; //velocita accelerazione iniziale(0 - 10000)
//-------------------
int maxspeedres1 = 500; //velocita di movimento nel reset(0 - 10000)
int accelerationres1 = 500; //velocita accelerazione iniziale nel reset(0 - 10000)
//Scarico (4)
//-----------------------------------------------------------------------------
//Seriale
int ENCSerial = 0; //set 0 disable - 1 enable (Abilitazione Seriale)
int Serialab = 0; //set 0 disable - 1 enable
//Seriale
//Variabili -------------------------------------------------------------------
//pin uscite digitali
#define encoderPinA 2
#define encoderPinB 3
//-------------------
#define resswitch1 4
//-------------------
#define resswitch2 5
#define tub 8
//-------------------
#define resswitch3 11
#define tub1 12
// ------------------
#define ResetPuls 13
// ------------------
volatile long pos = 0;
int pulsres = 0;
int reset = 0;
int res1 = 0;
int res2 = 0;
int pos1;
int tubeon = 0;
int tubeon1 = 0;
int dueres = 0;
int stoped = 0;
void setup() {
pinMode(encoderPinA, INPUT_PULLUP);
pinMode(encoderPinB, INPUT_PULLUP);
pinMode(resswitch1, INPUT_PULLUP);
pinMode(resswitch2, INPUT_PULLUP);
pinMode(resswitch3, INPUT_PULLUP);
pinMode(ResetPuls, INPUT_PULLUP);
pinMode(tub, INPUT_PULLUP);
pinMode(tub1, INPUT_PULLUP);
stepper.setMaxSpeed(maxspeed);
stepper.setAcceleration(acceleration);
stepper1.setMaxSpeed(maxspeed1);
stepper1.setAcceleration(acceleration1);
attachInterrupt(0, doEncoderA, CHANGE);
attachInterrupt(1, doEncoderB, CHANGE);
if(Serialab == 1){Serial.begin(9600);}
}
void loop() {
//Velocità ---------------------------------------------------------------------------------------------------------------------
if(reset == 1){ //velocità di funzionamento
stepper.setMaxSpeed(maxspeed);
stepper.setAcceleration(acceleration);
stepper1.setMaxSpeed(maxspeed1);
stepper1.setAcceleration(acceleration1);
}
else{ // velocità di reset
stepper.setMaxSpeed(maxspeedres);
stepper.setAcceleration(accelerationres);
stepper1.setMaxSpeed(maxspeedres1);
stepper1.setAcceleration(accelerationres1);
}
// lettuta encoder posizione ---------------------------------------------------------------------------------------------------
if(reset == 1){
if(stoped == 0){
pos1 = map(pos, 0, varpot, 0, varstep);
if(ENCSerial == 1){
Serial.println(pos1);
//if(digitalRead(resswitch1) != varsw1 && pos >= 100){pulsres = 1;reset = 0;res1 = 1;res2 = 0}
}
//funzionamento DRIVER 1 -------------------------------------------------------------------------------------------------------
if(digitalRead(tub) == vartub){ //lettura tubo
tubeon = 1;
}
else if(digitalRead(tub) != vartub && digitalRead(resswitch1) != varsw1 && digitalRead(resswitch2) != varsw2 && tubeon == 1){ //reset scarico (3)
tubeon = 0;
delay(1000);
pos = 0;
}
if(tubeon == 1){
if(digitalRead(resswitch2) != varsw2 && stepper.currentPosition() >= 1000){
reset = 0;
stepper.runToNewPosition(0);
}
else{
stepper.runToNewPosition(pos1);
}
}
//funzionamento DRIVER 2 -------------------------------------------------------------------------------------------------------
if(scarico4 == 1){
if(digitalRead(tub1) == vartub1){ //lettura tubo
tubeon1 = 1;
}
else if(digitalRead(tub1) != vartub1 && digitalRead(resswitch1) != varsw1 && digitalRead(resswitch3) != varsw3 && tubeon1 == 1){ //reset scarico (3)
tubeon1 = 0;
delay(1000);
pos = 0;
}
if(tubeon1 == 1){
if(digitalRead(resswitch2) != varsw2 && pos1 >= 1000){
pulsres = 1;
reset = 0;
res1 = 1;
res2 = 0;
}
else{
stepper1.runToNewPosition(pos1);
}
}
}
}
}
//reset DRIVER 1 ---------------------------------------------------------------------------------------------------------------
if(reset == 0){
if(pulsres == 0){
if(scarico4 == 0){
if(digitalRead(resswitch1) != varsw1 && reset == 0 && res1 == 1){ //controllo reset completato scarico (2) + avvio reset scarico (3)
if(digitalRead(resswitch2) == varsw2){
stepper.moveTo(-100000);
stepper.run();
res2 = 1;
//Serial.println("3");
}
else if(digitalRead(resswitch2) != varsw2 && res2 == 1){ //controllo reset del punto zero del encoder e del motore
stepper.stop();
stepper.setCurrentPosition(0);
pos = 0;
reset = 1;
//Serial.println("5");
}
else{ //reset scarico (3)
stepper.runToNewPosition(2000);
//Serial.println("4");
}
}
else if(digitalRead(resswitch1) == varsw1 && dueres == 3 && reset == 0 && res1 == 0){ //controllo reset (2) scarico
res1 = 1;
//Serial.println("2");
}
else if(digitalRead(resswitch1) != varsw1 && dueres == 2){ //doppio reset
dueres = 3;
//Serial.println("1-2");
delay(500);
}
else if(digitalRead(resswitch1) == varsw1 && dueres == 1){ //doppio reset
dueres = 2;
//Serial.println("1-1");
delay(500);
}
else if(digitalRead(resswitch1) != varsw1 && dueres == 0){ //doppio reset
dueres = 1;
//Serial.println("1");
delay(500);
}
}
//reset DRIVER 1 - 2 -----------------------------------------------------------------------------------------------------------
if(scarico4 == 1){
if(digitalRead(resswitch1) == varsw1 && reset == 0 && res1 == 0){ //controllo reset (2) scarico
res1 = 1;
}
if(digitalRead(resswitch1) != varsw1 && dueres == 0){ //doppio reset
dueres = 1;
//Serial.println("1");
}
else if(digitalRead(resswitch1) == varsw1 && dueres == 1){ //doppio reset
dueres = 2;
//Serial.println("1-1");
}
else if(digitalRead(resswitch1) != varsw1 && dueres == 2){ //doppio reset
dueres = 3;
//Serial.println("1-2");
}
else if(digitalRead(resswitch1) != varsw1 && dueres == 3 && reset == 0 && res1 == 1){ //controllo reset completato scarico (2) + avvio reset scarico (3)
if(digitalRead(resswitch2) == varsw2){
res2 = 1;
stepper.moveTo(-100000);
stepper.run();
}
else if(digitalRead(resswitch2) != varsw2 && res2 == 1){ //controllo reset del punto zero del encoder e del motore
stepper.stop();
stepper.setCurrentPosition(0);
res1 = 2;
}
else{ //reset scarico (3)
stepper.runToNewPosition(2000);
}
}
else if(digitalRead(resswitch1) != varsw1 && reset == 0 && res1 == 2){ //controllo reset completato scarico (2) + avvio reset scarico (3)
if(digitalRead(resswitch3) == varsw3){
res2 = 2;
stepper1.moveTo(-100000);
stepper1.run();
}
else if(digitalRead(resswitch3) != varsw3 && res2 == 2){ //controllo reset del punto zero del encoder e del motore
stepper1.stop();
stepper1.setCurrentPosition(0);
pos = 0;
reset = 1;
}
else{ //reset scarico (3)
stepper1.runToNewPosition(2000);
}
}
}
}
}
//reset DRIVER 1 RESET PULSANTE-------------------------------------------------------------------------------------------------
if(reseton == 1){
if(digitalRead(ResetPuls) == ResPuls){
//Serial.println("1");
pulsres = 1;
reset = 0;
res1 = 1;
res2 = 0;
}
if(pulsres == 1){
if(reset == 0){
if(scarico4 == 0){
if(reset == 0 && res1 == 1){ //controllo reset completato scarico (2) + avvio reset scarico (3)
if(digitalRead(resswitch2) == varsw2){
stepper.moveTo(-100000);
stepper.run();
res2 = 1;
}
else if(digitalRead(resswitch2) != varsw2 && res2 == 1){ //controllo reset del punto zero del encoder e del motore
stepper.stop();
stepper.setCurrentPosition(0);
pos = 0;
reset = 1;
stoped = 0;
}
else{ //reset scarico (3)
stepper.runToNewPosition(2000);
}
}
}
//reset DRIVER 1 - 2 RESET PULSANTE---------------------------------------------------------------------------------------------
if(scarico4 == 1){
if(reset == 0 && res1 == 1){ //controllo reset completato scarico (2) + avvio reset scarico (3)
if(digitalRead(resswitch2) == varsw2){
res2 = 1;
stepper.moveTo(-100000);
stepper.run();
}
else if(digitalRead(resswitch2) != varsw2 && res2 == 1){ //controllo reset del punto zero del encoder e del motore
stepper.stop();
stepper.setCurrentPosition(0);
res1 = 2;
}
else{ //reset scarico (3)
stepper.runToNewPosition(2000);
}
}
else if(reset == 0 && res1 == 2){ //controllo reset completato scarico (2) + avvio reset scarico (3)
if(digitalRead(resswitch3) == varsw3){
res2 = 2;
stepper1.moveTo(-100000);
stepper1.run();
}
else if(digitalRead(resswitch3) != varsw3 && res2 == 2){ //controllo reset del punto zero del encoder e del motore
stepper1.stop();
stepper1.setCurrentPosition(0);
pos = 0;
reset = 1;
pulsres = 0;
stoped = 0;
}
else{ //reset scarico (3)
stepper1.runToNewPosition(2000);
}
}
}
}
}
}
}
//encoder ----------------------------------------------------------------------------------------------------------------------
void doEncoderA() {
if (digitalRead(encoderPinA) == HIGH) {
if (digitalRead(encoderPinB) == LOW) {
pos = pos + 1; // CW
}
else {
pos = pos - 1; // CCW
}
}
else {
if (digitalRead(encoderPinB) == HIGH) {
pos = pos + 1; // CW
}
else {
pos = pos - 1; // CCW
}
}
}
void doEncoderB() {
if (digitalRead(encoderPinB) == HIGH) {
if (digitalRead(encoderPinA) == HIGH) {
pos = pos + 1; // CW
}
else {
pos = pos - 1; // CCW
}
}
else {
if (digitalRead(encoderPinA) == LOW) {
pos = pos + 1; // CW
}
else {
pos = pos - 1; // CCW
}
}
}