Evitar golpe o freno de motores PAP Libreria AccelStepper

Matias24 · September 10, 2022, 3:28pm

Hola! estoy trabajando en un proyecto que consiste en una maquina de rehabilitacion o de guia para mejorar el paso de una persona.
Consiste en 6 Motores PASO A PASO, el objetivo es que a un nene que no puede caminar, ayudarlo con esta maquina para que le sea de guia y asi aprender.
Estoy utilizando un arduino MEGA y 6 motores paso a paso nema 23 conectados a unos drivers DM542E. Es totalmente programable desde un keypad y un lcd conectados a arduino.
El array DATOS, son los datos de velocidad, desplazamiento longitudinal, desplazamiento vertical, inclinacion del tobillo, respectivamente.

El array positions, son los motores.

positions[0] = desplazamiento longitudinal pie 1
positions[1] = desplazamiento vertical pie 1
positions[2] = inclinacion pie 1
positions[3] = desplazamiento longitudinal pie 2
positions[4] = desplazamiento vertical pie 2
positions[5] = inclinacion pie 2

La maquina al iniciar, hace un seteo a 0, Se va a un extremo y luego se posiciona en la mitad del desplazamiento longitudinal, abajo en vertical y paralelo al piso la inclinacion, ese es el 0 de maquina.
Tuve que dividir el paso en 4 instancias.
El problema es cuando pasa por la instancia 2-3 que tengo un pequeño golpe de los motores, es decir, se frena justo en mitad del paso, esto porque tengo que hacer el cambio de giro de los motores verticales y los inclinadores.
Intente varias alternativas pero no funcionan, no puedo cambiar el sentido de giro de un par de motores sin que todos lleguen a su destino.
Dejo el codigo y un video muestra de como funciona asi resulta mas facil!

//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();

Surbyte · September 10, 2022, 3:36pm

Código parcial, que esperas que te digamos?
Quita los delay() si haces una máquina de estados (que espero todo funcione de ese modo) no puedes tener momentos en los que no se haga nada.
Tienes ahi delay(100) 100 mseg sin hacer nada y no se porque no se ve el código si eso no se anida y se suma como un corte mas largo.

Matias24 · September 10, 2022, 4:27pm

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();

}
}

Surbyte · September 10, 2022, 7:55pm

Hola @Matias24 ahora mejor. Se ve todo el código y ayuda aunque no lo creas a entender que ocurre sino queda descontextualizado.

Veo que estamos dentro de un while por ende ese delay() me sigue haciendo ruido.
Consulta, probaste sin delay(100)?
Solo para probar si tiene o no incidencia. Te aclaro que es el responsable de casi todos los males y como ahora se ve que estamos dentro del while

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();
  }

su efecto es acumulativo como te dije antes.
Próba sin el, o agregá a tu manejo de ingreso de datos algo que te permita cambiar ese delay de tal forma que suavices el comportamiento.
Una tecla que cada vez que lo presiones baje 10 mseg por ejemplo.
Encima hay dos, uno de 2 a 3 y otro luego de la instancia 4.
ya son 200 mseg en las que los motores no reciben ordenes.

Matias24 · September 11, 2022, 1:47am

Gracias por la respuesta! Probé sin ese delay, pero lo sigue haciendo igual, entiendo que la librería llega a la posición final, frena, y ahí lee las siguientes posiciones y comienza de 0 él movimiento.
Intente hacer que el motor que desplaza longitudinal haga todo el recorrido sin frenar al medio, es decir, que pase de un extremo a otro, pero no puedo modificar los demás motores hasta que no se termine el movimiento de este.

system · March 10, 2023, 1:47am

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.

Topic		Replies	Views
Generando pulsos motor PaP Software	6	1512	May 7, 2021
como controlar motores paso a paso Español	3	884	May 6, 2021
Controlar dos motores paso a paso simultaneamente. Software	2	7124	May 7, 2021
problema con programa para un motor pap Software	2	571	May 7, 2021
Mover motor PaP con "IF" si 1 o 2 interruptores están LOW [SOLUCIONADO] Software	4	1058	May 7, 2021

Evitar golpe o freno de motores PAP Libreria AccelStepper

Related topics