   delayMicroseconds(1000);
 }
 
+/**
+* Gets the last known position of each articulation joint, and updates acordingly.
+*/
+void updateArticulationValues()
+{
+  int temp = EncoderFL::getPosition();
+  if (temp < 0)
+  {
+    temp += 400;
+  }
+  wheelStatus.fl_articulation_angle = (int)((temp * 9L) / 10L);
+
+  temp = EncoderML::getPosition();
+  if (temp < 0)
+  {
+    temp += 400;
+  }
+  wheelStatus.ml_articulation_angle = (int)((temp * 9L) / 10L);
+
+  temp = EncoderRL::getPosition();
+  if (temp < 0)
+  {
+    temp += 400;
+  }
+  wheelStatus.rl_articulation_angle = (int)((temp * 9L) / 10L);
+
+  temp = EncoderFR::getPosition() - 200; // Subtract 200 b/c the right side is flipped.
+  if (temp < 0)
+  {
+    temp += 400;
+  }
+  wheelStatus.fr_articulation_angle = (int)((temp * 9L) / 10L);
+
+  temp = EncoderMR::getPosition() - 200;
+  if (temp < 0)
+  {
+    temp += 400;
+  }
+  wheelStatus.mr_articulation_angle = (int)((temp * 9L) / 10L);
+
+  temp = EncoderRR::getPosition() - 200;
+  if (temp < 0)
+  {
+    temp += 400;
+  }
+
+  wheelStatus.rr_articulation_angle = (int)((temp * 9L) / 10L);
+
+  pubwheelStatus.publish(&wheelStatus);
+}
+
 void unitTest(){
   // delay after set up
   for(int j = 0; j < 100; j++){

  }

  if (currentStatus == ARTICULATING) {
    char msg[] = "articulating";
    debugMsg.data = msg;
    pubDebug.publish(&debugMsg);
    
    if (needsToArticulate() == true) {
      articulateAllWheels();

  }
}



/**
 * Is called on starting the arduino.
 * 
 */
void setup(){
  pinMode(13, OUTPUT);

  // Setup the encoders
  EncoderFL::setupEncoderFL();
