     }
 
     tries++;
-    if(tries > 2000) {
+    if(tries > 720) {
       // Insertion sort
       for (int j = 0; j < 6; j++)
       {

  sabertoothDriverNode.initNode();
  sabertoothDriverNode.subscribe(commandSubscriber);
  sabertoothDriverNode.subscribe(setActualArticulationSubscriber);
  sabertoothDriverNode.advertise(pubwheelStatus);
  sabertoothDriverNode.advertise(pubDebug);


}

void newWinchCommandCallback(const std_msgs::Int16& newWinchCommand){
    if(newWinchCommand.data == 0){
      driveClockwise(WINCH_MOTOR_ID, 0);
    }
    else if( newWinchCommand.data > 0) {
      driveClockwise(WINCH_MOTOR_ID, WINCH_SPEED_UP);
      delaySeconds(1);
      driveClockwise(WINCH_MOTOR_ID, 0);
    }
    else if(newWinchCommand.data < 0) {
      driveCounterclockwise(WINCH_MOTOR_ID, WINCH_SPEED_DOWN);
      delaySeconds(newWinchCommand.data);
      driveCounterclockwise(WINCH_MOTOR_ID, 0);
    }
}


