### How go back in the code

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?

 ``123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180`` ``````#include //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 #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!"); }``````

Many thanks,

Urko.
Hello urko18,

If you are going to use a "goto", which is not the best idea, use it correctly. The labe for a goto is written as `labelName:`. The ';' that you have on line 93 is not needed. A do/while could be a replacement for the "goto".

When I look at lines 152 and 155 I wonder if you meant to use the ';' at the end of the line or if the following line should be part of the while loop? As it is the while loop will continue to read until the condition fails.

Your use of "delay()" has good chance of being specific to the operating system that you are using. You might want to give this a try:
`std::this_thread::sleep_for(std::chrono::seconds(1)); // Requires header files "chrono" and "thread" `. You could change "seconds" to "microseconds" and use "500" for 1/2 second or "1500" for 1.5 seconds.

Hope that helps,

Andy
Topic archived. No new replies allowed.