     temp += 400;
   }
   currentWheelStatus[FRONT_LEFT_DRIVE_MOTOR_ID].orientation = (int)((temp * 9L) / 10L);
-  
+
   temp = EncoderML::getPosition();
   if (temp < 0)
   {
     temp += 400;
   }
   currentWheelStatus[MIDDLE_LEFT_DRIVE_MOTOR_ID].orientation = (int)((temp * 9L) / 10L);
-  
+
   temp = EncoderRL::getPosition();
   if (temp < 0)
   {
     temp += 400;
   }
   currentWheelStatus[REAR_LEFT_DRIVE_MOTOR_ID].orientation = (int)((temp * 9L) / 10L);
-  
+
   temp = EncoderFR::getPosition() - 200;
   if (temp < 0)
   {
     temp += 400;
   }
   currentWheelStatus[FRONT_RIGHT_DRIVE_MOTOR_ID].orientation = (int)((temp * 9L) / 10L);
-  
+
   temp = EncoderMR::getPosition() - 200;
   if (temp < 0)
   {
     temp += 400;
   }
   currentWheelStatus[MIDDLE_RIGHT_DRIVE_MOTOR_ID].orientation = (int)((temp * 9L) / 10L);
-  
+
   temp = EncoderRR::getPosition() - 200;
   if (temp < 0)
   {

  EncoderFR::setupEncoderFR();
  EncoderMR::setupEncoderMR();
  EncoderRR::setupEncoderRR();
  
  sabertoothDriverNode.initNode();
  sabertoothDriverNode.subscribe(mcSUB);
  sabertoothDriverNode.advertise(pubResults);
  // Communicate with the Sabertooth
  Serial3.begin(9600);
  
  // Initialize current wheel status: Assume we're in "closed" position
  currentWheelStatus[0].orientation = 0.0;     //.fl_articulation_angle = 0.0;
  currentWheelStatus[1].orientation = 180.0;   //.fr_articulation_angle = 180.0;

  updateArticulationValues();
  continueDriving();
  // TODO: Delete this?
//  msg.data = (long)currentWheelStatus[FRONT_LEFT_DRIVE_MOTOR_ID].orientation;
//  pubResults.publish(&msg);

}

