How go back in code of Arduino

[urko18]

Hello!
I would like to know how I can go back in the following code:

 #include <ax12.h> //Include ArbotiX DYNAMIXEL library
const int SERVO_ID[] = {1, 2, 3, 4}; // const correct / You might want to use just one for testing...
const int servo_count = sizeof(SERVO_ID) / sizeof(*SERVO_ID); // Let the compiler calculate the amount of servos, so that you can change it easily
int a, b, z, y, j, t, interrupt_state;
int Speed = 700;
int m = 400; //columns
#include <avr/interrupt.h>
#define PULSADOR_EMERGENCIA (PINB&0x02)
int pos2[] = {3820, 400, 230, 2395}; //rest position of the dynamixel motors
void setup()
{
  dxlInit(1000000); //start dynamixel library at 1mbps to communicate with the servos
  Serial.begin(9600); //start serial at 9600 for reporting data.

  for (int i = 0; i < servo_count; ++i)
  {
    Relax(SERVO_ID[i]);
    Serial.print(F("ID: "));
    Serial.println(SERVO_ID[i]);
  }

  PORTB |= 0x1E;
  PCICR |= (1 << PCIE1);
  PCMSK1 |= 0x1E;
  interrupt_state = 0;
  interrupts();

  delay(1000);

}

void loop()
{
  int positionn[servo_count][m]; //Matrix of movements
  while (Serial.read() != 'a') {}

  Serial.print(F("a = operario preparado "));
  delay(2000);
  Serial.println(F("Positions vector "));
  Serial.print(F(": ["));

  for (int i = 0; i < m; i++) // structure to create columns
  {
    for (int j = 0; j < servo_count; j++) // structure to create columns
    {
      positionn[j][i] = dxlGetPosition(SERVO_ID[j]); //read and save the actual position
    }

    delay(10);

    for (int j = 0; j < servo_count; j++) // structure to create columns
    {
      Serial.print(positionn[j][i]); //Display the vector
      Serial.print(F(", "));
    }
  }
  Serial.print(F("]\n"));
  delay(5000);


  /***The servos will move according to registered movements***/

  for (int e = 0; e < 1; e++) //Repetition of the process (e = number of sequences)
  {
    Serial.print(F("SEQUENCE "));
    Serial.println(a + 1);

    int position[servo_count];
    int turns[servo_count];
    int pos1[servo_count];
    int pos2[servo_count];
    int current[servo_count];

    for (int i = 0; i < servo_count; i++)
    {
      current[i] = positionn[i][0];
      position[i] = positionn[i][0];
      turns[i] = 0;
      pos1[i] = dxlGetPosition(SERVO_ID[i]); //Actual servo position
      pos2[i] = positionn[i][0]; //Initial position of the movement (objective)
    }
    Serial.println(F("The servos will move to the initial position."));
    for (int servo = 0; servo < servo_count; ++servo)
    {
      go_to_position(pos1, pos2, servo); //Function that moves the robot to the initial position
    }

    while (Serial.read() != 'b') {}
    Serial.println(F("b = repetir secuencia una vez "));
    delay(2000);
while (j=1) {
    Serial.println(F("Now the servos will do the registered movements."));
    delay(2000);

    for (int movement = 0; movement < m; movement++)
    {
      for (int servo = 0; servo < servo_count; servo++)
      {
        if (positionn[servo][movement] != current[servo])
        {
          int next_pos = 1;
          if (positionn[servo][movement] < current[servo])
            next_pos = -1;
          while (positionn[servo][movement] != current[servo])
          {
            dxlSetGoalPosition(SERVO_ID[servo], current[servo]);
            current[servo] += next_pos;
            delayMicroseconds(Speed);

            if (current[servo] == position[servo] + MX_MAX_POSITION_VALUE)
            {
              position[servo] = current[servo];
              turns[servo]++;
            }
            else if (current[servo] == position[servo] - MX_MAX_POSITION_VALUE)
            {
              position[servo] = current[servo];
              turns[servo]--;
            }
          }
        }
      }
    }
    for (int i = 0; i < servo_count; i++)
    {
      Serial.print(F("Turns engine "));
      Serial.print(i + 1);
      Serial.print(F(": "));
      Serial.println(turns[i]);
      Serial.println(" ");
    }
  delay(3000);

  /****REST POSITION****/
  Serial.println(F("The robot will move to the resting position."));
  int pos1[servo_count];
  for (int i = 0; i < servo_count; i++)
  {
    pos1[i] = dxlGetPosition(SERVO_ID[i]); //Actual servo position
  }
  for (int servo = 0; servo < servo_count; ++servo)
  {
    go_to_position(pos1, pos2, servo);  //Function that moves the robot to the initial position
  }
  delay(1000);
  dxlTorqueOffAll();
  Serial.println(F("z"));
  while (Serial.read() != 'y');
  Serial.println("y");
  j=1;
  }
  j=0;
  }
  while (Serial.read() != 'w');
  Serial.println("w");
}
void go_to_position(int pos1[], int pos2[], int servo)//function
{
  while (pos1[servo] != pos2[servo])
  {
    if (pos2[servo] < pos1[servo])
    {
      dxlSetGoalPosition(SERVO_ID[servo], pos1[servo]);
      pos1[servo]--;
      delayMicroseconds(800);
    }
    else if (pos2[servo] > pos1[servo])
    {
      dxlSetGoalPosition(SERVO_ID[servo], pos1[servo]);
      pos1[servo]++;
      delayMicroseconds(800);
    }
  }
}
ISR(PCINT1_vect) {
  interrupt_state = (PINB & 0x02) >> 1;
  Serial.println("EMERGENCY BUTTON!");
}

To be understood, what I want is that when I press "Y" the code goes back to the line 91, [while (j = 1)], which is already working correctly.

But, now, what I want is that when I press "W", in line 154, that the code go back to beginning of the void loop, that is, to the line 34.

I know that they are some things in the program that have not sense, like for example the FOR of the line 63, but is a test program!

I imagine that there will be much more efficient ways to make the code return to the point you want, so if anyone has any ideas is welcome!

Thank you very much!

[coder777]

You can make a do { ... } while(...); loop. Place the do where ever you want to loop back.

You need better indention. It is actually hard to tell what line 151/153 belongs to. And you should seperate long function into smaller ones.

[SigSeg]

Arduino does something like:

int main() 
{
  setup();
  while (true) loop();
}

So if you press w on line 154 loop() finishes and restarts. (as you want it to?)
But you never reach that line because you do

while (j == 1)
{
  // Line 91-149
  j = 1;
}

you never leave that while (j = 1).
Are those
while (Serial.read() != KEY); intentional? You aren't deciding what to do based on the input. You are just waiting till exactly this button is pressed, since it translates to

while (KEY is not pressed)
{
  // do nothing
}

Maybe you wan't something like that instead of line 148-150?

// Wait till a button is pressed
while (!Serial.available()) ;
int input = Serial.read();
if (input == 'w') 
{
    // leave loop() and let it restart if w is pressed
    return;
}
else if (input == 'y')
{
    // another iteration of while (j == 1) if y is pressed
    j = 1;
}
else
{
    // leave while (j == 1) and run the next iteration of: for (int e = 0; e < 1; e++)
    // if neither w nor y is pressed
    j = 0;
}

Also a second reason you aren't leaving the while (j == 1) loop is because you wrote = instead of == which assigns j to 1 instead of compare it to 1. and an assignment returns the assigned value so it translates roughly to

while (1)
{
  j = 1;
  // Line 91-149
  j = 1;
}

Also

for (int e = 0; e < 1; e++)
{
  // something
}

doesn't make sense either. Since e can only be 0 it does the same as

int e = 0;
// something 

Edit: missing line

[urko18]

Thank you for answering!

coder777 I already tried a do ... while, but I did not achieve my goal of going back to the beginning by pressing "W" or going back to the part of playing a sequence (line 91) by pressing "Y".

SigSeg,

You are just waiting till exactly this button is pressed

Yes, that's what I want, that the program runs normally until you press any of the letters, so they interfere with the program.

// Wait till a button is pressed
while (!Serial.available()) ;
int input = Serial.read();
if (input == 'w') 
{
    // leave loop() and let it restart if w is pressed
    return;
}
else if (input == 'y')
{
    // another iteration of while (j == 1) if y is pressed
    j = 1;
}
else
{
    // leave while (j == 1) and run the next iteration of: for (int e = 0; e < 1; e++)
    // if neither w nor y is pressed
    j = 0;
}

That's what I did! but I still do not know how to do to have the option to go back twice, once at the beginning (W) and once at line 91 (Y).

Urko

[coder777]

Maybe something like this:

bool is_Y_pressed = false;

void loop()
{
if(! is_Y_pressed)
  process_line_34_90();

process_line_91_rest();


// Wait till a button is pressed
while (!Serial.available()) ;
int input = Serial.read();
if (input == 'w') 
{
is_Y_pressed=false;
    // leave loop() and let it restart if w is pressed
    return;
}
else if (input == 'y')
{
is_Y_pressed=true;
    // another iteration of while (j == 1) if y is pressed
    j = 1;
}
else
{
    // leave while (j == 1) and run the next iteration of: for (int e = 0; e < 1; e++)
    // if neither w nor y is pressed
    j = 0;
}

Note that only 'w' and 'Y' actually change the state.