El codigo entero no lo subi porque es extenso, pense que con eso era sufienciente, pero ya lo agrego!
Los delay estan puestos en los extremos nada mas, pero el golpe me lo hace a mitad del paso, es decir, cuando se cruzan ambos pies que seria la instancia 2-3, justo en ese cambio entre la 2 y el inicio de la 3.
En los extremos si quiero un tiempo en que el paso este frenado, esto me ayuda con el tema de la aceleracion, ya que en este modo la libreria no me permite usarla, si quito los delay, el motor pega un golpe mucho mas fuerte y a veces llega a perder algun paso.
El boton conectado al pin 14 es el que se utiliza para iniciar el moviemiento, previamente seteado a 0.
X, Y, Z correspoden al pie derecho
X2, Y2, Z2 al pie izquierdo
#include "AccelStepper.h"
#include <MultiStepper.h>
#include <Keypad.h>
#include <LiquidCrystal_I2C.h>
#include <EEPROM.h>
#define STEPPERX_DIR_PINX 3
#define STEPPERX_STEP_PINX 2
#define STEPPERY_DIR_PINY 5
#define STEPPERY_STEP_PINY 4
#define STEPPERZ_DIR_PINZ 7
#define STEPPERZ_STEP_PINZ 6
#define STEPPERX2_DIR_PINX2 9
#define STEPPERX2_STEP_PINX2 8
#define STEPPERY2_DIR_PINY2 11
#define STEPPERY2_STEP_PINY2 10
#define STEPPERZ2_DIR_PINZ2 13
#define STEPPERZ2_STEP_PINZ2 12
AccelStepper stepperX(AccelStepper::DRIVER, STEPPERX_STEP_PINX, STEPPERX_DIR_PINX);
AccelStepper stepperY(AccelStepper::DRIVER, STEPPERY_STEP_PINY, STEPPERY_DIR_PINY);
AccelStepper stepperZ(AccelStepper::DRIVER, STEPPERZ_STEP_PINZ, STEPPERZ_DIR_PINZ);
AccelStepper stepperX2(AccelStepper::DRIVER, STEPPERX2_STEP_PINX2, STEPPERX2_DIR_PINX2);
AccelStepper stepperY2(AccelStepper::DRIVER, STEPPERY2_STEP_PINY2, STEPPERY2_DIR_PINY2);
AccelStepper stepperZ2(AccelStepper::DRIVER, STEPPERZ2_STEP_PINZ2, STEPPERZ2_DIR_PINZ2);
// Define the Pins used
#define home_switchX 53
#define home_switchY 52
#define home_switchZ 50
#define home_switchX2 49
#define home_switchY2 48
#define home_switchZ2 51
int move_finishedX = 1;
long initial_homingX = -1;
int move_finishedY = 1;
long initial_homingY = -1;
int move_finishedZ = 1;
long initial_homingZ = -1;
int move_finishedX2 = 1;
long initial_homingX2 = -1;
int move_finishedY2 = 1;
long initial_homingY2 = -1;
int move_finishedZ2 = 1;
long initial_homingZ2 = -1;
MultiStepper steppers;
const int ROW_NUM = 4; // four rows
const int COLUMN_NUM = 4; // four columns
byte customChar[] = {
B00000,
B01100,
B00110,
B11111,
B11111,
B00110,
B01100,
B00000
};
byte customChar1[] = {
B00000,
B00000,
B00000,
B00000,
B00000,
B00000,
B00000,
B00000
};
char keys[ROW_NUM][COLUMN_NUM] = {
{'1', '2', '3', 'A'},
{'4', '5', '6', 'B'},
{'7', '8', '9', 'C'},
{'*', '0', '#', 'D'}
};
byte pin_rows[ROW_NUM] = {22, 24, 26, 28}; // connect to the row pinouts of the keypad
byte pin_column[COLUMN_NUM] = {30, 32, 34, 36}; // connect to the column pinouts of the keypad
Keypad keypad = Keypad(makeKeymap(keys), pin_rows, pin_column, ROW_NUM, COLUMN_NUM );
LiquidCrystal_I2C lcd(0x27, 20, 4); // I2C address 0x27, 16 column and 2
int cursorColumn = 16;
int fila = 0;
char pulsacion;
String PIN_Tecleado = ""; // teclas digitadas
char cadena[17];
int Dato[4] ;
int boton = 0;
const int timeThreshold = 150;
const int intPin = 18;
volatile int ISRCounter = 0;
long startTime = 0;
void setup() {
pinMode(intPin, INPUT_PULLUP);
Serial.begin(9600);
lcd.init(); // initialize the lcd
lcd.backlight();
lcd.setCursor(5,0);
lcd.print("BIENVENIDO");
lcd.setCursor(1,1);
lcd.print("Maquina de caminar");
lcd.setCursor(2,2);
lcd.print("Ver Softaware 1.0");
lcd.setCursor(2,3);
lcd.print("Cargando Datos...");
delay(5000);
lcd.clear();
Dato[0] = EEPROM.read(0);
Dato[1] = EEPROM.read(1);
Dato[2] = EEPROM.read(2);
Dato[3] = EEPROM.read(3);
lcd.setCursor(0, 0);
lcd.print("A-Velocidad:");
lcd.setCursor(16,0);
lcd.print(Dato[0]);
lcd.setCursor(0, 1);
lcd.print("B-Longitud:");
lcd.setCursor(16,1);
lcd.print(Dato[1]);
lcd.setCursor(0, 2);
lcd.print("C-Altura:");
lcd.setCursor(16,2);
lcd.print(Dato[2]);
lcd.setCursor(0, 3);
lcd.print("D-Inclinacion:");
lcd.setCursor(16,3);
lcd.print(Dato[3]);
lcd.createChar(0, customChar);
lcd.createChar(1, customChar1);
Serial.begin(9600);
pinMode(home_switchX, INPUT_PULLUP);
pinMode(home_switchY, INPUT_PULLUP);
pinMode(home_switchZ, INPUT_PULLUP);
pinMode(home_switchX2, INPUT_PULLUP);
pinMode(home_switchY2, INPUT_PULLUP);
pinMode(home_switchZ2, INPUT_PULLUP);
pinMode(14,INPUT_PULLUP);
delay(5);
stepperX.setMaxSpeed(1000.0);
stepperX.setAcceleration(1000.0);
stepperY.setMaxSpeed(2000.0);
stepperY.setAcceleration(1000.0);
stepperZ.setMaxSpeed(200.0);
stepperZ.setAcceleration(100.0);
stepperX2.setMaxSpeed(1000.0);
stepperX2.setAcceleration(1000.0);
stepperY2.setMaxSpeed(2000.0);
stepperY2.setAcceleration(1000.0);
stepperZ2.setMaxSpeed(200.0);
stepperZ2.setAcceleration(100.0);
stepperX.setCurrentPosition(0);
stepperY.setCurrentPosition(0);
stepperZ.setCurrentPosition(0);
stepperX2.setCurrentPosition(0);
stepperY2.setCurrentPosition(0);
stepperZ2.setCurrentPosition(0);
// Start Homing procedure of Stepper Motor at startup
while (!digitalRead(home_switchX)) { // Make the Stepper move CCW until the switch is activated
stepperX.moveTo(initial_homingX); // Set the position to move to
initial_homingX--; // Decrease by 1 for next move if needed
stepperX.run(); // Start moving the stepper
}
stepperX.setCurrentPosition(0); // Set the current position as zero for now
stepperX.moveTo(3000);
while ((stepperX.distanceToGo() != 0)) {
stepperX.run();
}
stepperX.setCurrentPosition(0);
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
while (!digitalRead(home_switchY)) { // Make the Stepper move CCW until the switch is activated
stepperY.moveTo(initial_homingY); // Set the position to move to
initial_homingY--; // Decrease by 1 for next move if needed
stepperY.run(); // Start moving the stepper
}
stepperY.setCurrentPosition(0); // Set the current position as zero for now
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
while (digitalRead(home_switchZ)) { // Make the Stepper move CCW until the switch is activated
stepperZ.moveTo(initial_homingZ); // Set the position to move to
initial_homingZ--; // Decrease by 1 for next move if needed
stepperZ.run(); // Start moving the stepper
}
// stepperZ.setCurrentPosition(0);
// stepperZ.moveTo(230);
//while ((stepperZ.distanceToGo() != 0)) {
//stepperZ.run();
//}
stepperZ.setCurrentPosition(0);
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////7
while (!digitalRead(home_switchX2)) { // Make the Stepper move CCW until the switch is activated
stepperX2.moveTo(initial_homingX2); // Set the position to move to
initial_homingX2--; // Decrease by 1 for next move if needed
stepperX2.run(); // Start moving the stepper
}
stepperX2.setCurrentPosition(0); // Set the current position as zero for now
stepperX2.moveTo(3000);
while ((stepperX2.distanceToGo() != 0)) {
stepperX2.run();
}
stepperX2.setCurrentPosition(0);
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
while (!digitalRead(home_switchY2)) { // Make the Stepper move CCW until the switch is activated
stepperY2.moveTo(initial_homingY2); // Set the position to move to
initial_homingY2--; // Decrease by 1 for next move if needed
stepperY2.run(); // Start moving the stepper
}
stepperY2.setCurrentPosition(0); // Set the current position as zero for now
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
while (digitalRead(home_switchZ2)) { // Make the Stepper move CCW until the switch is activated
stepperZ2.moveTo(initial_homingZ2); // Set the position to move to
initial_homingZ2--; // Decrease by 1 for next move if needed
stepperZ2.run(); // Start moving the stepper
}
// stepperZ2.setCurrentPosition(0);
// stepperZ2.moveTo(230);
//while ((stepperZ2.distanceToGo() != 0)) {
//stepperZ2.run();
//}
stepperZ2.setCurrentPosition(0);
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
steppers.addStepper(stepperX);
steppers.addStepper(stepperY);
steppers.addStepper(stepperZ);
steppers.addStepper(stepperX2);
steppers.addStepper(stepperY2);
steppers.addStepper(stepperZ2);
}
void loop() {
pulsacion = keypad.getKey();
switch (pulsacion) {
case 'A':
fila = 0;
lcd.setCursor(14, fila);
lcd.write(0);
lcd.setCursor(14, 1);
lcd.write(1);
lcd.setCursor(14, 2);
lcd.write(1);
lcd.setCursor(14, 3);
lcd.write(1);
break;
case 'B':
fila = 1;
lcd.setCursor(14, fila);
lcd.write(0);
lcd.setCursor(14, 0);
lcd.write(1);
lcd.setCursor(14, 2);
lcd.write(1);
lcd.setCursor(14, 3);
lcd.write(1);
break;
case 'C':
fila = 2;
lcd.setCursor(14, fila);
lcd.write(0);
lcd.setCursor(14, 0);
lcd.write(1);
lcd.setCursor(14, 1);
lcd.write(1);
lcd.setCursor(14, 3);
lcd.write(1);
break;
case 'D':
fila = 3;
lcd.setCursor(14, fila);
lcd.write(0);
lcd.setCursor(14, 0);
lcd.write(1);
lcd.setCursor(14, 2);
lcd.write(1);
lcd.setCursor(14, 1);
lcd.write(1);
break;
case '0' ... '9': // o dígitos,
PIN_Tecleado += pulsacion; // los agregamos a la caden
lcd.setCursor(cursorColumn, fila); // move cursor to (cursorColumn, 0)
lcd.print(pulsacion);
cursorColumn++; // move cursor to next position
break;
case '#': // Lo tomaremos como enter para guardar el dato
PIN_Tecleado.toCharArray(cadena, 17);
Dato[fila] = atof (cadena);
PIN_Tecleado = "";
cursorColumn = 16;
lcd.setCursor(16, fila);
lcd.print(Dato[fila]);
break;
case '*':
delay(1500);
EEPROM.write(0, Dato[0]);
EEPROM.write(1, Dato[1]);
EEPROM.write(2, Dato[2]);
EEPROM.write(3, Dato[3]);
lcd.clear();
lcd.print("Info Guardada");
delay(2000);
lcd.setCursor(0, 0);
lcd.print("A-Velocidad:");
lcd.setCursor(16,0);
lcd.print(Dato[0]);
lcd.setCursor(0, 1);
lcd.print("B-Longitud:");
lcd.setCursor(16,1);
lcd.print(Dato[1]);
lcd.setCursor(0, 2);
lcd.print("C-Altura:");
lcd.setCursor(16,2);
lcd.print(Dato[2]);
lcd.setCursor(0, 3);
lcd.print("D-Inclinacion:");
lcd.setCursor(16,3);
lcd.print(Dato[3]);
break;
}
bool valor = digitalRead(14);
if ( valor == false ){
boton = 1;
stepperX.setMaxSpeed((Dato[0]*10));
stepperY.setMaxSpeed(100000);
stepperZ.setMaxSpeed(100000);
stepperX2.setMaxSpeed((Dato[0]*10));
stepperY2.setMaxSpeed(100000);
stepperZ2.setMaxSpeed(100000);
}
while( boton == 1){
long positions[6]; // Array of desired stepper positions
//Instancia 1, pie derecho hacia adelante mientras bajo, izquierdo asi atras mientras inclino
positions[0] = ((Dato[1])*69)/2;
positions[1] = 0;
positions[2] = 0;
positions[3] = -((Dato[1])*69)/2;
positions[4] = 0;
positions[5] = (Dato[3]*4);
steppers.moveTo(positions);
steppers.runSpeedToPosition();
//Instancia 2, ambos pies al medio, mientras elevo izquierdo e inclino la mitad del recorrido
positions[0] = 0;
positions[1] = 0;
positions[2] = 0;
positions[3] = 0;
positions[4] = (Dato[2]*40);
positions[5] = (Dato[3]*4);
delay(100); // delay para evitar un cambio brusco de movimiento, ya que no funciona acelaracion
//Instancia 3, pie derecho hacia atras mientras lo inclino, pie izquiero hacia adelante mientras bajo y termino de inclinar a 0
steppers.moveTo(positions);
steppers.runSpeedToPosition();
positions[0] = -((Dato[1])*69)/2;
positions[1] = 0;
positions[2] = (Dato[3]*4)/2;
positions[3] = ((Dato[1])*69)/2;
positions[4] = 0;
positions[5] = 0;
steppers.moveTo(positions);
steppers.runSpeedToPosition();
//Instacia 4, ambos pies al medio, pie derecho elevo e inclino a mitad del recorrido
positions[0] = 0;
positions[1] = Dato[2]*40;
positions[2] = (Dato[3]*4)/2;
positions[3] = 0;
positions[4] = 0;
positions[5] = 0;
delay(100);
steppers.moveTo(positions);
steppers.runSpeedToPosition();
}
}