Process queue data.h

From HotDec

Psuedo-Code Description

process_queue_data (type) -- type is to distinguish between a message from USER2CTRL_QUEUE or from POSE_QUEUE {

  Switch on the type of packet
     
     MOVE packet:
        Assign the target pose
        Switch on different flag options
           Depending on the case, set flags to turn on/off the lift fan, etc.

     VISION packet:
        Record current pose as delivered by either USER2CTRL_QUEUE or POSE_QUEUE

     SET_ESTIMATOR packet:
        Reset forces and initialize

     SET_LIFT_SPEED packet:
        Assign lift speed

}