 }
 
 void newWinchCommandCallback(const std_msgs::Int16& newWinchCommand){
-  if(newWinchCommand.data != 0){
-    //currentWinchStatus = START_ROTATING_WINCH;
-    //winchRotationTime = newWinchCommand.data;
-
-    if( newWinchCommand.data > 0) {
+    if(newWinchCommand.data == 0){
+      driveClockwise(WINCH_MOTOR_ID, 0);
+    }
+    else if( newWinchCommand.data > 0) {
       driveClockwise(WINCH_MOTOR_ID, WINCH_SPEED_UP);
-      delaySeconds((double)newWinchCommand.data);
+      delaySeconds(1);
       driveClockwise(WINCH_MOTOR_ID, 0);
     }
-    else {
+    else if(newWinchCommand.data < 0) {
       driveCounterclockwise(WINCH_MOTOR_ID, WINCH_SPEED_DOWN);
-      delaySeconds((double)newWinchCommand.data);
+      delaySeconds(newWinchCommand.data);
       driveCounterclockwise(WINCH_MOTOR_ID, 0);
     }
-
-  }
 }
 
 

  sabertoothDriverNode.initNode();
  sabertoothDriverNode.subscribe(commandSubscriber);
  sabertoothDriverNode.subscribe(setActualArticulationSubscriber);
  sabertoothDriverNode.advertise(pubwheelStatus);
  sabertoothDriverNode.advertise(pubDebug);



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

ros::Subscriber<std_msgs::Int16> winchSubscriber("TransportWinchCommand", &newWinchCommandCallback);


// Print error message to "sabertooth_debugger" topic
