How to go back in the code

Projects Programming
1

Hello!

I have a problem, how can I change the GOTO (line 154 to line 93) of the following code for some other structure that does not give so many problems?

#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, 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 = Ready "));
  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 = do the sequence "));
    delay(2000);

    v:;

    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");
  goto v
  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!");
}

Thank you!!

Urko.

2

how can I change the GOTO

Delete it. There is NO excuse for using GOTO in a C or C++ program.

Use a while loop, or some other looping structure, to perform the code between the label and the goto statement.

  for (int i = 0; i < m; i++) // structure to create columns

That comment has NOTHING to do with what the code is doing. GET RID OF IT!

  for (int e = 0; e < 1; e++)

You look stupid when you use a for loop to iterate once. Was that your intent?

3 
  while (Serial.read() != 'y');
  Serial.println("y");
  goto v
  while (Serial.read() != 'w');
  Serial.println("w");


}

Those two lines after the goto will never be executed so you can delete them.

  for (int e = 0; e < 1; e++) //Repetition of the process (e = number of sequences)
  {

This is equivalent to "int e=0;" so you can change it to that and remove the formerly matching close bracket.

After that the label for your goto is not in the middle of a loop. Make the label a "while(1) {" and put the matching "}" in place of the goto.

#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, 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 = Ready "));
  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***/


  int e = 0;


  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 = do the sequence "));
  delay(2000);


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


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!");
}