################################################# # RaSCL : Robotics & Sensorial Control Language # # # # http://www.slyware.com/projects_rascl.shtml # # Simon Chudley (simon@slyware.com) # # # ################################################# # # File : libraries/lib_servos.rascl # Summary : Common code for manipulating servos. # # Contains common functions for manipulating servos. ################## ## SERVO OBJECT ## ################## ### # Function : Resource<Servo>create (Servo constructor) # Args : servo_id # Purpose : Initiates a specialised abstract servo representation of a resource. # Pre-cnd : - # Post-cnd : Servo member variables will be initiated. # (::complex_function Resource<Servo> create) (->set .mId func_args) (->set .mPosition 0) (->set .mCalibration {}) (::endfunc) ### # Function : Resource<Servo>setpos_abs # Args : RaSCL list { real_position speed } # Purpose : This sets the servo to a real position (not normalised). It has no protection # over whether the specified position is valid, or outside the servos calibration. # Consider using Resource<Servo>setpos or Resource<Servo>setpos_real. # Pre-cnd : The Servo has been added to a resource container that has correctly implemented # the encode_servo_command and servo_move_time methods. # Post-cnd : Servo will be moving (or moved) to the specified position. # Returns : Returns the time (in miliseconds) it will take to move the servo (given its # last known position) if successful. # Throws : ServoCommandFailedException : If the command could not be encoded and written # InvalidParametersException : If invalid parameters are given # (::complex_function Resource<Servo> setpos_abs) # Check for correct number of arguments (::call .check_arguments (list 2 {Resource<Servo>setpos_abs expects { <real_position> <speed> }} func_args)) # Split Arguments (::= .lPos (icar func_args 0)) # Absolute target position (::= .lSpeed (icar func_args 1)) # Speed # Can't continue if we're not added to a resource container! (::if { (not (isptr (->get .mParent))) }) (::= .lId (->get .mId)) (::throw .ServoCommandFailedException (cdrpush lId {No parent controller exists for servo id:})) (::endif) # Member mParent holds a RaSCL pointer to our parent controller, but we need to dynamically cast it # to get a RaSCL pointer to the specific type of controller (::= .lController (dynamic_cast (->get .mParent))) # Ask controller how long it will take to execute this command (::= .lTime (*lController::servo_move_time (list (abs (- (->get .mPosition) lPos)) lSpeed))) # Ask our parent controller to encode this servo command (::= .lCommand (*lController::encode_servo_command (list (->get .mId) lPos lSpeed))) # Send command if we encoded it successfully (::if { (and (> (count lCommand) 0) (*lController::write lCommand)) }) # Update the new position (->set .mPosition lPos) # Return the time taken (::return lTime) (::endif) # If we get here, something went wrong (::= .lId (->get .mId)) (::throw .ServoCommandFailedException (cdrpush lId {Unable to encode and send command to servo id:})) (::endfunc) ### # Function : Resource<Servo>setpos_real # Args : RaSCL list { real_position speed } # Purpose : This sets the servo to a real position (not normalised). It adds the protection # that if calibration information has been supplied for this servo, it will not # let you move it outside its calibration range. If the servo is not calibrated, it # provides no protection and behaves like Resource<Servo>setpos_abs. # Pre-cnd : The Servo has been added to a resource container that has correctly implemented # the encode_servo_command and servo_move_time methods. Servo calibration information has # been correctly set-up. # Post-cnd : Servo will be moving (or moved) to the specified position. # Returns : Returns the time (in miliseconds) it will take to move the servo (given its # last known position) if successful. # Throws : ServoCommandFailedException : If the command could not be encoded and written # InvalidParametersException : If invalid parameters are given # (::complex_function Resource<Servo> setpos_real) # Check for correct number of arguments (::call .check_arguments (list 2 {Resource<Servo>setpos_real expects { <real_position> <speed> }} func_args)) # Arguments (::= .lPos (icar func_args 0)) # Absolute target position (::= .lSpeed (icar func_args 1)) # Speed (::= .lId (->get .mId)) # Servo ID (::= .lCal (->get .mCalibration)) # Calibration settings # Any calibration settings? (::if { (> (count lCal) 0) }) # Get calibration values (::= .lMin (icar lCal 0)) (::= .lMax (icar lCal 1)) # Servo limit protection (::if { (< lPos lMin) }) (log {WARNING: Attempting to set servo id:} lId {to out-of-range value:} lPos {min:} lMin) (::= .lPos lMin) (::else) (::if { (> lRealPos lMax) }) (log {WARNING: Attempting to set servo id:} lId {to out-of-range value:} lPos {max:} lMax) (::= .lPos lMax) (::endif) (::endif) (::else) (log {WARNING: No calibration information available for servo id:} lId) (::endif) # Execute this command (::return (->setpos_abs (list lPos lSpeed))) (::endfunc) ### # Function : Resource<Servo>setpos # Args : RaSCL list { normalised_position speed } # Purpose : Using the pre-set normalisation and calibration settings for this # servo, this moves the servo to the specified position at the specified speed. # Pre-cnd : The Servo has been added to a resource container that has correctly implemented # the encode_servo_command and servo_move_time methods. Servo calibration information has # been correctly set-up. # Post-cnd : Servo will be moving (or moved) to the specified position. # Returns : Returns the time (in miliseconds) it will take to move the servo (given its # last known position) if successful. # Throws : ServoCommandFailedException : If the command could not be encoded and written # ServoNotCalibratedException : If the servo has not been calibrated # InvalidParametersException : If invalid parameters are given # (::complex_function Resource<Servo> setpos) # Check for correct number of arguments (::call .check_arguments (list 2 {Resource<Servo>setpos expects { <normalised_position> <speed> }} func_args)) # Arguments (::= .lNormPos (icar func_args 0)) # Target normalised position (::= .lSpeed (icar func_args 1)) # Speed (::= .lId (->get .mId)) # Servo ID (::= .lCal (->get .mCalibration)) # Calibration settings # Limit check on normalised value (::if { (< lNormPos 0) }) (log {WARNING: Attempting to set servo id:} lId {to out-of-range normalised value:} lNormPos {min: 0}) (::= .lNormPos 0) (::else) (::if { (> lNormPos gCALMAX) }) (log {WARNING: Attempting to set servo id:} lId {to out-of-range normalised value:} lNormPos {max:} gCALMAX) (::= .lNormPos gCALMAX) (::endif) (::endif) # Convert the normalised position to a real one (::if { (> (count lCal) 0) }) # Get calibration values and convert (::= .lMin (icar lCal 0)) (::= .lStep (icar lCal 2)) (::= .lNormPos (+ lMin (* lStep lNormPos))) (::else) (::throw .ServoNotCalibratedException (cdrpush lId {No calibration information set for servo id:})) (::endif) # Set the real absolute position, and return the result (::return (->setpos_abs (list lNormPos lSpeed))) (::endfunc) ### # Function : Resource<Servo>getpos # Args : - # Purpose : Using the pre-set normalisation and calibration settings for this # servo, this returns the normalised last known position of the servo. # Pre-cnd : Servo calibration information has been correctly set-up. # Post-cnd : - # Returns : Returns the last known normalised position. # Throws : ServoNotCalibratedException : If the servo has not been calibrated # (::complex_function Resource<Servo> getpos) # Get state (::= .lCal (->get .mCalibration)) # Calibration settings (::= .lPos (->get .mPosition)) # Last known position # Convert the real position to a normalised one (::if { (> (count lCal) 0) }) # Get calibration values, convert and return (::= .lMin (icar lCal 0)) (::= .lStep (icar lCal 2)) (::return (/ (- lPos lMin) lStep)) (::else) (::= .lId (->get .mId)) (::throw .ServoNotCalibratedException (cdrpush lId {No calibration information set for servo id:})) (::endif) (::endfunc) ### # Function : Resource<Servo>getpos_real # Args : - # Purpose : Returns the real last known position of this servo. # Pre-cnd : - # Post-cnd : Returns the last known real position. # (::complex_function Resource<Servo> getpos_real) # Return position (::return (->get .mPosition)) (::endfunc) ### # Function : set_all_servos # Args : RaSCL list { normalised_position speed } # Purpose : Using the pre-set normalisation and calibration settings for the # servos, this sets all servos to the specified normalised position at # the specified speed. # Pre-cnd : All servo resources have been calibrated and generic_botinit.rascl has # normalised the calibration settings. # Post-cnd : All servos will be set to the specified position. # Returns : Returns the time in milliseconds that it will take the servo with the # furthest distance to move to move to its new position, given its current # position. # Throws : ServoCommandFailedException : If a servo command could not be encoded and written # ServoNotCalibratedException : If a servo has not been calibrated # InvalidPointerException : If an invalid servo pointer was given # InvalidParametersException : If invalid parameters are given # (::*complex_function set_all_servos) # Check for correct number of arguments (::call .check_arguments (list 2 {set_all_servos expects { <normalised_position> <speed> }} func_args)) # Params (::= .lPos (icar func_args 0)) (::= .lSpeed (icar func_args 1)) (::= .lTimes {}) # For each servo (::foreach gBotServos) (::= .lEntry iItem) (::= .lServo (dynamic_cast (car lEntry))) # Check it is a valid servo pointer (::call .test_rascl_pointer (list .Resource<Servo> lServo)) # Move servo, and add the returned time to the list (::= .lTimes (carpush (*lServo::setpos (list lPos lSpeed)) lTimes)) (::endforeach) (::return (max lTimes)) (::endfunc) ### # Function : center_all_servos # Args : RaSCL list { speed } # Purpose : Using the pre-set normalisation and calibration settings for the # servos, this sets all servos to their calibrated center positions. # Pre-cnd : All servo resources have been calibrated and generic_botinit.rascl has # normalised the calibration settings. # Post-cnd : All servos will be moved (or moving) to their center positions. # Returns : Returns the time in milliseconds that it will take the servo with the # furthest distance to move to move to its new position, given its current # position. # Throws : ServoCommandFailedException : If a servo command could not be encoded and written # ServoNotCalibratedException : If a servo has not been calibrated # InvalidPointerException : If an invalid servo pointer was given # InvalidParametersException : If invalid parameters are given # (::*complex_function center_all_servos) # Check for correct number of arguments (::call .check_arguments (list 1 {center_all_servos { <speed> }} func_args)) # Params (::= .lSpeed (icar func_args 0)) (::= .lTimes {}) # For each servo (::foreach gBotServos) (::= .lEntry iItem) (::= .lServo (dynamic_cast (car lEntry))) # Check it is a valid servo pointer (::call .test_rascl_pointer (list .Resource<Servo> lServo)) # Calibrated center point for this servo (::= .lCenter (icar (*lServo::get .mCalibration) 3)) # Move servo, and add the returned time to the list (::= .lTimes (carpush (*lServo::setpos (list lCenter lSpeed)) lTimes)) (::endforeach) # Return the maximum time to wait for all servos to move (::return (max lTimes)) (::endfunc)