 void driveUp(void);
 void driveDown(void);
 
-ros::Subscriber<std_msgs::Int32> cvDegreeReader("camera_angle", &desiredCameraPosChanged);
+ros::Subscriber<std_msgs::Int32> cvDegreeReader("target_camera_angle", &desiredCameraPosChanged);
+
+std_msgs::Int32 currentCameraAngle;
+ros::Publisher cvDegreePublisher("current_camera_angle", &currentCameraAngle);
 
 void setup(){
 

    desiredDegrees = angle % 360;
    // Step = Degrees / steps per degree * gear ratio
    desiredStep = int(desiredDegrees/stepDegree)*19;
    Serial.print("Going to degree: ");
    Serial.print(desiredDegrees); 
    Serial.print(" (Desired step: ");
    Serial.print(desiredStep);
    Serial.print("; current step: ");
    Serial.println(stepCount);
  if(stepCount == desiredStep)
  {
	  return;

  digitalWrite(ENABLE, HIGH); //the motor should not make loud screeching
  nh.initNode();
  nh.subscribe(cvDegreeReader);
  nh.advertise(cvDegreePublisher);
  //Serial.begin(9600);
}

