An RTOS program was written to control servos for pan and tilt using a joystick. This code was kept for debugging. I wrote a previous blog post called "mbed RTOS - Roborealm, Joystick, and Servo Pan & Tilt - Thomas Murphy" explaining this control of the servos. This control of the servos was taken forward to the final stage where the lamp will track a face or a green ball with the three degrees of motion. This blog will detail the operation of the servo control and briefly describe the interface code between the servo control and the vision system.
mbed Code Explanation
The code, shown at the bottom of this post, is designed to take in control values serially from the vision system, designed by the vision team. The vision code sends a string containing three values representing the position of the head being tracked which are the x and y coordinates in degrees as well as how far the person is from the computer in centimeters.
The code starts with all the definitions, includes, etc. that are needed and then three message structures were written, one for each servo. The debug variable is set to 0 which results in the vision system values being used to control the servo positions as explained in this blog. When debug is set to 1, pan and tilt is controlled using the joystick. Note that in debug mode the lean servo is not controlled. This debug control was explained in the post mentioned above.
Main is the first thread to run where the three servos are given their individual home positions. Since debug is 0, a string is read in over serial from the vision system. This string is split up, using the strtok() function, into the x and y degree values for controlling pan and tilt, and the d centimeter value which was used for controlling lean. The pan control is more direct where the input degree value is taken away from the home position value and later sent as a position to control the pan position. Taking away the x value results in the pan turning to track the direction of the user. Adding x will result in the head turning away from the user (mirrored pan). The pan servo is not under much weight pressure and is unlikely to break the head mount under the fast movement since there is not too much weight in the head. Writing directly to the motor ensure the motor goes to the position as fast as possible. The commented out pan code gives a slower ore gradual movement which should be used in any future iteration that has a heavier head implementation. The pan limits were set based on what looked best and could be adjusted if needed.
The tilt and lean limits were set to prevent breaking the motor 3-D printed mounts. Avoid changing these limits. The tilt servo is incremented/decremented in 5º steps based on whether the input from the vision is bigger or smaller than the current motor position and within the limits. The 5º steps give the tilt enough speed while preventing rapid jerk movements. The tilt should always be incremented/decremented to a position instead of writing the position directly to avoid breaking the motor mount controlling the tilt. The lean servo is controlled in the same way but the d value from the vision is converted from its 100cm range to 0 - 20º. It is converted to that range since the lean range is max 20º to avoid breaking the lean motor mount. The lean servo's position is based on how far away the tracked face is. If the face is far away the lamp moves towards it and if the face is too close the lamp will back away. The new servo positions are then loaded into their respective memory pools and queues.
The lean, pan, and tilt motor position threads then run in that order. Each of these threads operate in the same way where they read their individual queues, take the servo position from the memory pool location indicated by the queue, write the servo position to the servo, and free the memory pool.
Main runs again and the cycle continues. Note: This is an RTOS with a thread wait of 100ms for all the threads. If the main thread is pulled off the processor before the new servo positions are set up, there is no need to be concerned about getting random values to the servos. The queues only get a memory pool locations when the servo positions have been placed fully into the memory pools.
Below is a short video showing the lamp tracking my face. My final reflection contains videos of the final lamp with the face implemented and tracking the ball.
Motors Team Final Code