Problem with Serial.print frozen my sketch

put all code please!

For my part I see print (motor);
you should try print(motor, DEC)

It would be a lot easier to guess what is happening if I could see the rest of the sketch. The problem is most likely NOT in the Serial.print() parts.

Sorry new in the forum. I try to attach the whole Skecth but I am a new user And not allowed.

I put the capture of the skecth right here:

#include  <Servo.h>

#define debug

int display = 1;

int upCount = 10;

#ifdef debug
int downCount = 10;
#else
int downCount = 500;
#endif
boolean emergencyState = false;

int motor = 0;

int test = 1;

int up_down = 0;
int up_down_max = 2;



// value to set axes position
//int retardoStep = 100;
//int countMax = 1;
// value to test axes movement
int retardoStep = 1;

int servoIni = 0;  //0-1, 2-3, 4-5, 6-7, 8-9, 10-11/
int servoNumber = 12;


boolean notStopped = true;
boolean stateChanged = true;
int previousState = 0;

int genState = 0;


class ServoControl {

  private :
    Servo servo;
    int pinPwm;
    int pinUp;
    boolean tipo; // true cw=cw,ucw=ucw   false cw=ucw, ucw=cw
    int cwN = 0;
    int ucwN = 180;
    int state = 0;
    int speed = 90;
    int countUp;
    int countDown;
    int countCurrent;
    boolean ready = true;
    
    int contador = 0;

  public:
    ServoControl() {}

    void init (int pinPwm, int pinUp, boolean tipo, int countUp, int countDown) {
      servo.attach(pinPwm);
      this->pinPwm = pinPwm;
      this->pinUp = pinUp;
      this->tipo = tipo;
      this->countUp = countUp;
      this->countDown = countDown;
      countCurrent = 0;
      state = 0;
      speed = 90;
      contador = 0;
      pinMode(pinUp, OUTPUT);
      if ( !tipo ) {
        cwN = 180;
        ucwN = 0;
      }
    }


    void cw() {
      if (motor and !emergencyState) {
        servo.write(cwN);
      }
    }


    void ucw() {
      if (motor and !emergencyState) {
        servo.write(ucwN);
      }
    }


    void selectSpeed(int speed) {
      int speedApplied;
      if (!tipo) {
        speedApplied = 180 - speed;
      } else {
        speedApplied = speed;
      }
      if (motor and !emergencyState) {
        servo.write(speedApplied);
      }
      this->speed = speed;
    }

    void stop() {
      if (motor) {
        servo.write(90);
      }
    }

    void setState(int state) {
      this->state = state;
    }
    int getState() {
      return (state);
    }

    int getCountUp() {
      return (countUp);
    }
    int getCountDown() {
      return (countDown);
    }

    int getCountCurrent () {
      return (countCurrent);
    }

    void setCountCurrent (int countCurrent) {
      this->countCurrent = countCurrent;
    }

    int getSpeed() {
      return (speed);
    }

    void setReady (boolean ready) {
      this->ready = ready;
    }
    boolean getReady () {
      return (ready);
    }

    void setContador (int contador) {
      this->contador = contador;
    }
    int getContador () {
      return(contador);
    }

    boolean getPinUp() {
 /*
      if (contador > 3){
        return(false);        
      } 
*/
      if ( digitalRead(pinUp) == HIGH ) {
        return (true);
      } else {
        return (false);
      }
    }

};

ServoControl servos[] = { ServoControl(), ServoControl(), ServoControl(), ServoControl(), ServoControl(), ServoControl(),
                          ServoControl(), ServoControl(), ServoControl(), ServoControl(), ServoControl(), ServoControl()
                        };

void up () {
  notStopped = true;
  for (int motor = servoIni; motor < servoNumber; motor++) {

#ifdef debug
    Serial.print("UP Ini Motor : ");
    Serial.print(motor);
    Serial.print("  state: ");
    Serial.print(servos[motor].getState());
      Serial.print("  Ready: ");
      Serial.print(servos[motor].getReady());
      Serial.print("  ser: ");
      Serial.print(servos[motor].getPinUp());
      Serial.print("  contador: ");
      Serial.print(servos[motor].getContador());
      Serial.print("  speed: ");
      Serial.println(servos[motor].getSpeed());
#endif

    if ( servos[motor].getState() == 0 and ( ! (servos[motor].getPinUp() or ! servos[motor].getReady() or emergencyState ) ) ) {

#ifdef debug
      Serial.print("  state: ");
      Serial.print(servos[motor].getState());
      Serial.print("  ser: ");
      Serial.print(servos[motor].getPinUp());
      Serial.print("  Ready: ");
      Serial.print(servos[motor].getReady());
      Serial.print("  EmergencySatet : ");
      Serial.println(emergencyState);
        Serial.print("=================================================  speed 90 up limit inicial ");
        Serial.println(motor);
      if ( servos[motor].getPinUp() ) {
        Serial.print("=================================================  HIGH ");
        Serial.println();
      } else {
        Serial.print("=================================================  LOW ");
        Serial.println();
      }
#endif

      servos[motor].setState(0);
      servos[motor].selectSpeed(90);
      servos[motor].setReady(false);
      servos[motor].setCountCurrent(0);

    } else if ( servos[motor].getState() == 0 and servos[motor].getReady() and servos[motor].getPinUp() ) {
      servos[motor].setState(10);
      servos[motor].selectSpeed(0);
      servos[motor].setCountCurrent(0);
#ifdef debug

 Serial.print("=================================================  speed 0 ");
        Serial.println(motor);
      if ( servos[motor].getPinUp() ) {
        Serial.print("=================================================  HIGH ");
        Serial.println();
      } else {
        Serial.print("=================================================  LOW ");
        Serial.println();
      }
#endif

//      servos[motor].setContador(0);
      

    } else if ( servos[motor].getState() == 10 and servos[motor].getReady() and servos[motor].getPinUp()  ) {
      servos[motor].setCountCurrent(servos[motor].getCountCurrent() + 1);
      servos[motor].setState(10);

#ifdef debug
        Serial.print("=================================================  speed 0 cont ");
        Serial.println(servos[motor].getCountCurrent() );
      if ( servos[motor].getCountCurrent() > servos[motor].getCountUp() ) {
        servos[motor].setReady(false);
        Serial.print("=================================================  speed 0 cont max reached ");
        Serial.println(servos[motor].getCountCurrent() );
      }
#endif
//      servos[motor].setContador(servos[motor].getContador() + 1);

    } else if ( servos[motor].getState() == 10 and ( ! servos[motor].getReady() or ! servos[motor].getPinUp() ) ) {

#ifdef debug
        Serial.print("=================================================  speed 90 up limit final ");
        Serial.println(motor);
      if ( servos[motor].getPinUp() ) {
        Serial.print("=================================================  HIGH ");
        Serial.println();
      } else {
        Serial.print("=================================================  LOW ");
        Serial.println();
      }
#endif 

      servos[motor].setState(0);
      servos[motor].selectSpeed(90);
      servos[motor].setReady(false);

//      servos[motor].setContador(0);

    } else {

#ifdef debug
        Serial.print("=================================================  speed 90 end Other ");
        Serial.println(motor);
      if ( servos[motor].getPinUp() ) {
        Serial.print("=================================================  HIGH ");
        Serial.println();
      } else {
        Serial.print("=================================================  LOW ");
        Serial.println();
      }
#endif
      
      servos[motor].setState(0);
      servos[motor].selectSpeed(90);
      servos[motor].setReady(false);
    }

#ifdef debug
      Serial.print("UP End Motor : ");
      Serial.print(motor);
      Serial.print("  state: ");
      Serial.print(servos[motor].getState());
      Serial.print("  Ready: ");
      Serial.print(servos[motor].getReady());
      Serial.print("  ser: ");
      Serial.print(servos[motor].getPinUp());
      Serial.print("  speed: ");
      Serial.println(servos[motor].getSpeed());
      Serial.println("^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^");
#endif
  }
  // check if all the motor have been stopped
  int total = 0;
  for (int motor = servoIni; motor < servoNumber; motor++) {
    total = total + servos[motor].getState();
  }
  if ( total == 0 ) {
    genState = 0;
    notStopped = false;
    for (int motor = servoIni; motor < servoNumber; motor++) {
      servos[motor].setReady(true);
      servos[motor].setCountCurrent(0);
      servos[motor].setState(0);
      servos[motor].selectSpeed(90);

#ifdef debug
        Serial.print("Up end. Set Ready Motor : ");
        Serial.println (motor);
#endif

    }
  }
}

void down () {
  notStopped = true;
  for (int motor = servoIni; motor < servoNumber; motor++) {

    if ( servos[motor].getState() == 0 and servos[motor].getReady() ) {

#ifdef debug
        Serial.print("=================================================  speed 180 ");
        Serial.println(motor);
#endif

      servos[motor].setState(20) ;
      servos[motor].selectSpeed(180);
      servos[motor].setCountCurrent(0);
    } else if ( servos[motor].getState() == 20  and servos[motor].getReady() ) {

#ifdef debug
        Serial.print("=================================================  speed 180 cont ");
        Serial.println(servos[motor].getCountCurrent() );
#endif

      servos[motor].setState(20) ;
      servos[motor].setCountCurrent(servos[motor].getCountCurrent() + 1);
      if ( servos[motor].getCountCurrent() > servos[motor].getCountDown() ) {
        servos[motor].setReady(false);
      }
    } else if ( servos[motor].getState() == 20  and ( ! servos[motor].getReady() ) ) {

#ifdef debug
        Serial.print("=================================================  speed 90 End count");
        Serial.println(motor);
#endif

      servos[motor].setState(0);
      servos[motor].selectSpeed(90);
      servos[motor].setReady(false);
    } else {

#ifdef debug
        Serial.print("=================================================  speed 90 Other");
        Serial.println(motor);
#endif

      servos[motor].setState(0);
      servos[motor].selectSpeed(90);
      servos[motor].setReady(false);
    }

#ifdef debug
      Serial.print("Motor : ");
      Serial.print(motor);
      Serial.print("  state: ");
      Serial.print(servos[motor].getState());
      Serial.print("  Ready: ");
      Serial.print(servos[motor].getReady());
      Serial.print("  Ser: ");
      Serial.print(servos[motor].getPinUp());
      Serial.print("  speed: ");
      Serial.println(servos[motor].getSpeed());
#endif

  }
  // check if all the motor have bee stopped
  int total = 0;
  for (int motor = servoIni; motor < servoNumber; motor++) {
    total = total + servos[motor].getState();
  }
  if ( total == 0 ) {
    genState = 0;
    notStopped = true;
    for (int motor = servoIni; motor < servoNumber; motor++) {
      servos[motor].setState(0);
      servos[motor].selectSpeed(90);
      servos[motor].setCountCurrent(0);
      servos[motor].setReady(true);

#ifdef debug
        Serial.print("Down end. Set Ready Motor : ");
        Serial.println (motor);
#endif

    }
  }
}

void stop() {
  for (int motor = servoIni; motor < servoNumber; motor++) {

#ifdef debug
        stateChanged &&   Serial.print("Stop Motor : ");
        stateChanged &&   Serial.print(motor);
        stateChanged &&   Serial.print("  state: ");
        stateChanged &&   Serial.print(servos[motor].getState());
        stateChanged &&   Serial.print("  Ready: ");
        stateChanged &&   Serial.print(servos[motor].getReady());
        stateChanged &&   Serial.print("  Ser: ");
        stateChanged &&   Serial.print(servos[motor].getPinUp());
        stateChanged &&   Serial.print("  speed: ");
        stateChanged &&   Serial.println(servos[motor].getSpeed());
#endif

    servos[motor].setState(0);
    servos[motor].selectSpeed(90);
    servos[motor].setCountCurrent(0);
    servos[motor].setReady(true);
  }
  notStopped = false;

}

void emergency () {

#ifdef debug
    Serial.print("====================================================================");
    Serial.print("");
    Serial.print("            Emergency State");
    Serial.print("");
    Serial.print("====================================================================");
#endif 

  for (int motor = servoIni; motor < servoNumber; motor++) {
    servos[motor].setReady(false);
  }
  emergencyState = true;
  motor = 0;
  
//  debug = 1;
  display = 1; 
}

void setup() {
  Serial.begin(1000000);
  delay(100);
  display && Serial.println("Inicializacion");
  //  while (!Serial){delay(100)};
  //  pinMode(TriggerPinA,OUTPUT);
  //  pinMode(EchoPinA,INPUT);

  // Distance axes init configuration

  delay (1000);
  display && Serial.println("Distance Axes Initialized");
  // put your setup code here, to run once:

  // servo motor init configuration

  servos[0].init( 2, 23, true,   upCount, downCount);
  servos[1].init( 3, 22, false , upCount, downCount);

  servos[2].init( 4, 25, false,  upCount, downCount); // 30, 28
  servos[3].init( 5, 24, true ,  upCount, downCount);

  servos[4].init( 6, 27, true ,  upCount, downCount);
  servos[5].init( 7, 26, false,  upCount, downCount);

  servos[6].init( 8, 29, true ,  upCount, downCount);
  servos[7].init( 9, 28, false,  upCount, downCount);

  servos[8].init(10, 31, true ,  upCount, downCount);
  servos[9].init(11, 30, false,  upCount, downCount);

  servos[10].init(12, 33, false, upCount, downCount);
  servos[11].init(13, 32, true , upCount, downCount);

/*
  servos[0].init( 2, 22, true,   upCount, downCount);
  servos[1].init( 3, 23, false , upCount, downCount);

  servos[2].init( 4, 24, false,  upCount, downCount); // 30, 28
  servos[3].init( 5, 25, true ,  upCount, downCount);

  servos[4].init( 6, 26, true ,  upCount, downCount);
  servos[5].init( 7, 27, false,  upCount, downCount);

  servos[6].init( 8, 28, true ,  upCount, downCount);
  servos[7].init( 9, 29, false,  upCount, downCount);

  servos[8].init(10, 30, true ,  upCount, downCount);
  servos[9].init(11, 31, false,  upCount, downCount);

  servos[10].init(12, 32, false, upCount, downCount);
  servos[11].init(13, 33, true , upCount, downCount);
*/

  delay (1000);
  display && Serial.println("Servo Motor Initialized");

  while (true) {
    char in_char;
    int inByte;

    if (Serial.available() > 0 ) {
      inByte = Serial.read();
      while ( Serial.available() ) {
        Serial.read();
      };
      //    in_char = Serial.read();
    }
    //   char in_char='s';
    stateChanged &&  Serial.println ("=============================================================================");
    stateChanged &&  Serial.println ("=============================================================================");
    stateChanged &&  Serial.println ("");
    stateChanged &&  Serial.print ("=================================  ");
    stateChanged &&  Serial.print ("Leido: ");
    stateChanged &&  Serial.println (inByte);
    //  Serial.println(in_char);
    if ( inByte == 's' ) {
      genState = 0;
      test = 0;

    } else if ( inByte == 'c' ) {  // go up
      genState = 10;
      test = 0;

    } else if ( inByte == 'u' ) {  // go down
      genState = 20;
      test = 0;

    } else if ( inByte == 'e' ) {  // Emergency motor Stop
      motor = 0;
      test = 0;

    } else if ( inByte == 'n' ) {  // normal motor state
      motor = 1;
      test = 0;

    } else if ( inByte == 't' ) {  // test only distance
      test = 1;

    } else if ( inByte == 'd' ) {  // test only distance
      if (display == 0) {
        display = 1;
      } else {
        display = 0;
      }

    }
    inByte = 1;
    if ( previousState != genState ) {
      previousState = genState;
      stateChanged = true;
    } else {
      stateChanged = false;
    }
    stateChanged && display && Serial.print ("Display: ");
    stateChanged && display && Serial.print (display);
    stateChanged && display && Serial.print ("  test: ");
    stateChanged && display && Serial.print (test);
    stateChanged && display && Serial.print (" Motor:  ");
    stateChanged && display && Serial.println (motor);

    if ( test == 1 ) {
      display && Serial.println("===============================================");
      delay(1000);
      for (int motor = servoIni; motor < servoNumber; motor++) {

      Serial.print("Motor : ");
      Serial.print(motor);
      Serial.print("  state: ");
      Serial.print(servos[motor].getState());
      Serial.print("  Ready: ");
      Serial.print(servos[motor].getReady());
      Serial.print("  speed: ");
      Serial.print(servos[motor].getSpeed());
      Serial.print("  ser: ");
      Serial.println(servos[motor].getPinUp());
       
      }
    } else {

      //delay (10) ;
      stateChanged && display && Serial.println("===========================================================================================================");
      stateChanged && display && Serial.print("gen State: ");
      stateChanged && display && Serial.print(genState);
      stateChanged && display && Serial.println(" ");

      if ( genState == 10 ) { // go up
        notStopped = true;
        up();
      } else if ( genState == 20 ) {  // go down
        notStopped = true;
        down();
      } else {  // stop all motors
        notStopped = false;
        stop();
      }

      delay(retardoStep);
    }
  }
}

void loop() {

}

(Code tags added by Moderator. #statements apparently not handled too well in code tags.)

when you compile have you some warning or error

Any error and no advice about excess of memory use:

Sketch uses 8718 bytes (3%) of program storage space. Maximum is 253952 bytes.
Global variables use 1969 bytes (24%) of dynamic memory, leaving 6223 bytes for local variables. Maximum is 8192 bytes.

Hi, @msdevega
Welcome to the forum.

Please read the post at the start of any forum , entitled “How to use this Forum”.

To add code please click this link;

Please read it will help you to post your code. :+1:

Thanks… Tom… :grinning: :+1: :coffee: :australia:

I will do. Thanks for the advice.

you have nothing in your loop()

Where did you called your fonction()…

The code in correct forum format


#include  <Servo.h>

#define debug

int display = 1;

int upCount = 10;

#ifdef debug
int downCount = 10;
#else
int downCount = 500;
#endif
boolean emergencyState = false;

int motor = 0;

int test = 1;

int up_down = 0;
int up_down_max = 2;



// value to set axes position
//int retardoStep = 100;
//int countMax = 1;
// value to test axes movement
int retardoStep = 1;

int servoIni = 4;  //0-1, 2-3, 4-5, 6-7, 8-9, 10-11/
int servoNumber = 6;


boolean notStopped = true;
boolean stateChanged = true;
int previousState = 0;

int genState = 0;


class ServoControl {

  private :
    Servo servo;
    int pinPwm;
    int pinUp;
    boolean tipo; // true cw=cw,ucw=ucw   false cw=ucw, ucw=cw
    int cwN = 0;
    int ucwN = 180;
    int state = 0;
    int speed = 90;
    int countUp;
    int countDown;
    int countCurrent;
    boolean ready = true;
    
    int contador = 0;

  public:
    ServoControl() {}

    void init (int pinPwm, int pinUp, boolean tipo, int countUp, int countDown) {
      servo.attach(pinPwm);
      this->pinPwm = pinPwm;
      this->pinUp = pinUp;
      this->tipo = tipo;
      this->countUp = countUp;
      this->countDown = countDown;
      countCurrent = 0;
      state = 0;
      speed = 90;
      contador = 0;
      pinMode(pinUp, OUTPUT);
      if ( !tipo ) {
        cwN = 180;
        ucwN = 0;
      }
    }


    void cw() {
      if (motor and !emergencyState) {
        servo.write(cwN);
      }
    }


    void ucw() {
      if (motor and !emergencyState) {
        servo.write(ucwN);
      }
    }


    void selectSpeed(int speed) {
      int speedApplied;
      if (!tipo) {
        speedApplied = 180 - speed;
      } else {
        speedApplied = speed;
      }
      if (motor and !emergencyState) {
        servo.write(speedApplied);
      }
      this->speed = speed;
    }

    void stop() {
      if (motor) {
        servo.write(90);
      }
    }

    void setState(int state) {
      this->state = state;
    }
    int getState() {
      return (state);
    }

    int getCountUp() {
      return (countUp);
    }
    int getCountDown() {
      return (countDown);
    }

    int getCountCurrent () {
      return (countCurrent);
    }

    void setCountCurrent (int countCurrent) {
      this->countCurrent = countCurrent;
    }

    int getSpeed() {
      return (speed);
    }

    void setReady (boolean ready) {
      this->ready = ready;
    }
    boolean getReady () {
      return (ready);
    }

    void setContador (int contador) {
      this->contador = contador;
    }
    int getContador () {
      return(contador);
    }

    boolean getPinUp() {
 /*
      if (contador > 3){
        return(false);        
      } 
*/
      if ( digitalRead(pinUp) == HIGH ) {
        return (true);
      } else {
        return (false);
      }
    }

};








ServoControl servos[] = { ServoControl(), ServoControl(), ServoControl(), ServoControl(), ServoControl(), ServoControl(),
                          ServoControl(), ServoControl(), ServoControl(), ServoControl(), ServoControl(), ServoControl()
                        };



void up () {
  notStopped = true;
  for (int motor = servoIni; motor < servoNumber; motor++) {

#ifdef debug
    Serial.print("UP Ini Motor : ");
    Serial.print(motor);
    Serial.print("  state: ");
    Serial.print(servos[motor].getState());
      Serial.print("  Ready: ");
      Serial.print(servos[motor].getReady());
      Serial.print("  ser: ");
      Serial.print(servos[motor].getPinUp());
      Serial.print("  contador: ");
      Serial.print(servos[motor].getContador());
      Serial.print("  speed: ");
      Serial.println(servos[motor].getSpeed());
#endif

    if ( servos[motor].getState() == 0 and ( ! (servos[motor].getPinUp() or ! servos[motor].getReady() or emergencyState ) ) ) {

#ifdef debug
      Serial.print("  state: ");
      Serial.print(servos[motor].getState());
      Serial.print("  ser: ");
      Serial.print(servos[motor].getPinUp());
      Serial.print("  Ready: ");
      Serial.print(servos[motor].getReady());
      Serial.print("  EmergencySatet : ");
      Serial.println(emergencyState);
        Serial.print("=================================================  speed 90 up limit inicial ");
        Serial.println(motor);
      if ( servos[motor].getPinUp() ) {
        Serial.print("=================================================  HIGH ");
        Serial.println();
      } else {
        Serial.print("=================================================  LOW ");
        Serial.println();
      }
#endif

      servos[motor].setState(0);
      servos[motor].selectSpeed(90);
      servos[motor].setReady(false);
      servos[motor].setCountCurrent(0);

    } else if ( servos[motor].getState() == 0 and servos[motor].getReady() and servos[motor].getPinUp() ) {
      servos[motor].setState(10);
      servos[motor].selectSpeed(0);
      servos[motor].setCountCurrent(0);
#ifdef debug

        Serial.print("=================================================  speed 0 ");
        Serial.println(motor);
      if ( servos[motor].getPinUp() ) {
        Serial.print("=================================================  HIGH ");
        Serial.println();
      } else {
        Serial.print("=================================================  LOW ");
        Serial.println();
      }
#endif

//      servos[motor].setContador(0);
      

    } else if ( servos[motor].getState() == 10 and servos[motor].getReady() and servos[motor].getPinUp()  ) {
      servos[motor].setCountCurrent(servos[motor].getCountCurrent() + 1);
      servos[motor].setState(10);

#ifdef debug
        Serial.print("=================================================  speed 0 cont ");
        Serial.println(servos[motor].getCountCurrent() );
      if ( servos[motor].getCountCurrent() > servos[motor].getCountUp() ) {
        servos[motor].setReady(false);
        Serial.print("=================================================  speed 0 cont max reached ");
        Serial.println(servos[motor].getCountCurrent() );
      }
#endif
//      servos[motor].setContador(servos[motor].getContador() + 1);

    
    } else if ( servos[motor].getState() == 10 and ( ! servos[motor].getReady() or ! servos[motor].getPinUp() ) ) {

#ifdef debug
        Serial.print("=================================================  speed 90 up limit final ");
        Serial.println(motor);
      if ( servos[motor].getPinUp() ) {
        Serial.print("=================================================  HIGH ");
        Serial.println();
      } else {
        Serial.print("=================================================  LOW ");
        Serial.println();
      }
#endif 

      servos[motor].setState(0);
      servos[motor].selectSpeed(90);
      servos[motor].setReady(false);

//      servos[motor].setContador(0);
    
    
    
    } else {

#ifdef debug
        Serial.print("=================================================  speed 90 end Other ");
        Serial.println(motor);
      if ( servos[motor].getPinUp() ) {
        Serial.print("=================================================  HIGH ");
        Serial.println();
      } else {
        Serial.print("=================================================  LOW ");
        Serial.println();
      }
#endif
      
      servos[motor].setState(0);
      servos[motor].selectSpeed(90);
      servos[motor].setReady(false);
    }

#ifdef debug
      Serial.print("UP End Motor : ");
      Serial.print(motor);
      Serial.print("  state: ");
      Serial.print(servos[motor].getState());
      Serial.print("  Ready: ");
      Serial.print(servos[motor].getReady());
      Serial.print("  ser: ");
      Serial.print(servos[motor].getPinUp());
      Serial.print("  speed: ");
      Serial.println(servos[motor].getSpeed());
      Serial.println("^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^");
#endif
  }
  // check if all the motor have been stopped
  int total = 0;
  for (int motor = servoIni; motor < servoNumber; motor++) {
    total = total + servos[motor].getState();
  }
  if ( total == 0 ) {
    genState = 0;
    notStopped = false;
    for (int motor = servoIni; motor < servoNumber; motor++) {
      servos[motor].setReady(true);
      servos[motor].setCountCurrent(0);
      servos[motor].setState(0);
      servos[motor].selectSpeed(90);

#ifdef debug
        Serial.print("Up end. Set Ready Motor : ");
        Serial.println (motor);
#endif

    }
  }
}


void down () {
  notStopped = true;
  for (int motor = servoIni; motor < servoNumber; motor++) {




    if ( servos[motor].getState() == 0 and servos[motor].getReady() ) {

#ifdef debug
        Serial.print("=================================================  speed 180 ");
        Serial.println(motor);
#endif

      servos[motor].setState(20) ;
      servos[motor].selectSpeed(180);
      servos[motor].setCountCurrent(0);
    } else if ( servos[motor].getState() == 20  and servos[motor].getReady() ) {

#ifdef debug
        Serial.print("=================================================  speed 180 cont ");
        Serial.println(servos[motor].getCountCurrent() );
#endif

      servos[motor].setState(20) ;
      servos[motor].setCountCurrent(servos[motor].getCountCurrent() + 1);
      if ( servos[motor].getCountCurrent() > servos[motor].getCountDown() ) {
        servos[motor].setReady(false);
      }
    } else if ( servos[motor].getState() == 20  and ( ! servos[motor].getReady() ) ) {

#ifdef debug
        Serial.print("=================================================  speed 90 End count");
        Serial.println(motor);
#endif

      servos[motor].setState(0);
      servos[motor].selectSpeed(90);
      servos[motor].setReady(false);
    } else {

#ifdef debug
        Serial.print("=================================================  speed 90 Other");
        Serial.println(motor);
#endif

      servos[motor].setState(0);
      servos[motor].selectSpeed(90);
      servos[motor].setReady(false);
    }

#ifdef debug
      Serial.print("Motor : ");
      Serial.print(motor);
      Serial.print("  state: ");
      Serial.print(servos[motor].getState());
      Serial.print("  Ready: ");
      Serial.print(servos[motor].getReady());
      Serial.print("  Ser: ");
      Serial.print(servos[motor].getPinUp());
      Serial.print("  speed: ");
      Serial.println(servos[motor].getSpeed());
#endif

  }
  // check if all the motor have bee stopped
  int total = 0;
  for (int motor = servoIni; motor < servoNumber; motor++) {
    total = total + servos[motor].getState();
  }
  if ( total == 0 ) {
    genState = 0;
    notStopped = true;
    for (int motor = servoIni; motor < servoNumber; motor++) {
      servos[motor].setState(0);
      servos[motor].selectSpeed(90);
      servos[motor].setCountCurrent(0);
      servos[motor].setReady(true);

#ifdef debug
        Serial.print("Down end. Set Ready Motor : ");
        Serial.println (motor);
#endif

    }
  }
}

void stop() {
  for (int motor = servoIni; motor < servoNumber; motor++) {

#ifdef debug
        stateChanged &&   Serial.print("Stop Motor : ");
        stateChanged &&   Serial.print(motor);
        stateChanged &&   Serial.print("  state: ");
        stateChanged &&   Serial.print(servos[motor].getState());
        stateChanged &&   Serial.print("  Ready: ");
        stateChanged &&   Serial.print(servos[motor].getReady());
        stateChanged &&   Serial.print("  Ser: ");
        stateChanged &&   Serial.print(servos[motor].getPinUp());
        stateChanged &&   Serial.print("  speed: ");
        stateChanged &&   Serial.println(servos[motor].getSpeed());
#endif

    servos[motor].setState(0);
    servos[motor].selectSpeed(90);
    servos[motor].setCountCurrent(0);
    servos[motor].setReady(true);
  }
  notStopped = false;

}

void emergency () {

#ifdef debug
    Serial.print("====================================================================");
    Serial.print("");
    Serial.print("            Emergency State");
    Serial.print("");
    Serial.print("====================================================================");
#endif 

  for (int motor = servoIni; motor < servoNumber; motor++) {
    servos[motor].setReady(false);
  }
  emergencyState = true;
  motor = 0;
  
//  debug = 1;
  display = 1; 

}


void setup() {
  Serial.begin(57600);
  delay(100);
  display && Serial.println("Inicializacion");
  //  while (!Serial){delay(100)};
  //  pinMode(TriggerPinA,OUTPUT);
  //  pinMode(EchoPinA,INPUT);


  // Distance axes init configuration



  delay (1000);
  display && Serial.println("Distance Axes Initialized");
  // put your setup code here, to run once:

  // servo motor init configuration

  servos[0].init( 2, 23, false,   upCount, downCount);
  servos[1].init( 3, 22, true , upCount, downCount);

  servos[2].init( 4, 25, false,  upCount, downCount); // 30, 28
  servos[3].init( 5, 24, true ,  upCount, downCount);

  servos[4].init( 6, 27, false ,  upCount, downCount);
  servos[5].init( 7, 26, true,  upCount, downCount);

  servos[6].init( 8, 29, true ,  upCount, downCount);
  servos[7].init( 9, 28, false,  upCount, downCount);

  servos[8].init(10, 31, true ,  upCount, downCount);
  servos[9].init(11, 30, false,  upCount, downCount);

  servos[10].init(12, 33, false, upCount, downCount);
  servos[11].init(13, 32, true , upCount, downCount);

/*
  servos[0].init( 2, 22, true,   upCount, downCount);
  servos[1].init( 3, 23, false , upCount, downCount);

  servos[2].init( 4, 24, false,  upCount, downCount); // 30, 28
  servos[3].init( 5, 25, true ,  upCount, downCount);

  servos[4].init( 6, 26, true ,  upCount, downCount);
  servos[5].init( 7, 27, false,  upCount, downCount);

  servos[6].init( 8, 28, true ,  upCount, downCount);
  servos[7].init( 9, 29, false,  upCount, downCount);

  servos[8].init(10, 30, true ,  upCount, downCount);
  servos[9].init(11, 31, false,  upCount, downCount);

  servos[10].init(12, 32, false, upCount, downCount);
  servos[11].init(13, 33, true , upCount, downCount);
*/

  delay (1000);
  display && Serial.println("Servo Motor Initialized");






  while (true) {
    char in_char;
    int inByte;



    if (Serial.available() > 0 ) {
      inByte = Serial.read();
      while ( Serial.available() ) {
        Serial.read();
      };
      //    in_char = Serial.read();
    }
    //   char in_char='s';
    stateChanged &&  Serial.println ("=============================================================================");
    stateChanged &&  Serial.println ("=============================================================================");
    stateChanged &&  Serial.println ("");
    stateChanged &&  Serial.print ("=================================  ");
    stateChanged &&  Serial.print ("Leido: ");
    stateChanged &&  Serial.println (inByte);
    //  Serial.println(in_char);
    if ( inByte == 's' ) {
      genState = 0;
      test = 0;

    } else if ( inByte == 'c' ) {  // go up
      genState = 10;
      test = 0;

    } else if ( inByte == 'u' ) {  // go down
      genState = 20;
      test = 0;

    } else if ( inByte == 'e' ) {  // Emergency motor Stop
      motor = 0;
      test = 0;

    } else if ( inByte == 'n' ) {  // normal motor state
      motor = 1;
      test = 0;

    } else if ( inByte == 't' ) {  // test only distance
      test = 1;

    } else if ( inByte == 'd' ) {  // test only distance
      if (display == 0) {
        display = 1;
      } else {
        display = 0;
      }

    }
    inByte = 1;
    if ( previousState != genState ) {
      previousState = genState;
      stateChanged = true;
    } else {
      stateChanged = false;
    }
    stateChanged && display && Serial.print ("Display: ");
    stateChanged && display && Serial.print (display);
    stateChanged && display && Serial.print ("  test: ");
    stateChanged && display && Serial.print (test);
    stateChanged && display && Serial.print (" Motor:  ");
    stateChanged && display && Serial.println (motor);

    if ( test == 1 ) {


      display && Serial.println("===============================================");
      delay(1000);
      for (int motor = servoIni; motor < servoNumber; motor++) {

      Serial.print("Motor : ");
      Serial.print(motor);
      Serial.print("  state: ");
      Serial.print(servos[motor].getState());
      Serial.print("  Ready: ");
      Serial.print(servos[motor].getReady());
      Serial.print("  speed: ");
      Serial.print(servos[motor].getSpeed());
      Serial.print("  ser: ");
      Serial.println(servos[motor].getPinUp());
       
      }

    } else {



      //delay (10) ;
      stateChanged && display && Serial.println("===========================================================================================================");
      stateChanged && display && Serial.print("gen State: ");
      stateChanged && display && Serial.print(genState);
      stateChanged && display && Serial.println(" ");



      if ( genState == 10 ) { // go up
        notStopped = true;
        up();
      } else if ( genState == 20 ) {  // go down
        notStopped = true;
        down();
      } else {  // stop all motors
        notStopped = false;
        stop();


      }

      delay(retardoStep);

    }

  }


}


void loop() {



}

This a test release a

while ( true ) {
...

in the setup section do the job for now.

Using the F() macro when printing string constants can save a lot of RAM (“dynamic memory”). You say you are using “1969 bytes (24%) of dynamic memory” but when I add the F() macros it goes down to 536 bytes and easily fits on an UNO.

#include <Servo.h>

#define debug

int display = 1;

int upCount = 10;

#ifdef debug
int downCount = 10;
#else
int downCount = 500;
#endif
boolean emergencyState = false;

int motor = 0;

int test = 1;

int up_down = 0;
int up_down_max = 2;

// value to set axes position
//int retardoStep = 100;
//int countMax = 1;
// value to test axes movement
int retardoStep = 1;

const int servoIni = 0; //0-1, 2-3, 4-5, 6-7, 8-9, 10-11/
const int servoNumber = 12;

boolean notStopped = true;
boolean stateChanged = true;
int previousState = 0;

int genState = 0;

class ServoControl
{

  private :
    Servo servo;
    int pinPwm;
    int pinUp;
    boolean tipo; // true cw=cw,ucw=ucw false cw=ucw, ucw=cw
    int cwN = 0;
    int ucwN = 180;
    int state = 0;
    int speed = 90;
    int countUp;
    int countDown;
    int countCurrent;
    boolean ready = true;
    int contador = 0;

  public:
    ServoControl() {}

    void init (int pinPwm, int pinUp, boolean tipo, int countUp, int countDown)
    {
      servo.attach(pinPwm);
      this->pinPwm = pinPwm;
      this->pinUp = pinUp;
      this->tipo = tipo;
      this->countUp = countUp;
      this->countDown = countDown;
      countCurrent = 0;
      state = 0;
      speed = 90;
      contador = 0;
      pinMode(pinUp, OUTPUT);
      if ( !tipo )
      {
        cwN = 180;
        ucwN = 0;
      }
    }


    void cw()
    {
      if (motor and !emergencyState)
      {
        servo.write(cwN);
      }
    }


    void ucw()
    {
      if (motor and !emergencyState)
      {
        servo.write(ucwN);
      }
    }


    void selectSpeed(int speed)
    {
      int speedApplied;
      if (!tipo)
      {
        speedApplied = 180 - speed;
      }
      else
      {
        speedApplied = speed;
      }
      if (motor and !emergencyState)
      {
        servo.write(speedApplied);
      }
      this->speed = speed;
    }

    void stop()
    {
      if (motor)
      {
        servo.write(90);
      }
    }

    void setState(int state)
    {
      this->state = state;
    }

    int getState()
    {
      return (state);
    }

    int getCountUp()
    {
      return (countUp);
    }
    int getCountDown()
    {
      return (countDown);
    }

    int getCountCurrent ()
    {
      return (countCurrent);
    }

    void setCountCurrent (int countCurrent)
    {
      this->countCurrent = countCurrent;
    }

    int getSpeed()
    {
      return (speed);
    }

    void setReady (boolean ready)
    {
      this->ready = ready;
    }
    boolean getReady ()
    {
      return (ready);
    }

    void setContador (int contador)
    {
      this->contador = contador;
    }
    int getContador ()
    {
      return (contador);
    }

    boolean getPinUp()
    {
      /*
        if (contador > 3){
        return(false);
        }
      */
      if ( digitalRead(pinUp) == HIGH )
      {
        return (true);
      }
      else
      {
        return (false);
      }
    }

};

ServoControl servos[12] =
{
  ServoControl(), ServoControl(), ServoControl(), ServoControl(),
  ServoControl(), ServoControl(), ServoControl(), ServoControl(),
  ServoControl(), ServoControl(), ServoControl(), ServoControl()
};

void up ()
{
  notStopped = true;
  for (int motor = servoIni; motor < servoNumber; motor++)
  {
#ifdef debug
    Serial.print(F("UP Ini Motor : "));
    Serial.print(motor);
    Serial.print(F(" state: "));
    Serial.print(servos[motor].getState());
    Serial.print(F(" Ready: "));
    Serial.print(servos[motor].getReady());
    Serial.print(F(" ser: "));
    Serial.print(servos[motor].getPinUp());
    Serial.print(F(" contador: "));
    Serial.print(servos[motor].getContador());
    Serial.print(F(" speed: "));
    Serial.println(servos[motor].getSpeed());
#endif

    if ( servos[motor].getState() == 0 and ( ! (servos[motor].getPinUp() or ! servos[motor].getReady() or emergencyState ) ) )
    {
#ifdef debug
      Serial.print(F(" state: "));
      Serial.print(servos[motor].getState());
      Serial.print(F(" ser: "));
      Serial.print(servos[motor].getPinUp());
      Serial.print(F(" Ready: "));
      Serial.print(servos[motor].getReady());
      Serial.print(F(" EmergencySatet : "));
      Serial.println(emergencyState);
      Serial.print(F("================================================= speed 90 up limit inicial "));
      Serial.println(motor);
      if ( servos[motor].getPinUp() )
      {
        Serial.print(F("================================================= HIGH "));
        Serial.println();
      }
      else
      {
        Serial.print(F("================================================= LOW "));
        Serial.println();
      }
#endif

      servos[motor].setState(0);
      servos[motor].selectSpeed(90);
      servos[motor].setReady(false);
      servos[motor].setCountCurrent(0);

    }
    else if ( servos[motor].getState() == 0 and servos[motor].getReady() and servos[motor].getPinUp() )
    {
      servos[motor].setState(10);
      servos[motor].selectSpeed(0);
      servos[motor].setCountCurrent(0);
#ifdef debug

      Serial.print(F("=================================================  speed 0 "));
      Serial.println(motor);
      if ( servos[motor].getPinUp() )
      {
        Serial.print(F("=================================================  HIGH "));
        Serial.println();
      }
      else
      {
        Serial.print(F("=================================================  LOW "));
        Serial.println();
      }
#endif

      // servos[motor].setContador(0);

    }
    else if ( servos[motor].getState() == 10 and servos[motor].getReady() and servos[motor].getPinUp()  )
    {
      servos[motor].setCountCurrent(servos[motor].getCountCurrent() + 1);
      servos[motor].setState(10);
#ifdef debug
      Serial.print(F("================================================= speed 0 cont "));
      Serial.println(servos[motor].getCountCurrent() );
      if ( servos[motor].getCountCurrent() > servos[motor].getCountUp() )
      {
        servos[motor].setReady(false);
        Serial.print(F("================================================= speed 0 cont max reached "));
        Serial.println(servos[motor].getCountCurrent() );
      }
#endif
      // servos[motor].setContador(servos[motor].getContador() + 1);

    }
    else if ( servos[motor].getState() == 10 and ( ! servos[motor].getReady() or ! servos[motor].getPinUp() ) )
    {
#ifdef debug
      Serial.print(F("================================================= speed 90 up limit final "));
      Serial.println(motor);
      if ( servos[motor].getPinUp() )
      {
        Serial.print(F("================================================= HIGH "));
        Serial.println();
      }
      else
      {
        Serial.print(F("================================================= LOW "));
        Serial.println();
      }
#endif

      servos[motor].setState(0);
      servos[motor].selectSpeed(90);
      servos[motor].setReady(false);
      // servos[motor].setContador(0);

    }
    else
    {
#ifdef debug
      Serial.print(F("================================================= speed 90 end Other "));
      Serial.println(motor);
      if ( servos[motor].getPinUp() )
      {
        Serial.print(F("================================================= HIGH "));
        Serial.println();
      }
      else
      {
        Serial.print(F("================================================= LOW "));
        Serial.println();
      }
#endif

      servos[motor].setState(0);
      servos[motor].selectSpeed(90);
      servos[motor].setReady(false);
    }
#ifdef debug
    Serial.print(F("UP End Motor : "));
    Serial.print(motor);
    Serial.print(F(" state: "));
    Serial.print(servos[motor].getState());
    Serial.print(F(" Ready: "));
    Serial.print(servos[motor].getReady());
    Serial.print(F(" ser: "));
    Serial.print(servos[motor].getPinUp());
    Serial.print(F(" speed: "));
    Serial.println(servos[motor].getSpeed());
    Serial.println(F("^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^"));
#endif
  }
  
  // check if all the motor have been stopped
  int total = 0;
  for (int motor = servoIni; motor < servoNumber; motor++)
  {
    total = total + servos[motor].getState();
  }
  
  if ( total == 0 )  // All servos are in state 0
  {
    genState = 0;
    notStopped = false;
    for (int motor = servoIni; motor < servoNumber; motor++)
    {
      servos[motor].setReady(true);
      servos[motor].setCountCurrent(0);
      servos[motor].setState(0);
      servos[motor].selectSpeed(90);

#ifdef debug
      Serial.print(F("Up end. Set Ready Motor : "));
      Serial.println (motor);
#endif

    }
  }
}

void down ()
{
  notStopped = true;
  for (int motor = servoIni; motor < servoNumber; motor++)
  {

    if ( servos[motor].getState() == 0 and servos[motor].getReady() )
    {
#ifdef debug
      Serial.print(F("================================================= speed 180 "));
      Serial.println(motor);
#endif

      servos[motor].setState(20) ;
      servos[motor].selectSpeed(180);
      servos[motor].setCountCurrent(0);
    }
    else if ( servos[motor].getState() == 20  and servos[motor].getReady() )
    {
#ifdef debug
      Serial.print(F("================================================= speed 180 cont "));
      Serial.println(servos[motor].getCountCurrent() );
#endif

      servos[motor].setState(20) ;
      servos[motor].setCountCurrent(servos[motor].getCountCurrent() + 1);
      if ( servos[motor].getCountCurrent() > servos[motor].getCountDown() )
      {
        servos[motor].setReady(false);
      }
    }
    else if ( servos[motor].getState() == 20  and ( ! servos[motor].getReady() ) )
    {
#ifdef debug
      Serial.print(F("================================================= speed 90 End count"));
      Serial.println(motor);
#endif

      servos[motor].setState(0);
      servos[motor].selectSpeed(90);
      servos[motor].setReady(false);
    }
    else
    {
#ifdef debug
      Serial.print(F("================================================= speed 90 Other"));
      Serial.println(motor);
#endif

      servos[motor].setState(0);
      servos[motor].selectSpeed(90);
      servos[motor].setReady(false);
    }
#ifdef debug
    Serial.print(F("Motor : "));
    Serial.print(motor);
    Serial.print(F(" state: "));
    Serial.print(servos[motor].getState());
    Serial.print(F(" Ready: "));
    Serial.print(servos[motor].getReady());
    Serial.print(F(" Ser: "));
    Serial.print(servos[motor].getPinUp());
    Serial.print(F(" speed: "));
    Serial.println(servos[motor].getSpeed());
#endif

  }
  // check if all the motor have bee stopped
  int total = 0;
  for (int motor = servoIni; motor < servoNumber; motor++)
  {
    total = total + servos[motor].getState();
  }
  if ( total == 0 )
  {
    genState = 0;
    notStopped = true;
    for (int motor = servoIni; motor < servoNumber; motor++)
    {
      servos[motor].setState(0);
      servos[motor].selectSpeed(90);
      servos[motor].setCountCurrent(0);
      servos[motor].setReady(true);

#ifdef debug
      Serial.print(F("Down end. Set Ready Motor : "));
      Serial.println (motor);
#endif

    }
  }
}

void stop()
{
  for (int motor = servoIni; motor < servoNumber; motor++)
  {

#ifdef debug
    stateChanged && Serial.print(F("Stop Motor : "));
    stateChanged && Serial.print(motor);
    stateChanged && Serial.print(F(" state: "));
    stateChanged && Serial.print(servos[motor].getState());
    stateChanged && Serial.print(F(" Ready: "));
    stateChanged && Serial.print(servos[motor].getReady());
    stateChanged && Serial.print(F(" Ser: "));
    stateChanged && Serial.print(servos[motor].getPinUp());
    stateChanged && Serial.print(F(" speed: "));
    stateChanged && Serial.println(servos[motor].getSpeed());
#endif

    servos[motor].setState(0);
    servos[motor].selectSpeed(90);
    servos[motor].setCountCurrent(0);
    servos[motor].setReady(true);
  }
  notStopped = false;

}

void emergency ()
{

#ifdef debug
  Serial.print(F("===================================================================="));
  Serial.print(F(""));
  Serial.print(F(" Emergency State"));
  Serial.print(F(""));
  Serial.print(F("===================================================================="));
#endif

  for (int motor = servoIni; motor < servoNumber; motor++)
  {
    servos[motor].setReady(false);
  }
  emergencyState = true;
  motor = 0;

  // debug = 1;
  display = 1;

}

void setup()
{
  Serial.begin(1000000);
  delay(100);
  display && Serial.println(F("Inicializacion"));
  // while (!Serial){delay(100)};
  // pinMode(TriggerPinA,OUTPUT);
  // pinMode(EchoPinA,INPUT);

  // Distance axes init configuration

  delay (1000);
  display && Serial.println(F("Distance Axes Initialized"));
  // put your setup code here, to run once:

  // servo motor init configuration

  servos[0].init( 2, 23, true, upCount, downCount);
  servos[1].init( 3, 22, false , upCount, downCount);

  servos[2].init( 4, 25, false, upCount, downCount); // 30, 28
  servos[3].init( 5, 24, true , upCount, downCount);

  servos[4].init( 6, 27, true , upCount, downCount);
  servos[5].init( 7, 26, false, upCount, downCount);

  servos[6].init( 8, 29, true , upCount, downCount);
  servos[7].init( 9, 28, false, upCount, downCount);

  servos[8].init(10, 31, true , upCount, downCount);
  servos[9].init(11, 30, false, upCount, downCount);

  servos[10].init(12, 33, false, upCount, downCount);
  servos[11].init(13, 32, true , upCount, downCount);

  /*
    servos[0].init( 2, 22, true, upCount, downCount);
    servos[1].init( 3, 23, false , upCount, downCount);

    servos[2].init( 4, 24, false, upCount, downCount); // 30, 28
    servos[3].init( 5, 25, true , upCount, downCount);

    servos[4].init( 6, 26, true , upCount, downCount);
    servos[5].init( 7, 27, false, upCount, downCount);

    servos[6].init( 8, 28, true , upCount, downCount);
    servos[7].init( 9, 29, false, upCount, downCount);

    servos[8].init(10, 30, true , upCount, downCount);
    servos[9].init(11, 31, false, upCount, downCount);

    servos[10].init(12, 32, false, upCount, downCount);
    servos[11].init(13, 33, true , upCount, downCount);
  */

  delay (1000);
  display && Serial.println(F("Servo Motor Initialized"));

  while (true)
  {
    int inByte;

    if (Serial.available() > 0 )
    {
      inByte = Serial.read();
      while ( Serial.available() )
      {
        Serial.read();
      };
      //    in_char = Serial.read();
    }
    
    //   char in_char='s';
    stateChanged &&  Serial.println (F("============================================================================="));
    stateChanged &&  Serial.println (F("============================================================================="));
    stateChanged &&  Serial.println (F(""));
    stateChanged &&  Serial.print(F("=================================  "));
    stateChanged &&  Serial.print(F("Leido: "));
    stateChanged &&  Serial.println (inByte);
    //  Serial.println(in_char);
    if ( inByte == 's' )
    {
      genState = 0;
      test = 0;
    }
    else if ( inByte == 'c' )      // go up
    {
      genState = 10;
      test = 0;

    }
    else if ( inByte == 'u' )      // go down
    {
      genState = 20;
      test = 0;
    }
    else if ( inByte == 'e' )      // Emergency motor Stop
    {
      motor = 0;
      test = 0;
    }
    else if ( inByte == 'n' )      // normal motor state
    {
      motor = 1;
      test = 0;
    }
    else if ( inByte == 't' )      // test only distance
    {
      test = 1;
    }
    else if ( inByte == 'd' )      // test only distance
    {
      if (display == 0)
      {
        display = 1;
      }
      else
      {
        display = 0;
      }
    }
    inByte = 1;
    if ( previousState != genState )
    {
      previousState = genState;
      stateChanged = true;
    }
    else
    {
      stateChanged = false;
    }
    stateChanged && display && Serial.print(F("Display: "));
    stateChanged && display && Serial.print (display);
    stateChanged && display && Serial.print(F("  test: "));
    stateChanged && display && Serial.print (test);
    stateChanged && display && Serial.print(F(" Motor:  "));
    stateChanged && display && Serial.println (motor);

    if ( test == 1 )
    {
      display && Serial.println(F("==============================================="));
      delay(1000);
      for (int motor = servoIni; motor < servoNumber; motor++)
      {

        Serial.print(F("Motor : "));
        Serial.print(motor);
        Serial.print(F("  state: "));
        Serial.print(servos[motor].getState());
        Serial.print(F("  Ready: "));
        Serial.print(servos[motor].getReady());
        Serial.print(F("  speed: "));
        Serial.print(servos[motor].getSpeed());
        Serial.print(F("  ser: "));
        Serial.println(servos[motor].getPinUp());

      }

    }
    else
    {
      //delay (10) ;
      stateChanged && display && Serial.println(F("==========================================================================================================="));
      stateChanged && display && Serial.print(F("gen State: "));
      stateChanged && display && Serial.print(genState);
      stateChanged && display && Serial.println(F(" "));

      if ( genState == 10 )   // go up
      {
        notStopped = true;
        up();
      }
      else if ( genState == 20 )      // go down
      {
        notStopped = true;
        down();
      }
      else      // stop all motors
      {
        notStopped = false;
        stop();
      }
      delay(retardoStep);
    }
  }

}

void loop() {}

Hi,
What servos do you have that are 360Deg
OR
Are they continuous rotation servos?

What is the application?
Can you post a picture of your project?
Can you post a circuit diagram of your project?
What are you powering the Mega with?
What are you powering the servos with?
Does your code work at all, do you get servo motion before the Serial.print stalls the code?

Can I suggest you

Serial.begin(115200);

Thanks… Tom… :grinning: :+1: :coffee: :australia:

I read About the F() macro and planning to use in the next release, but for now the SRAM memory doesn’t seem to be overused.

When you are sending a million bits per second it does not take much to disrupt the serial stream. Can you use a lower bit rate?

My servoMotors are 360º ones.

What is the application?
The app is to control a mobile roof.

Can you post a picture of your project?
I will try to put some pictures of the model.

Can you post a circuit diagram of your project?
I will make a simple schema of ie.
What are you powering the Mega with?
What are you powering the servos with?
I use a different power system for the servos and the Arduino get the power from the computer USB.
Does your code work at all, do you get servo motion before the Serial.print stalls the code?
The system work well most of the time, but occasionally I see lot of “?” in serial monitor and
I lost control over the servo for some second. The situation is dangerous is the roof is near the
maximum higher position.

Hi,
Your code is hard to follow, are you using pins 0 and 1, if so then that could be your problem.
Pin 0 and Pin 1 are used for TX/RX Serial Monitor.

Thanks… Tom… :grinning: :+1: :coffee: :australia:

Can you explained what you want to do. You will read a lot but after that…

I do not use pin 0, 1. We control servomotor with pins 2 to 13

Does that mean they ONLY turn 0 to 360Deg
OR
Can accomplish may rotations?
Can you please post a link to specs/data please?

Tom… :grinning: :+1: :coffee: :australia:
PS. Code would be good.

This is only a temporary code to read control commands from the serial monitor:
s stop all the servos
c order the roof to go in higher position
u order the roof to go to the lower position
e emergency system stop
t to test distance in next development step
d display active/inactive

I read one character and discard any other one to avoid repetition.

Don’t really sure of this part of the code ;-(