     temp += 400;
   }
   currentWheelStatus[MIDDLE_RIGHT_DRIVE_MOTOR_ID].orientation = (int)((temp * 9L) / 10L);
+  currentRotationMessage.mr_articulation_angle = currentWheelStatus[MIDDLE_RIGHT_DRIVE_MOTOR_ID].orientation;
 
   temp = EncoderRR::getPosition() - 200;
   if (temp < 0)
   {
     temp += 400;
   }
+
   currentWheelStatus[REAR_RIGHT_DRIVE_MOTOR_ID].orientation = (int)((temp * 9L) / 10L);
-}
-
+  currentRotationMessage.rr_articulation_angle = currentWheelStatus[REAR_RIGHT_DRIVE_MOTOR_ID].orientation;
+
+  pubCurrentRotation.publish(&currentRotationMessage);
+}
+
+/**
+* Is called on starting the arduino.
+* 
+*/
 void setup(){
 
   // Communicate with the computer

    temp += 400;
  }
  currentWheelStatus[FRONT_LEFT_DRIVE_MOTOR_ID].orientation = (int)((temp * 9L) / 10L);

  temp = EncoderML::getPosition();
  if (temp < 0)


  sabertoothDriverNode.initNode();
  sabertoothDriverNode.subscribe(mcSUB);

  // Communicate with the Sabertooth
  Serial3.begin(9600);

  // Initialize the message
  currentRotationMessage.fl_articulation_angle = 0;
  currentRotationMessage.fr_articulation_angle = 180;
  currentRotationMessage.ml_articulation_angle = 0;
  currentRotationMessage.mr_articulation_angle = 180;
  currentRotationMessage.rl_articulation_angle = 0;
  currentRotationMessage.rr_articulation_angle = 180;
  
  currentRotationMessage.fl_drive_speed = 0;
  currentRotationMessage.fr_drive_speed = 0;
  currentRotationMessage.ml_drive_speed = 0;
  currentRotationMessage.mr_drive_speed = 0;
  currentRotationMessage.rl_drive_speed = 0;
  currentRotationMessage.rr_drive_speed = 0;

  // Initialize current wheel status: Assume we're in "closed" position
  currentWheelStatus[FRONT_LEFT_DRIVE_MOTOR_ID].orientation = 0.0;     //.fl_articulation_angle = 0.0;
  currentWheelStatus[FRONT_RIGHT_DRIVE_MOTOR_ID].orientation = 180.0;   //.fr_articulation_angle = 180.0;
  currentWheelStatus[MIDDLE_LEFT_DRIVE_MOTOR_ID].orientation = 0.0;//.ml_articulation_angle = 0.0;
  currentWheelStatus[MIDDLE_RIGHT_DRIVE_MOTOR_ID].orientation = 180.0;//.mr_articulation_angle = 180.0;
  currentWheelStatus[REAR_LEFT_DRIVE_MOTOR_ID].orientation = 0.0;//.rl_articulation_angle = 0.0;
  currentWheelStatus[REAR_RIGHT_DRIVE_MOTOR_ID].orientation = 180.0; //.rr_articulation_angle = 180.0;
  
  currentWheelStatus[FRONT_LEFT_DRIVE_MOTOR_ID].velocity = 0.0;//.fl_drive_speed = 0.0;
  currentWheelStatus[FRONT_RIGHT_DRIVE_MOTOR_ID].velocity = 0.0;//.fr_drive_speed = 0.0;
  currentWheelStatus[MIDDLE_LEFT_DRIVE_MOTOR_ID].velocity = 0.0;//.ml_drive_speed = 0.0;
  currentWheelStatus[MIDDLE_RIGHT_DRIVE_MOTOR_ID].velocity = 0.0;//.mr_drive_speed = 0.0;
  currentWheelStatus[REAR_LEFT_DRIVE_MOTOR_ID].velocity = 0.0;//.rl_drive_speed = 0.0;
  currentWheelStatus[REAR_RIGHT_DRIVE_MOTOR_ID].velocity = 0.0;//.rr_drive_speed = 0.0;

  // Publish our stuff
  sabertoothDriverNode.advertise(pubResults);
  sabertoothDriverNode.advertise(pubCurrentRotation);
}

// This program does whatever ROS directs it to do.
// So far, it drives the robot forwards and backwards.
void loop(){
  delayMicroseconds(100000);
  sabertoothDriverNode.spinOnce(); // Check for subscriber update/update timestamp
  updateArticulationValues();
  continueDriving();
  // TODO: Delete this?
