   wheelStatus.fr_articulation_angle = (int)wheelTarget.fr_articulation_angle;
   */
   
-  int encoderPostition = EncoderFL::getPosition();
-  if (encoderPostition < 0)
+  int encoderPostition = EncoderFL::getPosition() + (int)(wheelOffset.fl_articulation_angle / 360 * 400);
+  while (encoderPostition < 0)
   {
     encoderPostition += ENCODER_POSITIONS;
   }
   wheelStatus.fl_articulation_angle = (int)((encoderPostition * 9L) / 10L);
 
-  encoderPostition = EncoderML::getPosition();
-  if (encoderPostition < 0)
+  encoderPostition = EncoderML::getPosition() + (int)(wheelOffset.ml_articulation_angle / 360 * 400);
+  while (encoderPostition < 0)
   {
     encoderPostition += ENCODER_POSITIONS;
   }
   wheelStatus.ml_articulation_angle = (int)((encoderPostition * 9L) / 10L);
 
-  encoderPostition = EncoderRL::getPosition();
-  if (encoderPostition < 0)
+  encoderPostition = EncoderRL::getPosition() + (int)(wheelOffset.rl_articulation_angle / 360 * 400);
+  while (encoderPostition < 0)
   {
     encoderPostition += ENCODER_POSITIONS;
   }
   wheelStatus.rl_articulation_angle = (int)((encoderPostition * 9L) / 10L);
 
-  encoderPostition = EncoderFR::getPosition() - 200; // Subtract 200 b/c the right side is flipped.
-  if (encoderPostition < 0)
+  encoderPostition = EncoderFR::getPosition() + (int)(wheelOffset.fr_articulation_angle / 360 * 400) - 200; // Subtract 200 b/c the right side is flipped.
+  while (encoderPostition < 0)
   {
     encoderPostition += ENCODER_POSITIONS;
   }
   wheelStatus.fr_articulation_angle = (int)((encoderPostition * 9L) / 10L);
 
-  encoderPostition = EncoderMR::getPosition() - 200;
-  if (encoderPostition < 0)
+  encoderPostition = EncoderMR::getPosition() + (int)(wheelOffset.mr_articulation_angle / 360 * 400) - 200;
+  while (encoderPostition < 0)
   {
     encoderPostition += ENCODER_POSITIONS;
   }
   wheelStatus.mr_articulation_angle = (int)((encoderPostition * 9L) / 10L);
 
-  encoderPostition = EncoderRR::getPosition() - 200;
-  if (encoderPostition < 0)
+  encoderPostition = EncoderRR::getPosition() + (int)(wheelOffset.rr_articulation_angle / 360 * 400) - 200;
+  while (encoderPostition < 0)
   {
     encoderPostition += ENCODER_POSITIONS;
   }
-
   wheelStatus.rr_articulation_angle = (int)((encoderPostition * 9L) / 10L);
 }
 

}


ros::Subscriber<command2ros::ManualCommand> commandSubscriber("ManualCommand", &newManualCommandCallback);

ros::Subscriber<std_msgs::Int16> conveyorSubscriber("ConveyorCommand", &newConveyorCommandCallback);


    }
  }
  // Test the winch.
  //unitTestWinch();
  // Test the conveyor.
  //unitTestConveyor(); 
}  
  
void unitTestConveyor()
