 
 }
 
+void setActualArticulationValues(const command2ros::ManualCommand& articulationValues){
+  //Set articulation values
+  wheelOffset.fl_articulation_angle = (int)(articulationValues.fl_articulation_angle - wheelStatus.fl_articulation_angle - wheelOffset.fl_articulation_angle);
+  wheelOffset.ml_articulation_angle = (int)(articulationValues.ml_articulation_angle - wheelStatus.ml_articulation_angle - wheelOffset.ml_articulation_angle);
+  wheelOffset.rl_articulation_angle = (int)(articulationValues.rl_articulation_angle - wheelStatus.rl_articulation_angle - wheelOffset.rl_articulation_angle);
+
+  wheelOffset.fr_articulation_angle = (int)(articulationValues.fr_articulation_angle - wheelStatus.fr_articulation_angle - wheelOffset.fr_articulation_angle);
+  wheelOffset.mr_articulation_angle = (int)(articulationValues.mr_articulation_angle - wheelStatus.mr_articulation_angle - wheelOffset.mr_articulation_angle);
+  wheelOffset.rr_articulation_angle = (int)(articulationValues.rr_articulation_angle - wheelStatus.rr_articulation_angle - wheelOffset.rr_articulation_angle);
+  
+  //pubwheelStatus.publish(&wheelOffset);// current rotation data for each wheel. 
+
+}
+
 void newConveyorCommandCallback(const std_msgs::Int16& newConveyorCommand){
   if(newConveyorCommand.data != 0){
     currentStatus = START_ROTATING_CONVEYOR;

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

}



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

ros::Subscriber<command2ros::ManualCommand> setActualArticulationSubscriber("SetActualArticulationValues", &setActualArticulationValues);

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

