Laser Deflection System: Disturbance Correction

Transcription

1 Laser Deflection System: Disturbance Correction Jack Damerji Matt DiLeo Tyler Ferman Final Report for ECSE-4962 Control System Design Professor: John Wen Date: April 30, 2003 Rensselaer Polytechnic Institute

2 Abstract: The purpose of this report is to introduce the Laser Deflection System and to present the detailed design process for this prototype. The goal of this project is to provide professional experience in designing a robust controller and implementing it into a physical system. The design and development of the Laser Deflection System involved the understanding control system design principals for disturbance correction, a topic that is found in both research and industry. This project is focused on the correction of a measurable disturbance in the trajectory of a laser. The idea is to redirect a laser off a mirror, despite the input path, to a specified point on the wall. This will involve the robust control of a mirror s angle to compensate for the variable input. This type of project will provide a unique educational experience while developing an industry-type prototype The mirror, once mounted on a pan-and-tilt mechanism, will be allowed 2 degrees of freedom of movement. This will provide a wide range of input trajectories to be accurately reflected to a specific point. Laser Deflection system has been the basis for a number of products. Tremor compensation for microsurgery, Satellite tracking receiver for a boat and isolation table are some products that implement this kind of technology. ii

3 Table of Contents Introduction:... 1 Project Goals:... 1 Design Procedure:...2 Design Detail:... 5 Input:... 5 Controller:... 5 Math Model:... 5 Pan-and-Tilt:... 9 Combination of Subsystems:... 9 Controller Development: Professional and Societal Considerations: Design Verification: Motors & Gears: Input/Controller accuracy: Linear and Non-linear Comparison: Model Accuracy: Controller Performance: Costs: Conclusion: Bibliography Appendix A: SolidWork Plots...I Appendix B:... V Appendix C... XXXIII Appendix E...XXXVIII ii

4 Table & Figures Figure 1: Design Model... 2 Figure 2: Design timeline... 4 Figure 3: Math Model Approaches... 7 Figure 4 : Pan Controller without actual friction Figure 5: Tilt Controller without actual friction Figure 6: Friction Response Figure 7: Pan Controller with actual friction Figure 8: Tilt Controller with actual friction Figure 9: Controller block diagram Figure 10: Block diagram with coulomb friction compensater Figure 11: Pan Real Controller simulated Figure 12: Tilt Real Controller simulated Figure 13: Motor Responses Figure 14: Controller Error Figure 15: Pan Controller Error Figure 16: Tilt Controller Error Figure 17: Input Accuracy Results Figure 18: Open-Loop Response Actual vs. Simulated Figure 19: Response of a Random input Figure 20: Step and torque Responses on Pan Side Figure 21: Step and Torque Responses for the Tilt Side Figure 22: Sinusoid Response for the Pan Side Figure 23: Results of Experiment1... VIII Figure 24: Results of Experiment2...IX Figure 25: Results of Experiment3... X Figure 26: Results of Experiment4...XII Figure 27: Pan Sinusoid Response... XIII Figure 28: Tilt Sinusoid Responce...XIV Figure 29: Tested Motor Measurements... XV Table 1: results of Friction measurement Table 2: Final Pan Controller Table 3: Final Pan Controller Table 4: Test Motor Specifications and Results Table 5: Error, Torque & Speed Measurements Table 6: Specification Comparison Table 7 : Frequency Response Results Table 8: List Of Parts, Materials, and Team Labor iii

5 iv

6 Introduction: The Laser Deflection System is a semester long project that is mainly focused on the design of a control system to accurately position a reflective surface. The system is broken into three parts: input, the exact location of the laser to be reflected, controller, the main part of this project and the output, the desired target. Two pan-and-tilt mechanisms are used, one for the input and the other for the controller. The Laser Deflection System is similar to some existing applications such as Inter-Satellite Link Technology. This device provides an optical data transmission link between low orbit satellites with satellites in parking, high orbits. Receiving data is done using a four-quadrant avalanche photo detector (APD) and two single-axis fine-steering mirrors, similar to our system. 1 There are several other applications where disturbance correction is vital. For example, for precision optical experiments and semiconductor production, a level surface is necessary. In this situation, a vibration isolation table is used so correct for any floor vibrations. These tables use pneumatic isolators and other vibration isolation devices to ensure a steady platform. 2 Another application would be for tremor compensation. Error compensation is vital for applications such as microsurgery. Even the slightest tremor could destroy the surgery, thus this is a research for a human-machine interface to correct for tremor is of interest in many top universities, such as Carnegie Mellon. 3 Another commercial use of disturbance correction would be satellite tracking on sea ships. As the boat rocks with the waves, the satellite antenna must remain pointing at a specific satellite, otherwise communication is lost. Systems such as Jason Electronics Scansat B 9900 are installed in many vessels to maintain satellite communication. 4 All of these products use disturbance correction to provide a more reliable system. These applications have a very common specification, a high accuracy requirement. There is no room for error; therefore any disturbances must be corrected. The Laser Deflection System is designed for this use in laser communication. In laser communication, there are several sources, such as a water droplet, that could alter the path of the laser. The main purpose of the system is to be able to accurately redirect the laser despite any measurable disturbance. This would improve the accuracy of the laser to guarantee a communication link at all times. The main goals of this project are recorded below: Project Goals: Compensate for a known input disturbance. The pan-and-tilt controlling the mirror will be controlled to compensate for trajectory disturbance and redirect laser to a specified target. Achieve the performance specifications mentioned in the verification section of the report such as settling time, overshoot, rise time, and steady state error. 1 Brandon; Chan; Kwan, Khosla; Riviere,

7 Design Procedure: Figure 1: Design Model The Laser Deflection System can be disassembled as shown above into three main parts. The first part is the input. The purpose of the input subsystem is to accurately calculate the input trajectory of the laser. This calculation provides the measured distortion of the laser. To do this, the laser pen is mounted onto a pan-and-tilt mechanism, allowing distortion in two degrees of freedom. The movement of this panand-tilt is changed by human intervention, representing a random input disturbance that could be introduced in a laser communication link. The exact direction of the laser pointer is determined by two encoders installed on each axis of the pan-and-tilt, one for each degree of freedom. With a fixed point and measured angles from the encoders, the input trajectory can be easily determined. The second subsystem of the device is the controller. This part also uses a panand-tilt mechanism with a mirror as a load. Two motors control this pan-and-tilt mechanism and it s position measured by two encoders. The motors will position the load to deflect the laser properly. Also included in the controller sub-system is the actual DSP controller itself. The code for this controller will be developed using MATLAB and ARCS development products. To develop this controller, the desired position of the mirror must be calculated from the measured input trajectory and desired target position. There are two possible approaches for the design of the controller. First approach is controlling the normal vector of the mirror. This means the DSP would use the input direction to find out the exact pan and tilt angles that is necessary to reflect the beam at the target. The second approach is to obtain the laser input direction, calculate the projected point on the wall, and find the error to the desired target location. The controller would then adjust to minimize this error. After further study, the group decided to go with the first approach because it keeps a comparable relation between the desired set point and the directly measurable actual angle position. 2

8 Before controller design can begin, a model of the system must be developed. This will provide an easy means of mathematical analysis and allow accurate design through simulation instead of physical testing. To build this model the moment of inertia, weight and center of gravity of the system must be calculated. After modeling and simulating the system, a correct selection of motors, gears and encoders needed to physically run the system can be made. At that time, a physical system can be built and any tuning can be done to meet specifications. The initial specifications made for the Laser Deflection System are: Initial Specifications: Keep laser within 1cm of target point on wall 3 feet away Wide range of inputs (range of motion ~53 o ) Large range of motion for controller (~ 35 o ) Assume maximum user speed of 50in/s (Controller tracking speed: ~6 rad/s) Overshoot: <1% Settling time: <0.1s Steady State error: <0.1 o Having a time line was a very crucial for our design procedure. We have a semester long project with several different phases. The figure below shows how the whole project was broken down and the desired deadlines for each sub functions. 3

9 Figure 2: Design timeline 4

10 Design Detail: Input: To create a laser to act as a communication link, a laser pen is used. The Laser is mounted so that the laser trajectory lies at the intersection of the pan rotation axis and tilt rotation axis on the input pan-and-tilt mechanism. Using the center of the mirror as an origin, called [x, y, z] = [0, 0, 0], then the location of the laser on the pan-and-tilt is initially designed to be at the location [-5in, -5in, 6in]. This location was chosen to provide a good range of angles for both input and mirror pan-and-tilt. As the tip of the laser-pen gets closer to the mirror, the range of angles for both the laser-pen pan-and-tilt mechanism and the mirror pan-and-tilt mechanism increases. The 5in x 5in x 6in offset was chosen as the best compromise between maximizing the range of motion for both pan-and-tilt mechanisms and minimizing the potential for collisions between the two panand-tilt mechanisms in terms of overlapping workspaces. A pan-and-tilt mechanism was selected to hold the laser-pen because it is prefabricated and ready to install needing little additional mechanical design. Another reason is that it has built in encoders to measure the pan-angle and tilt-angle of the laser. This reduced the amount of time necessary for sensor design allowing more time for controller design. The pan-and-tilt mechanisms come with S1 optical shaft encoders with 1024 clicks per revolution or degrees per tick with quaderature encoding. To achieve our performance specifications a 2cm diameter target, a minimum resolution of 0.1degree per tick is required. To increase measurement resolution to allow for some controller error, the S2 optical shaft encoders, which have 2048 clicks per revolution giving a resolution of degrees per tick, are used. The tradeoff of the S2 is its physical size and weight, but this effect should be minor and has been accounted for in the model. The operator of the Laser Deflection System generates the input angles recorded by the laser-pen encoders in real-time. Simply, the operator holds the end of the laser pen and moves the laser, which provides continuously changing inputs to the system. To physically build this pan-and-tilt mechanism, only two modifications were made. The first task was to unbolt the S1 encoder and bolt on the higher performance S2 encoder. The second task was to remove the original 3/8 s inch tilt shaft and replace it with a custom made shaft that our team designed and manufactured (see Figure?? in Appendix??). The objective of this custom shaft is to carry the laser pen directly centered on both axes and achieve the same pointing direction no matter how many times it loaded/unloaded by the user. An additional enhancement of the custom tilt shaft is that it depresses the on/off switch automatically so our laser pen itself did not need to be modified in any way. The entire manufacturing process took 2 hours and 15 minutes. Controller: Math Model: As mentioned before, the first step to designing the controller is building the mathematical model that will define the desired angle of the mirror. There were several 5

11 different approaches to developing this model. The first idea was to create a functional equation to relate the desired angles of the mirror with given input trajectory and desired target. The first attempt at solving this equation utilized vector geometry. In order to do this, first a relationship between the input angles, the angles of the mirror and the resulting reflected wave was made. This was done by breaking the input trajectory down into 2 vectors: one parallel to the mirror, and one perpendicular. First the intersection point between the laser pen and the reflective plane of the mirror was found. Basic definitions of a plane and a vector were used to determine this point. 5 By using this calculus, the x,y and z values of the intersection point were found to be: x y z int int int an x1ap + bn xp bp bn ap yp + bn y1ap + cn xp cp = an ap + bn bp + cn cp = = an ap yp - an xp bp an ap zp - an xp cp + an x1bp + bn y1bp + cn yp cp - cn bp zp + cn z1bp ll(2) an ap + bn bp + cn cp + an x1cp + bn bp zp - bn yp cp an ap + bn bp + cn cp cn ap zp + cn z1ap ll(1) + bn y1cp + cn z1cp ll(3) Where [a p,b p,c p ] and [x p,y p,z p ] are the direction and point that define the input laser trajectory, [a n,b n,c n ] and [x 1,x 2,x 3 ] are the defining direction and point for the plane, and [x int, y int, z int ] is the position of the intersection point. These equations were found by utilizing the symmetric equations of a line and the defining equation for a plane. This math can be seen in the maple file in Appendix D Using these equations, a MATLAB function vec_plane_int was created to solve for this intersection point, as seen in Appendix B. Utilizing this function, another MATLAB function was developed to calculate the reflected wave of the system when given the point and trajectory for the input laser and normal vector direction to the plane. This function, find_reflected, would calculate 2 line-plane intersection points. The first is the actual intersection point of the laser and the mirror. The second is the intersection point of the laser and the mirror if the direction of the laser was parallel to the normal vector, but opposite in direction. These two intersection points are then used to define the parallel and perpendicular vectors that, when added together, will make up the vector from the laser pen to the mirror. At this point, the reflected laser can be defined with the direction of the parallel vector minus the perpendicular vector and the calculated intersection point. Using this technique provides an accurate way to calculate the result reflected laser trajectory. However, in the controller that is being developed for the Laser Deflection System, is based on an input of the desired pan-and-tilt angles for the mirror. Therefore an equation must be calculated that can solve for these angles. First, the transition from angles to direction vector and back was found. (This equation also holds true for finding the input trajectory from the given angles from the input pan-and-tilt mechanism.) These transformation equations to the direction vector, [a n, b n, c n ], given the pan and tilt angles, pθ and tθ, are: 5 Geometry definitions provided by: Edward; Hostetler; Larsen; Calculus: Early Transcendental Functions 6

12 a n = sin(pθ) * cos(tθ) (4) b n = sin(tθ) (5) c n = cos(pθ) * cos(tθ) (6) And inversely: tθ= sin -1 (b n ) (7) pθ= sin -1 (a n / cos(tθ)) (8) In the case of pθ, solving using the a n was chosen in order to keep the proper sign of the angle. For example, if the c n equation was used, a negative and positive value for c n / cos(tθ) would return the same value for pθ. In order to actually create an equation to solve for the mirror angles, two approaches were taken. The first approach was a direct approach that would provide an equation that would solve for the mirror pan and tilt angles given the desired target and input trajectory (Figure 3) This direct approach was calculated using the program Maple and was solved in two ways. The first was to use the foreword equation to find the direction reflected laser, then (knowing the reflection point) solve for the needed reflected trajectory, and set these two directions equal to each other. This equation could then be solved for the mirror angels; however, due to the complexity of the problem, there is no inverse solution to the equation. These Maple files can be found in Appendix D. Figure 3: Math Model Approaches Another direct approach to determine the relationship between the input angles on the laser-pen pan-and-tilt mechanism and the output angles of the mirror pan-and-tilt mechanism is to use a vector geometry transformation approach. This method also showed that there was no inverse solution. The transformation can be achieved by determining the following: The origin of the world coordinate system (wcs) is located at the base of the mirror pan-and-tilt mechanism. The origin of the laser-pen coordinate system (bcs) is located at the base of the laser-pen pan-and-tilt mechanism. The output on the wall has a coordinate system (ocs) located at a predefined point in the wcs which is [x, y, z] = [5, 5, 36] A scalar position vector from the wcs to the bcs. A scalar position vector from the wcs to the ocs. 7

13 The laser-pen tip is located at the intersection of the pan axis of rotation and the tilt axis of rotation. This gives a scalar position vector from the the bcs to the tip of the laser. The center of the mirror, its reflection plane, and the mirrors normal vector intersect at the pan axis of rotation and the tilt axis of rotation. This gives a scalar position vector from the wcs to the aforementioned point. Now the two angles on the laser-pen pan-and-tilt mechanism are set to zero and this defines the home position configuration of the laser-pen panand-tilt mechanism. In this position, the scalar vector is [0, 0, -1]. Now the two angles on the mirror pan-and-tilt mechanism are set to zero and this defines the home position of the mirror pan-and-tilt mechanism. In this position, the scalar vector is [0, 0, 1]. A vector from the wcs to the input laser. Written in terms of variables. A vector from the wcs to the reflected laser. Written in terms of variables. Now these expressions will be rotated and transformed appropriately giving an expression that directly relates the input angles to the normal vector of the mirror. An output reflected laser can now be defined giving the desired mirror pan-and-tilt mechanism angles. Since no inverse solution could be found, the second approach is an indirect method of finding the pan and tilt angles for the mirror (Figure 3B). This method involves using the foreword equation to find the reflected dot, and calculating the error between this dot and the desired dot. By finding the values for the pan and tilt angles of the mirror through guessing that minimizes the error, the desired mirror position can be found. This is done in an efficient way by utilizing the MATLAB function fminsearch. Fminsearch returns the input value that will minimize the output the function, given an initial guess. This function, find_desired_norm, can be found in Appendix B, and was able to accurately calculate the desired mirror position. The only draw back to this solution is that this function will not be able to work real-time. Due to the recursive approach and the use of a MATLAB based function, find_desired_norm will not be able to run real-time on the DSP, and therefore another solution must be found. The only solution is to use a lookup table, which can be best described as a sampled representation of a function. This was the solution used for the Laser Deflection System. In our case, the index for the lookup table will be the input angles (pan & tilt angles) and the results will be the desired angle either the pan or tilt. Therefore, we need two lookup tables, one for each angle. The Look-Up Table (n-d) block in Simulink evaluates a sampled representation of a function in N variables by interpolating between samples to give an approximate value for F(x 1, x 2 ), even when the function F is known only empirically. In order to create the database for the lookup, the make_lookup() function was created. The make_lookup() function is composed of two loops that will iterate every possible combination of pan and tilt angles with a step size of 0.5 o. This function utilizes the indirect approach explained above to calculate the desired angles and create the database. In order to check our matrix to see if it reflects an acceptable shape, and to find 8

14 the pan and tilt limits, at first only the input trajectories that would intersect our 5in mirror were saved. Due to symmetry, it was seen that a successful lookup table was made. The make_lookup() function outputs the Pan desired and Tilt desired matrixes, error matrix (the final expected error for each combination), and the row and column matrixes for the lookup. (The make_lookup() implementation is shown in Appendix A.) Pan-and-Tilt: There are several requirements for the physical pan-and-tilt mechanism itself. First a 5in diameter mirror was mounted on the tilt shaft of the pan-and-tilt. However, the pan-and-tilt mechanism given to us does not fit this mirror, so we had to do some modification to the system. This initial design was created in SOLIDEWORKS. Appendix A shows this design. Before manufacturing the system, the pan-and-tilt was modeled. The moment of inertia matrix, center of mass, and weight of the system (without motor and load) was taken from Solid Works. The final inertia matrix, center of mass, and weight was done using Matlab GM9234S016_pantiltinit.m function (Appendix B) The second requirement for the pan-and-tilt was the controlling motor and gear selection. A number of motors were investigated with respect to a simple controller with a sinusoidal input. All Pitman GM8000 series failed to meet the torque and speed requirements. Therefore, we went up to GM9000 series, the first, and cheapest, motor in the list (GM9234S017 with a gear of 3:1 on the pan side and 2:1 on the tilt side) allowed more than enough torque for our application. This will give us room in implementing tougher controls. Once all parts were ordered, the pan-and-tilt was manufactured. This pan-tilt needed significant redesign to carry a 5 diameter mirror. The design work was done on SOLIDWORKS. This gave us the ability to consider the physical dimensions and placement of the motors, mirror, and other hardware so we could minimize the system s inertia. The yoke and the bearing blocks were remanufactured with one change, an increase in length. Designing the shaft was challenging and our team went through several iterations to achieve a shaft that achieved the following objectives: directly center the mirror on both the pan and tilt axes, place the reflection plane of the mirror on each axis centerline, securely bond the mirror and the shaft together in such a way that is safe to operate by the user. It took 5 hours and 30 minutes to manufacture these parts and assemble the pan-tilt. The final design can be seen in the Appendix A. Combination of Subsystems: Once each part of the system is completely designed and built, the next step is to incorporate all the parts together. The first step is physical building. Our design specifications required both pan-tilts be mounted a specific distance apart, meaning that the X, Y, and Z measurements between the pan-tilts must be precisely setup or our systems performance could be significantly compromised. Both pan-tilts were mounted on a common piece of Lexan. This was stiffened up and supported by 2in by 4in blocks of wood allowing clearance for the pan motor below the Lexan mount. We positioned 9

15 the pan-tilts as close to the desired position as possible and then securely bolted each pantilt base to the Lexan. The next step was to take accurate measurements of the X, Y, and Z distances and replace the desired values of the math model with our actual values. The manufacture and measurement process took 3 hours and can be seen in the Appendix A. When the Laser Deflection System was completely manufactured, the next step was to implement the math model, controller and physical system together. This was tested by verifying correct controller responses due to input variations. Once this test was complete, the controller was tuned for a finished product. Controller Development: The Laser Deflection System is a nonlinear system. Therefore, the first step in controller design is to linearize the system over a specific point, or angle. This is done by through Taylor series expansion of the nonlinear system. 6 The MATLAB linearization program, pantilt_lin, provided by the course TA Ben Postaid, accomplished this. The system was linearized over the angle [0,0]. Physically, this means that the mirror will be upright or facing sideways. Because of the symmetry of the system, second order transfer functions were found to model the pan-and-tilt. Transfer function from pan to output G11 = (9) s s Transfer function from tilt to output G22 = (10) s s In order to create the most accurate controller, all steady state error must be eliminated. Therefore the design began with a PID controller. The Laser Deflection System on its own is a type 1 system. After adding the PID, the system will have a second pole at the origin which means there should be zero steady state error for a step and ramp response and a finite error for a parabolic response. Our system will have a random input, simulated by the user input; therefore a response is difficult to estimate. The PID controller is of the form: ki kd s (1 + s / z1)(1 + s / z2) C( s) = K p + + = k (11) s s / p + 1 s(1 + s / p) The purpose of the additional pole on the derivative portion of the controller is to create a cut of for very high frequencies. A derivative controller is multiplied by the frequency and will magnify high frequency noise at the output. This extra pole is added to cancel the magnifying effect. The controller was designed by analyzing the step response and bode plot. These figures give a good estimate of how the controller should react. In order to do this, the MATLAB tool rltool was used. This tool allows the placement and movement of poles 6 Wen, MATLAB and Simulink overview. 10

16 and zeros with immediate plotting of the desired plots. Using this tool, fast controllers were found: Pan Side: ( s + 1.4)( s + 1.4) C 11( s) = 104 (12) s( s + 180) Figure 4 : Pan Controller without actual friction Tilt Side: ( s + 1.4)( s + 1.4) C 22 ( s) = (13) s( s + 180) Figure 5: Tilt Controller without actual friction Both compensators had Percent overshoot < 5% and settling time < 0.3sec. These values did not quite meet specifications, but seemed to be acceptable. In addition, the 11

17 designed controllers have a large bandwidth and a small phase shift over operable frequencies (~6 rad/s). Again, these characteristics are not exact to our specification, but it gave a very good idea on the response and frequency range. These controllers give a good beginning to a model that was used to design the controller once proper friction identification was done. The friction measurement was completed by running the pan-and-tilt several times with various torque inputs to the system and measuring the steady-state velocity for each torque. The results should give a graph that look like Figure 6 7 : Figure 6: Friction Response From the graph, Fv represents the viscous friction of the system and Fc represents the coulomb friction. This means that the dead-band offset is the coulomb friction and the slope is the viscous friction. The friction identification was done after having the mechanical part built. Using the ARCS software with the ability of real-time data acquisition and adjustment of DSP variables, friction identification was completed. The MATLAB files, frictdata.m & frictid.m, created by the course TA, Ben, used the acquired data to solve for the viscous and coulomb friction. The actual values found can be seen in Appendix B, and the estimated values used in simulation can be seen in the table below. Table 1: results of Friction measurement Friction Viscous Coulomb Pan Side Tilt Side The new linearized system with the friction calculation was found to be (G11 = pan axis, G22 = tilt axis): G11 = (14) s s 7 Wen; Friction Identification, model parameter identification, ARCS Interactive Development Environment (AIDE). 12

18 110 G22 = (15) s s Due to friction, the pole and zero placements of the system have changed, and the compensator was modified with the same method accordingly. Pan Side: ( s +.4)( s +.4) C 11( s) = 77 (16) s( s + 175) Figure 7: Pan Controller with actual friction Tilt Side: ( s +.4)( s +.4) C 22 ( s) = 94 (17) s( s + 180) Figure 8: Tilt Controller with actual friction 13

19 The response results meet our specification of a 0.1s settling time and the overshoot is close to 1%. In addition, the bandwidth is large, giving a bigger frequency range that the controller will be able to compensate for. However, there are some drawbacks to this design. The bode magnitude diagram shows a bump at some high frequencies. This bump will result in magnifying any high frequency noise or input in the system. However these bumps occur at relatively high frequencies compared to our 6rad/sec specification and should not have a large affect the controller. After finding the compensators, and most importantly finishing the building process, physical testing began using the DSP with ARCS Interactive Development Environment (AIDE). The ARCS software allows the design of a controller through Simulink, and will automatically generate machine code for the DSP. The Laser Deflection System is a feedback system that controls the position of the mirror. In a physical sense, the desired angles will be calculated using the lookup table, the torque will be the output to the system, and the encoder measurement will act as the feedback for the system. In general, the designed controller had a similar form to Figure 9: Figure 9: Controller block diagram Since the controllers were designed in a lineralized system, the viscous friction is taken in consideration in the controller design. However, the coulomb friction cannot because it is nonlinear, dependent on the system if it is moving or not. Coulomb friction will resist motion in the opposite direction of velocity. Therefore, in order to compensate for that friction, a block was created in the design that will derive the direction of the system velocity at that instance and adding coulomb compensation in that direction, as seen in Figure 9. Figure 10: Block diagram with coulomb friction compensater 14

20 The actual Simulink file used is in the Appendix B. Because the code developed will be interrupt based, a discrete controller must be used. To do this, the controller converted to discrete system using the c2d function in MATLAB. The default method of zero order hold transform was used. Once the system was up and running, it was seen that the designed controller was not responding as expected. The mirror movement was slow and inaccurate. It was later found that this was due to an inaccuracy in the simulated model of the system (Refer to the verification portion of the report). Because of this error, a new controller was built by analyzing the physical system as it moved. To allow flexibility with the controller in making modification while running in real-time, a new form of the controller was implemented into the system. This form will allow changing of the gains K P, K D K I on the fly. In order to transform the controller, the following equations were used: ( z1 + z2) p z1* z2) K p = K 2 p ( z1 + z2) p z1* z2) (1 ) 2 p Kd = K (18) p z1* z1 Ki = K p At this point the system needed higher proportional and derivative gain. (Higher Kd will decrease overshoot and higher Kp will decrease settling time). The controller design from this point on was based on the physical response of the system. For example, sluggish movements in the controller required an increase in proportional gain, and an overshoot required a larger derivative gain. When testing began, it was seen that controlling the resulting laser to stay within 1cm is difficult if not impossible with the given time and resources. At this point the goal was changed to try to design the controller within 1in of it target instead of the 1cm. During testing, several errors were found. Steady state error was created, because at slow movements friction compensation is ignored and the controller is unable to move the mirror. This is due to the quantization effect of the encoders. Quantization also was the source of some oscillations in the system. Because the derivative gain is effected by the speed of the system, major jumps in velocity will be amplified. The quantized system would create impulses in velocity, causing the mirror to overshoot and then oscillate. To avoid this, the derivative gain was kept at a low value. One way to reduce the quantization effect is to implement a Kalman filter. The Kalman filter would properly estimate the velocity and position of the pan-and-tilt mechanism, reducing the quantization effect. However, due to time constraints, this was not implemented. Through much testing and verification, a final controller was chosen. The gaines used for the Laser Deflection System can be seen in the table below. Table 2: Final Pan Controller K p1 15 K d1.5 K i1.5 P

21 Table 3: Final Pan Controller K p2 17 K d2 1.5 K i2.7 P Figure 11: Pan Real Controller simulated 16

22 Figure 12: Tilt Real Controller simulated 17

23 Professional and Societal Considerations: Extensive searching has found no products matching this project. However, some concepts of this project are used in commercially available products such as satellite-tosatellite communication and plane-tower laser communication systems. Comparing all these products demonstrates the significant difference in technology resulting in the unlikelihood of any patent infringement. This is a unique project, so it can be considered for commercialization. Unfortunately, it s only possible use could be for a teaching aid in controller design that seems implausible. The idea of the Laser Deflection System would be to guarantee a communication link despite variations of the laser input. This method of data transfer is advantageous because it is difficult to intercept and because of the small controlled area of information transmission. For example, battlefield communication could use this very secure transmission method to guarantee secure 2 way contact. The world today revolves around information, thus a communication link for this information should be flawless. If the Laser Deflection System was used to transmit data, the emphasis would be on maintaining the communication link. Any loss of information could greatly affect a large amount of people. 18

24 Design Verification: Motors & Gears: In order to choose the right motor for our system, the controller must first be designed. However, ordering became a priority before a correct controller was designed. To get around this a simple PD controller was used as a test controller, with a sinusoid input as test input conditions. A number of motors were selected that differs from each other by several specifications. The specification varies from a low torque and high speed to high torque low speed. The motor chosen was the only motor tested that could handle the required torque and speed requirements for this system. It has high speed and high torque. The graph below shows the variety of motors tested. Also, an example of the plot to test the motors for both a failing motor and the selected motor can be seen in table 4 and??. Table 4: Test Motor Specifications and Results Motors Weight (g) Torque Cst. (N-m/A) Inter Gear No Load Speed rad/s Stall Torque N-m Rotor Inertia Kg-m 2 Worked? Price $ GM e e-1 9.2e-7 N GM8724S e e-1 1.6e-6 N GM8724S e e-6 N GM8724S e e-6 N GM9234S e e-6 Y Prices are according to website Figure 13: Motor Responses Input/Controller accuracy: The next verification step is to check the necessary accuracy of the controller and input trajectory calculation. These calculations can be vital to reducing the output error. To determine how accurate each pan-and-tilt mechanism has to be was calculated by using the MATLAB function find_reflected_doterr, as seen in Appendix B). Tests were made that pointed the laser directly at the center of the mirror, and calculated what output 19

25 error would be caused by various angles of the pan-and-tilt. This result can be seen in the figure below. Figure 14: Controller Error Figures??-?? show the limits for the pan angle. The graph shows that in order to stay within one centimeter of the desired dot, the angle has a range of about 0.6 degrees. This allows a variance of 0.3 degrees in either direction. Figure 15: Pan Controller Error A similar result was found for the tilt angle of the controller. This plot shows that the tilt angle can stray about 0.35 degrees away from the desired value. These limits are quite limiting on the controller and will prove to be difficult to obtain. This may cause in reducing the strictness of the controller and allowing a wider range for the output to stay within. Figure 16: Tilt Controller Error 20

26 The same test was done, however this time changing the angles of the input panand-tilt. The figures below show the results. It is seen that in order to stay within the one centimeter output limit, a range of about a degree is allowed for the input side on both pan and tilt directions. However, it is desirable to reduce all output error due to a poor measurement on the input side. This is why the more accurate encoders, S2, will be used for the input purpose. As shown above the S2 will provide degrees per tick. It was calculated that a 0.1 degree error in the input measurement will result in about 0.2 centimeters error. This was shown using the following MATLAB code: >> [r,u,p]=find_reflected([0,-2.5,6], [0, ,-1], [0,0,1]); >> [x,y,z]=vec_plane_int(p,u,[0,0,-1],[0,0,36]) x = 0 y = z = 36 >> [r2,u2,p2]=find_reflected([0,-2.5,6], [0, ,-1], [0,0,1]); >> [x2,y2,z2]=vec_plane_int(p2,u2,[0,0,-1],[0,0,36]) x2 = 0 y2 = z2 = 36 The S2 with quaderature encoding will allow for twice that accuracy, providing the most accurate way to calculate the input trajectory. Figure 17: Input Accuracy Results 21

27 Linear and Non-linear Comparison: During the design process, it is important to check continuity between the linear and non-linear model. In order to test this, a test Simulink file was created which put each system, linear and nonlinear, under realistic conditions. The test file was created so that a trapezoid trajectory would be inputted into either the pan or tilt side of the controller. From here, the results from the linear model (blue), were plotted againt the results from the nonlinear model (green). These results are below. (The red curve represents the desired angle.) Because this test was done during the simulation part of the controller design, the second designed controller, the simulation controller with correct friction estimation, is shown above. The graphs show that there is little discrepancy between the two models. On the pan side, there is a slight offset, and on the tilt side there seems to only be a difference in steady state error. This difference between the models is due to the fact that coulomb friction is not included in the linear model. However, because the two models are so similar, using the linear model for design is acceptable. 22

28 Model Accuracy: Once the physical system was built, and our controller design was implemented, it was seen that the system did not respond as expected. After some testing, it was shown that the model of the Laser Deflection System did not correctly identify with the physical system. This is the reason for designing a controller specifically for the physical system. This difference is best shown through an open loop test of the system. This was done by forcing a constant torque to the system and plotting the position versus time. Large torque values, in comparison to the average found in typical tests, were used to show this difference over a short time, however, these torque values are reasonable and well under the saturation torque. The results for the modeled system (in green) and the physical device (in blue) are in Figures 18. This test proves the inaccuracy of the model, showing an difference of 4radians on the pan side and a 1 radians on the tilt side after only 1 second. Figure 18: Open-Loop Response Actual vs. Simulated Controller Performance: During the controller design, the performance of the controller was tested through the observation of how accurately that the mirror reflected the laser. Doing this allowed for a good design of a controller, however, before the design was complete, several performance tests were conducted. The first test, evaluated how the performance of the controller with a random input introduced to the input laser pen. The input pen angles, mirror angles, and several other values were recorded and evaluated. Using the mathematical model, a plot of the resulting laser point on the screen was calculated for every recording time-step (0.01s). Figure 19 shows the response for a typical random input to the Laser Deflection System. 23

29 Figure 19: Response of a Random input Each blue point in the plot above represents the laser point at any specific recording time for this experiment (#1). The red points indicate where the laser would have appeared on the screen if the exact desired values were achieved. This example shows the accuracy of the look-up table (by the location of the red points) and the accuracy of the controller (by the location of the blue points). The green circle shows the allowable error according to specifications of 1in away from the desired target of x =-5in, y = 5in. This test was run several times in order to test the controller under a variety of variable inputs. A summary of these experiments can be seen in Table 5. (Detailed figures describing these experiments can be found in Appendix??, page??) This table quickly shows that the controller works well under a slower changing input. In these cases, it is seen that the laser will be within the desired target range at least 89% of the time if the pan speed stays below 3 rad/s and the tilt speed stays below 5.4 rad/s. These experiments also show that the amount of torque used in the typical use of our system does not fully utilize the power of the motors used. This means that a more aggressive controller could have been implemented into the system. Table 5: Error, Torque & Speed Measurements Measurement\Experiment # Hit Rate (%) Avg. Pan Error (rad) Max. Pan Speed (rad/s) Avg. Pan Speed (rad/s) Avg. Pan Torque (N-m) Max. Pan Torque (N-m) Avg. Tilt Error (rad) Max. Tilt Speed (rad/s) Avg. Tilt Speed (rad/s) Avg. Tilt Torque (N-m) Max. Tilt Torque (N-m)

30 The second performance test was to physically recreate a step response for our system. In order to represent the worst possible error compensation needed for our system, each degree of freedom for the pan and tilt was tested with a step from the maximum and minimum angle allowed in the Laser Deflection System. In order to simplify this test and to make this physical step comparable to simulation, each axis was tested separately. This was done to reduce the effects of one axis on the other. Also, because of the difference in friction, the step response was tested for both a positive a negative step. Finally, the torque applied to the motors was also recorded and plotted, showing a good estimate of largest possible range of torque used for our system. The results for the pan sub-system can be seen in the figures below. Figure 20: Step and torque Responses on Pan Side Figures 20 show the positive and negative step response for the pan axis respectively. In these figures, the red curve shows the desired angle, the blue curve represents the physical response of the Laser Deflection System, and the green line is the response of the final controller used with the simulated model. These plots reiterate the fact that the simulated model had an error in it. If the model was correct, the simulated response should match the physical one. 25

31 Overall, the pan design provides a good compromise between a positive and negative error. The positive step response seems to be an over damped system with a small steady state error. On the other hand, using the same controller, the negative step response shows a slightly under damped system with a small oscillation as it s drawback. Between the two, the final design is a good compromise, with a quick response, a small overshoot, if any, and a small steady state error. Finally, with the maximum step inputs, the motor s power was used. In both cases, the torque used either came close to or met the saturation torque of 9.89 N-m. Similarly, the tilt controller was designed to receive an accurate and quick response for an error in either direction. Figures 21 show these responses. For the tilt side, both responses show an over damped system with a small steady state error. Similar to the pan side, these plots also show the difference between the simulated result and the physical result. In Figures 21 the torque applied to the tilt motor is shown. Again, the motor s potential was fully used, needing a torque of N-m, the saturation torque. Figure 21: Step and Torque Responses for the Tilt Side 26

32 This design does not quite meet the original specifications (seen in Table 6), however the system still performed with impressive accuracy. In fact, without disturbance correction, the final laser error had a maximum error of about 15in with an average of 13in on each side (top, bottom, left and right). The Laser Deflection System corrected for most disturbances, keeping the laser within one inch from the desired point. This results in a 1300% improvement in accuracy. Table 6: Specification Comparison Specification Actual Performance I/P Range of motion ( O, {rad}) 53, {0.925} Pan: 54, {0.94} Tilt: 40, {0.70} Controller range of motion ( O, {rad}) 35, {0.61} Pan: 26.4, {0.46} Tilt: 40.8, {0.71} Speed ( O /s, {rad/s}) 340, {6} ~170, {~3} Error (in, {cm}).394, {1} 1, {2.54} Settling time (s) * < 0.1 Pan[+,-]: [0.28, 0.36] Tilt[+,-]: [0.58, 0.52] Percent Overshoot * < 1% Pan[+,-]: [0%, 1.5%] Tilt[+,-]: [0%, 0%] Steady State Error ( O, {rad}) * < 0.1, {<.0017} Pan[+,-]: [-0.46, ], {-0.008, } Tilt[+,-]: [-0.34, 0.57], {-.006,.01} * Found from step responses (+ = positive step, - = negative step) Finally, a test of how the accurate the controller is under various frequencies was performed. Again, because there is a random input to the Laser Deflection System, there can be various frequencies at which the controller must perform. The goal of the design was to design a controller to maximize the range of controllable frequencies. In order to test, a sin input with the largest possible amplitude in the Laser Deflection System s range was applied to the system as the desired angles. Again, to reduce the disturbance between axis for this test, each degree of freedom was tested separately. This test was run over various frequencies and the actual and desired angles were plotted over time. An example of a low and a high frequency test on the pan axis can be seen below in Figure 22. The overall effect of frequency on the system is summarized in Table 7. (For more detailed plots, see Appendix?? Figures??-??.) 27

33 Figure 22: Sinusoid Response for the Pan Side Table 7 : Frequency Response Results Max. Pan Error vs. Frequency Frequency (Hz) Max. Error (rad) Visible Magnitude Change Visible Phase Shift None None None None None None Small reduction None Large reduction Large shift Max. Tilt Error vs. Frequency Frequency (Hz) Max. Error (rad) Visible Magnitude Change Visible Phase Shift None None None None None None None None Small amplification Small phase shift This table shows that the controller works well for all pan frequencies under 20 Hz and all tilt frequencies less than 40 Hz. This effect can best be explained by analyzing the bode plots in Figures 11 and 12. The frequency values are not comparable because the bode plots were created using the faulty model of our system, however a similar frequency response shape is expected in the physical system. The experimental data for the pan system shows that the controller will begin acting like a gain reduction at about 15 Hz, and will introduce a phase shift around 20 Hz (see Figure 22). On the other hand, the tilt system will act as a magnifying gain at around 40 Hz with a phase shift, and is expected to reduce the gain as the frequency is increased. 28

34 Costs: The cost of the project is broken down into three categories: List of Parts, List of Raw Materials, and Team Labor. Below are itemized tables of each category and a 4 th table that combines them for a total cost. Table 8: List Of Parts, Materials, and Team Labor List Of Parts Quantity Price ($) Total ($) Item Source 1 $3.97 $3.97 5'' diameter glass mirror Walmart 1 $31.99 $31.99 Laser pen Radio Shack 1 $97.59 $97.59 Pan motor, Pittman GM9234S017 Clickautomation 1 $97.59 $97.59 Pan motor, Pittman GM9234S017 Clickautomation 1 $9.97 $9.97 Pan Gear A, A6A16-060DF2508 sdn-si.com/estore 1 $22.02 $22.02 Pan Gear B, A6A16-120NF2512 sdn-si.com/estore 1 $7.95 $7.95 Tilt Gear A, A6A16-030DF2508 sdn-si.com/estore 1 $22.02 $22.02 Tilt Gear B, A6A16-120NF2512 sdn-si.com/estore 1 $3.92 $3.92 Pan Belt, A6A sdn-si.com/estore 1 $3.62 $3.62 Tilt Belt, A6B sdn-si.com/estore $0.00 $0.00 Misc. nuts and bolts CSD Lab Total: $ List Of Raw Materials Quantity Price ($) Total ($) Item Source 1 $0.75 $0.75 1/8'' x 1/2'' x 4.5'' Al, Mirror support Home Depot 1 $4.50 $4.50 1" x 7'' Al, Tilt shaft RPI Machine Shop 2 $1.25 $2.50 3/8'' x 1 1/4'' x 4'' Al, bearing blocks RPI Machine Shop 1 $2.00 $2.00 3/8'' x 1 1/4'' x 6'' Al, yoke RPI Machine Shop 1 $11.00 $11.00 ½ x 13 x 10 Lexan, base RPI Machine Shop Total: $20.75 Team Labor Rate Hours ($/hr) Total ($) Person 264 $30.00 $ Jack Damerji 276 $30.00 $ Matt Dileo $30.00 $ Tyler Ferman Total: $ Project Total: $

35 Conclusion: The Laser Deflection System is a device that is to be used in laser communications. It is a system that is meant to keep a laser communication link despite any measurable input disturbances. Because of the speed of communication today, and the severity of losing information if a signal is lost, the specifications for the Laser Deflection System are extremely demanding. Because of this, the controller for the system is also demanding and difficult to design. Three qualified engineers that used this experience to build off from prior class work took this challenge. Every topic in the production of the Laser Deflection System involved theories, ideas and methods covered in several engineering courses. The design of the math model used concepts ranging from Robotics to Calculus. The ideas of transfer functions, basic controllers, and frequency response were introduced in Signals and Systems. These ideas, along with topics including the linerization of a system, controller theory, controller design, continuous to discrete transformations, and state estimation (Kalman filter) were covered in Discrete Time Systems, Systems Analysis and Techniques and Control Systems Engineering. Overall there were several successes and challenges to the Laser Deflection System. Successes included the design of a robust controller that resulted in a 1300% disturbance correction and a quick interaction between a random user input and controller response through an accurate mathematical model. Some of the challenges included difficulty in the design of an accurate system model, quantization effects, angle calibration and the difficulty of compensating for a random disturbance. If the Laser Deflection System had more time for development, there is much room for improvement. First, to make controller design easier, an accurate model of the system should be built. This would involve correcting the error that must lie in inertia, mass or friction calculations. Secondly, to guarantee proper initialization, a calibration system could be implemented. Third, to reduce quantization effects and create a more accurate controller, a Kalman filter can be added to the system. Finally, for a more reliable controller, an adaptive controller could be used that would adapt so different input conditions such as speed and frequency. Building the Laser Deflection System was a valuable learning experience. This project introduced several considerations in designing a controller beyond poles, zeros and frequency response. Topics such as modeling, motor and gear selection, quantization, electrical physics and dynamics all must be understood for proper controller design. Development provided a real life aspect to controller design, and gave experience in solving problems using modeling, physical development and controller design. 30

36 Report Break Up: Introduction: Matt DiLeo Design Procedure: Jack Damerji Design Detail: Input: Tyler Ferman Math Model: Matt DiLeo (vector transformation approach done by Tyler) Pan-and-Tilt Model: Jack Damerji/Matt DiLeo Controller Development: Jack Damerji Professional and Societal Considerations: Tyler Ferman Design Verifications: Motor and Gears: Jack Damerji Input/Controller Accuracy: Matt DiLeo Linear and Non-linear Comparison: Matt DiLeo Model Accuracy: Matt DiLeo Controller Performance: Matt DiLeo Costs: Tyler Ferman Conclusion: Matt DiLeo Editing: Matt DiLeo Formatting: Jack Damerji Jack Damerji: Matt DiLeo Tyler Ferman: 31

37 Bibliography Brandon, William T.; Chan, Vincent W.S.; Kwan, Robert K., JAPANESE RESEARCH SATELLITE PROJECTS, July 1993, ClickAutomation, 2001, Edwards, Bruce H.; Hostetler, Robert P; Larsen, Ron; Calculus: Early Transcendental Functions; Houghton Mifflin Co.; Boston, NY; Khosla, P. K.; Riviere, C. N., Error Compensation in Human-machine Interfaces, Carnigie Mellon University, Oct. 1997, Pittman,2001, Ship Shape Electronics Co., 2002, Space Daily, Paris, 2001, Stock Drive Products/Sterling Instrument, 2003, Technical Manufacturing Corporation, 2002, Wen, John, Friction Identification, model parameter identification, ARCS Interactive Development Environment (AIDE), class notes for ECSE-4962, Departmant of Electrical and Computer Engineering, Rensselaer Polytechnic Institute, Spring Wen, John, MATLAB and Simulink overview, class notes for ECSE-4962, Departmant of Electrical and Computer Engineering, Rensselaer Polytechnic Institute, Spring

38 33

39 Appendix A: SolidWork Plots I

40 II

41 III

42 IV

43 Appendix B: ********************************************************** * Edubot Joint Friction Auto-Identification Program * * - Friction Parameter Identification Program - * * by Ben Potsaid 7/10/00 * ********************************************************** coulomb_neg = coulomb_pos = viscous_neg = e+002 offset_neg = viscous_pos = e+003 offset_pos = viscous_p = e-004 viscous_n = V

44 JOINT 2 ********************************************************** * Edubot Joint Friction Auto-Identification Program * * - Friction Parameter Identification Program - * * by Ben Potsaid 7/10/00 * ********************************************************** coulomb_neg = coulomb_pos = viscous_neg = e+002 offset_neg = viscous_pos = e+002 offset_pos = viscous_p = viscous_n = VI

45 VII

46 Figure 23: Results of Experiment1 VIII

47 Figure 24: Results of Experiment2 IX

48 Figure 25: Results of Experiment3 X

49 XI

50 Figure 26: Results of Experiment4 XII

51 Figure 27: Pan Sinusoid Response XIII

52 Figure 28: Tilt Sinusoid Responce XIV

53 Figure 29: Tested Motor Measurements %angle_calc.m function angles=angle_calc(dir) tilt=asin(dir(2)); pan=asin(dir(1)/cos(tilt)); angles=[pan,tilt]; % COMPOSITEBODIES.m calculates the mass and inertia properties for a 2 body composite % Ben Potsaid Jan 21, 2003 % [pc, mc, Ic] = compositebodies(p1, m1, I1, p2, m2, I2) % % -- input parameters -- % p1 is the 3x1 position vector from a coordinate reference frame to the center of mass of body 1 % m1 is the mass of body 1 % I1 is the 3x3 inertia tensor for body 1 about the center of mass of body 1 % p2 is the 3x1 position vector from a coordinate reference frame to the center of mass of body 2 % m1 is the mass of body 2 % I2 is the 3x3 inertia tensor for body 2 about the center of mass of body 2 % % -- returns -- % pc is the position vector to the center of mass of the composite body % mc is the mass of the composite body % Ic is the 3x3 inertia tensor for the composite body about the center of mass of the composite body function [pc, mc, Ic] = compositebodies(p1, m1, I1, p2, m2, I2) %calculate the center of mass for the composite body [pc, mc] = masscenter(p1,m1,p2,m2); % perform parallel axis to compute inertia tensor for the composite body [Ic] = parallelaxis(p1,m1,i1,p2,m2,i2,pc); XV

54 %% find_desired_norm.m function [Ang, int]=find_desired_norm(vp,vdir,walldir, desiredtarget, guess) %finds direction of the normal vector needed to reflect an incoming vector to a desired point on a wall %FIND_DESIRED_NORM [n,val]=find_desired_norm(vp,vdir,wallpos,walldir, desiredtarget, guess) % Takes in a 3X1 vector which defines a point on the line, a 3X1 vector which % defines the direction of the line and a 3X1 vector that defines the direction % of the vector normal to the wall, a 3X1 vector that defines a point on the wall, a % 3X1 vector that will define the desired target position, and an initial guess for % the the 2 angles of the mirror ([thetap,thetat]) %Inputs vp - point vector % vdir - line direction vector % walldir - normal vector direction of the wall % desiredtarget - the desired point to hit on the wall % guess - the initial mirror normal vector guess %Outputs n - the needed direction of the normal vector of the mirror % val - the error of the reflected dot on the wall % optimset(optimset,'maxfunevals',15); optimset(optimset,'display','off'); options=optimset; val=10; vguess=dir_calc(guess); num=0; [vec,val]=fminsearch('find_reflected_doterr',vguess,options,vp,vdir,walldir,desiredtarget); n=vec/norm(vec); a=angle_calc(n); [xi,yi,zi]=vec_plane_int(vp,vdir,n,[0,0,0]); Ang = [a(1),a(2)]; int = [xi,yi,zi]; %Find_reflected_dot.m function [x,y,z]=find_reflected_dot(vp,vdir,pnorm,wallpos,walldir) %finds direction of reflected vector %FIND_REFLECTED_DOT [reflected,u_reflected,reflected_point] = find_reflected(vp,vdir,pnorm) % Takes in a 3X1 vector which defines a point on the line, a 3X1 vector which % defines the direction of the line and a 3X1 vector that defines the direction % of the vector normal to the plane %Inputs vp - point vector % vdir - line direction vector % pnorm - plane normal vector direction %Outputs reflected - direction of reflected laser % u_reflected - unit direction of reflected laser % reflected_point - intersection point of mirror and laser % XVI

55 [xi,yi,zi]=vec_plane_int(vp,vdir,pnorm,[0,0,0]); [xi2,yi2,zi2]=vec_plane_int(vp,-1*pnorm,pnorm,[0,0,0]); i2=[xi2,yi2,zi2]; i=[xi,yi,zi]; par_vec=i-i2; vec=i-vp; perp_vec=vec-par_vec; reflected=par_vec+perp_vec*-1; u_reflected=reflected/sqrt(reflected(1)^2+reflected(2)^2+reflected(3)^2); reflected_point=i; [x,y,z]=vec_plane_int(reflected_point,u_reflected,walldir,wallpos); %end find_reflected find_reflected_doterror.m function [sqerr]=find_reflected_doterror(pnorm,vp,vdir,walldir,desired) %finds direction of reflected vector %FIND_REFLECTED_DOTERR [sqerr]=find_reflected_doterror(pnorm,vp,vdir,walldir,desired) % Takes in a 5 3X1 vectors which define a point on the input line, the direction % of the line, the direction of the vector normal to the plane of the mirror, the % normal vector of the wall and the desired reflected point on the wall %Inputs: pnorm - mirror normal vector direction % vp - point vector % vdir - line direction vector % walldir - wall normal vector direction % desired - the desired reflected point on the wall %Outputs: sqerr - the distance between the desired and actual reflected dot % [xi,yi,zi]=vec_plane_int(vp,vdir,pnorm,[0,0,0]); [xi2,yi2,zi2]=vec_plane_int(vp,-1*pnorm,pnorm,[0,0,0]); i2=[xi2,yi2,zi2]; i=[xi,yi,zi]; par_vec=i-i2; vec=i-vp; perp_vec=vec-par_vec; reflected=par_vec+perp_vec*-1; u_reflected=reflected/sqrt(reflected(1)^2+reflected(2)^2+reflected(3)^2); reflected_point=i; [x1,y1,z1]=vec_plane_int(reflected_point,u_reflected,walldir,desired); xerr=(x1-desired(1))^2; yerr=(y1-desired(2))^2; zerr=(z1-desired(3))^2; sqerr=sqrt(xerr+yerr+zerr); %end find_reflected GM9234S017_pantilt_lin.m % XVII

56 % linearized pan-tilt system about theta=0 thetadot =0 % % setup pan-tilt parameters GM9234S017_pantiltinit; IA11=I11_A; IA12=I12_A; IA13=I13_A; IA21=I21_A; IA22=I22_A; IA23=I23_A; IA31=I31_A; IA32=I32_A; IA33=I33_A; IB11=I11_B; IB12=I12_B; IB13=I13_B; IB21=I21_B; IB22=I22_B; IB23=I23_B; IB31=I31_B; IB32=I32_B; IB33=I33_B; IA=[IA11 IA12 IA13;IA12 IA22 IA23;IA13 IA23 IA33]; IB=[IB11 IB12 IB13;IB12 IB22 IB23;IB13 IB23 IB33]; ma=m_a; mb=m_b; pa=[lc_a1,lc_a2,lc_a3]; pb=[lc_b1,lc_b2,lc_b3]; N1=Ngear1; N2=Ngear2; Im1=Imj1; Im2=Imj2; % gravity g = 9.807; % get the linearized mass matrix theta1d=0;theta2d=0; M = massmatrix(ma,ia,pa,im1,n1,mb,ib,pb,im2,n2,theta1d,theta2d); % assume no friction for now d1=0.01; % viscuous friction for pan d2=0.01; % viscuous friction for tilt D = diag([d1 d2]); % get the linearized gravity term gradg=gravitylin(ma,ia,pa,im1,n1,mb,ib,pb,im2,n2,theta1d,theta2d); % define state space matrices XVIII

57 A = [zeros(2,2) eye(2,2); -inv(m)*gradg -inv(m)*d]; B = [zeros(2,2) ; inv(m)]; C = [eye(2,2), zeros(2,2)]; D = zeros(2,2); G = ss(a,b,c,d); disp('*** G ***'); disp(['open loop transfer function linearized about (',... num2str([theta1d theta2d]),')']); tf(g) G11=tf(G(1,1)); G22=tf(G(2,2)); % % controller parameters % load Gc11; load Gc22_; % sampling period ts =.005; % 200 Hz GM9234S017_pantiltint.m %Calculate the mass and inertia properties for body A clear all; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% % Calculate the position to the center of mass, the mass, and the inertia tensor for each body % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% % for the modified pan assembly %Body A with Motor p_sm = [ ]'; m_sm = ; I_sm = [ ; ; ]; %Calculate the mass and inertia properties for body B %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%% % Define some key dimensions and properties of the individual bodies. % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%% m_p = 0.055; % mass of Mirror XIX

58 r_p =.0635; %radius of Mirror th_p =.0026; % thickness of Mirror %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% % Calculate the position to the center of mass, the mass, and the inertia tensor for each body % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% % for the tilt assembly with pulley & bar p_skel = [ ]'; m_skel = ; I_skel = [ ; ; ]; p_payload = [r_p/2 0 0]'; %CG of Mirror m_payload =m_p; I_payload = [ 1/2*m_p*(r_p)^2 0 0; /12*m_p*(3*(r_p)^2+th_p^2) 0 ; /12*m_p*(3*(r_p)^2+th_p^2)]; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Start off with the skeletal body and add the hub of the pulley to form a composite body % % with new mass center at p_sh, new mass of m_sh, and new inertia tensor about p_sh of I_sh. % % One by one, add the rest of the bodies until all of the bodies in body B are included. % % Note that the hole has been included as a negative mass. % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % add to form skeletal-hub-pulley-bar-mirror [p_sp, m_sp, I_sp] = compositebodies(p_skel, m_skel, I_skel, p_payload, m_payload, I_payload); % Initialization file for Pantilt Ts = 0.001; sampletime=.001; %Sampling time of controller running on ARCS %Define motor and amplifier parameters torque_sat =.43;%2.1; Kmtr = 4.59e-2;%2.32e-2; % Motor torque constant Nm/A motor1_igr=11.5; % Motor1 internal gear ratio motor1_nls = 44.4*motor1_igr; % Motor1 no load speed at rotor in Rad/s motor1_st = 2.1/motor1_igr; % Motor1 stall torque at rotor in Nm motor2_igr=11.5; % Motor2 internal gear ratio motor2_nls = 44.4*motor2_igr; % Motor2 no load speed at rotor in Rad/s motor2_st = 2.1/motor2_igr; % Motor2 stall torque at rotor in Nm XX

59 theta1_start = 0.0; theta2_start = 0.0; thdot1_start = 0.0; thdot2_start = 0.0; % Define gravity constant g = 9.81; % acceleration due to gravity in m /(s^2) % Define Joint 1 Parameters %%PAN belt1_gr = 2; % belt gear ratio for joint 1 Ngear1 = belt1_gr*motor1_igr; % total gear ratio of internal and belt reduction Imj1 = 4.2e-6; % inertia of motor rotor in KG*m^2 as seen by motor reference fv1 = 0.01; % viscous friction for joint in NmS/Rad as seen by encoder fc1 = 0.001; % coulomb friction for joint in Nm as seen by encoder % Define Joint 2 Parameters %Tilt belt2_gr = 4; % belt gear ratio for joint 2 Ngear2 = belt2_gr*motor2_igr; % total gear ratio of internal and belt reduction Imj2 = 4.2e-6; % inertia of motor rotor in KG*m^2 as seen by motor reference fv2 = 0.01; % viscous friction for joint in NmS/Rad as seen by encoder fc2 = 0.001; % coulomb friction for joint in Nm as seen by encoder % Define Link A parameters m_a = m_sm; % mass of link A in Kg lc_a1 = p_sm(1); lc_a2 = p_sm(2); lc_a3 = p_sm(3); l_a = 0.0; % location of CG on link A in m I11_A = I_sm(1,1); I12_A = I_sm(1,2); I13_A = I_sm(1,3); I22_A = I_sm(2,2); I23_A = I_sm(2,3); I33_A = I_sm(3,3); % inertia of link 1 about CG in Kg*m^2 I21_A=I12_A;I31_A=I13_A;I32_A=I23_A; % Define Link B parameters m_b = m_sm; % mass of link A in Kg lc_b1 = p_sp(1); lc_b2 = p_sp(2); lc_b3 = p_sp(3); %l_a = 0.0; % location of CG on link A in m I11_B = I_sp(1,1); I12_B = I_sp(1,2); I13_B = I_sp(1,3); I22_B = I_sp(2,2); I23_B = I_sp(2,3); I33_B = I_sp(3,3); % inertia of link 1 about CG in Kg*m^2 I21_B=I12_B;I31_B=I13_B;I32_B=I23_B; XXI

60 % Design the controller %Kp1 = 100; %Kd1 =.85; %Kp2 = 55; %Kd2 =.225; %K = [ Kp1 0 Kd1 0; 0 Kp2 0 Kd2 ]; load Gc11; load Gc22; gravitylin.m % % Gravity Torques for pan-tilt % function gradg=gravitylin(ma,ia,pa,im1,n1,mb,ib,pb,im2,n2,theta1,theta2) gradg=zeros(2,2); g=9.8; IA11=IA(1,1);IA12=IA(1,2);IA13=IA(1,3); IA22=IA(2,2);IA23=IA(2,3);IA33=IA(3,3); IB11=IB(1,1);IB12=IB(1,2);IB13=IB(1,3); IB22=IB(2,2);IB23=IB(2,3);IB33=IB(3,3); lca1=pa(1);lca2=pa(2);lca3=pa(3); lcb1=pb(1);lcb2=pb(2);lcb3=pb(3); s2=sin(theta2);c2=cos(theta2); % G(1)=0; gradg(1,1:2)=[0 0]; % G(2)=-mB*g*c2*lcB1-mB*g*s2*lcB3; gradg(2,1)=0; gradg(2,2)=-mb*g*s2*lcb1-mb*g*c2*lcb3; masscenter.m % masscenter: find the center of mass for a composite body % Ben Potsaid Jan 21, 2003 % MASSCENTER calculates the center fo mass for a composite body % [pc, mc] = masscenter(p1, m1, p2, m2) % % -- input parameters -- % p1 is the 3x1 position vector from a coordinate reference frame to the center of mass of body 1 % m1 is the mass of body 1 % p2 is the 3x1 position vector from a coordinate reference frame to the center of mass of body 2 % m1 is the mass of body 2 % % -- returns -- % pc is the position vector to the center of mass of the composite body % mc is the mass of the composite body function [pc, mc] = masscenter(p1,m1,p2,m2); XXII

61 x1 = p1(1); y1 = p1(2); z1 = p1(3); x2 = p2(1); y2 = p2(2); z2 = p2(3); mc = m1+m2; xc = (m1*x1 + m2*x2)/mc; yc = (m1*y1 + m2*y2)/mc; zc = (m1*z1 + m2*z2)/mc; pc = [xc;yc;zc]; massmatrix.m % % mass matrix for pan-tilt % function M=massmatrix(mA,IA,pA,Im1,N1,mB,IB,pB,Im2,N2,theta1,theta2) M=zeros(2,2); % run pantilt to obtain symbolic form for M first % the use ccode function on M(1,1), M(1,2), M(2,2) % and modify terms to match input IA11=IA(1,1);IA12=IA(1,2);IA13=IA(1,3); IA22=IA(2,2);IA23=IA(2,3);IA33=IA(3,3); IB11=IB(1,1);IB12=IB(1,2);IB13=IB(1,3); IB22=IB(2,2);IB23=IB(2,3);IB33=IB(3,3); lca1=pa(1);lca2=pa(2);lca3=pa(3); lcb1=pb(1);lcb2=pb(2);lcb3=pb(3); s2=sin(theta2);c2=cos(theta2); M(1,1)=IA33+mA*lcA1*lcA1+mA*lcA2*lcA2+Im1*N1*N1+IB IB11*c2*c2+mB*lcB2*lcB2+mB*lcB3*lcB3... -mb*lcb3*lcb3*c2*c2-2.0*s2*c2*ib *s2*c2*mB*lcB1*lcB3+c2*c2*IB33+c2*c2*mB*lcB1*lcB1; M(1,2)=-s2*IB12+s2*mB*lcB1*lcB2+c2*IB23-c2*mB*lcB2*lcB3; M(2,2)=IB22+mB*lcB1*lcB1+mB*lcB3*lcB3+Im2*N2*N2; M(2,1)=M(1,2); PARALLELAXIS.m % PARALLELAXIS calculates the combine inertia tensor for a 2 body composite % Ben Potsaid Jan 21, 2003 % [Ic] = parallelaxis(p1,m1,i1,p2,m2,i2,pc) % % -- input parameters -- % p1 is the 3x1 position vector from a coordinate reference frame to the center of mass of body 1 % m1 is the mass of body 1 % I1 is the 3x3 inertia tensor for body 1 about the center of mass of body 1 % p2 is the 3x1 position vector from a coordinate reference frame to the center of mass of body 2 % m1 is the mass of body 2 % I2 is the 3x3 inertia tensor for body 2 about the center of mass of body 2 % pc is the 3x1 position vector to the center of mass of the composite body % XXIII

62 % -- returns -- % Ic is the 3x3 inertia tensor for the composite body about the center of mass of the composite body function [Ic] = parallelaxis(p1,m1,i1,p2,m2,i2,pc) %shift the first body to the new coordinate system at the composite center of mass p1_c = pc-p1; a = p1_c(1); b = p1_c(2); c = p1_c(3); I1_c = I1 + m1*[ b^2+c^2 -a*b -a*c; -a*b c^2+a^2 -b*c; -a*c -b*c a^2+b^2]; %shift the second body to the new coordinate system at the composite center of mass p2_c = pc-p2; a = p2_c(1); b = p2_c(2); c = p2_c(3); I2_c = I2 + m2*[ b^2+c^2 -a*b -a*c; -a*b c^2+a^2 -b*c; -a*c -b*c a^2+b^2]; Ic = I1_c + I2_c; PLOTS.m close all plot(time,desired_states(:,1)) hold; plot(time,states(:,1),'r') xlabel('time (s)') ylabel('pan Angle (rad) ') title('system Response ') figure(2); plot(time,desired_states(:,2)) hold; plot(time,states(:,2),'r') title('system Response ') xlabel('time (s)') ylabel('tilt Angle (rad) ') theta1 = states(:,1)'; theta2 = states(:,2)'; thdot1 = states(:,3)'; thdot2 = states(:,4)'; torque1 =torque(:,1)'; torque2 =torque(:,2)'; figure(3) subplot(1,2,1) plot(abs(torque1/ngear1),abs(ngear1*thdot1),'b',[0 motor1_st],[motor1_nls 0],'r') xlabel('motor 1 torque (Nm)') ylabel('motor 1 velocity (rad/s)'); XXIV

63 text(motor1_st/6,motor1_nls/6,'feasible'); text(3*motor1_st/4,3*motor1_nls/4,'not feasible'); title('pittman GM9234S016'); subplot(1,2,2); plot(abs(torque2/ngear2),abs(ngear2*thdot2),'b',[0 motor2_st],[motor2_nls 0],'r') xlabel('motor 2 torque (Nm)') ylabel('motor 2 velocity (rad/s)'); text(motor2_st/6,motor2_nls/6,'feasible'); text(3*motor2_st/4,3*motor2_nls/4,'not feasible'); title('pittman GM9234S016'); figure(4) plot(time,abs(desired_states(:,1)-states(:,1))) title('system Response ') xlabel('time (s)') ylabel('pan Error ') figure(5) plot(time,abs(desired_states(:,2)-states(:,2))) title('system Response ') xlabel('time (s)') ylabel('tilt Error ') start = 1; for I=start:length(time) norm_traj=traj_calc(states(i,1),states(i,2)); new_time(i-start+1)=time(i); found_error(istart+1)=find_reflected_doterr(norm_traj,vsim_p(i,:),sim_vdir(i,:),sim_walldir(i,:),sim_des_dot (I,:)); [pointsx(i-start+1),pointsy(i-start+1),pointsz(istart+1)]=find_reflected_dot(vsim_p(i,:),sim_vdir(i,:),norm_traj,sim_des_dot(i,:),sim_walldir(i, :)); end %figure(6) %plot(new_time,found_error); figure(7) plot(pointsx,pointsy); title('system Response ') xlabel('pan (inches)') ylabel('tilt (inches)') figure(8) plot3(pointsx,pointsy,time); title('system Response ') xlabel('pan (inches)') ylabel('tilt (inches)') zlabel('time (s)') XXV

64 vec_plane_int.m function [x_int,y_int,z_int] = vec_plane_int(vp,vdir,pnorm,p) %VEC_PLANE_INT [x_int,y_int,z_int] = vec_plane_int(vp,vdir,pnorm) % Takes in a 3X3 vector which defines a point on the line, a 3X3 vector which % defines the direction of the line and a 3X3 vector that defines the direction % of the vector normal to the plane %Inputs vp - point vector % vdir - line direction vector % pnorm - plane normal vector direction % p - point of center of plane %Outputs x_int - x intersection point % y_int - y intersection point % z_int - z intersection point % %old %z_int= (pnorm(1)*vdir(1)*vp(3)-pnorm(1)*vp(1)*vdir(3)+pnorm(2)*vdir(2)*vp(3)- pnorm(2)*vp(2)*vdir(3))/(pnorm(1)*vdir(1)+pnorm(2)*vdir(2)+pnorm(3)*vdir(3)); %y_int= -(-pnorm(1)*vdir(1)*vp(2)+pnorm(1)*vp(1)*vdir(2)+pnorm(3)*vdir(2)*vp(3)- pnorm(3)*vp(2)*vdir(3))/(pnorm(1)*vdir(1)+pnorm(2)*vdir(2)+pnorm(3)*vdir(3)); %x_int= (-pnorm(2)*vdir(1)*vp(2)+pnorm(2)*vp(1)*vdir(2)- pnorm(3)*vdir(1)*vp(3)+pnorm(3)*vp(1)*vdir(3))/(pnorm(1)*vdir(1)+pnorm(2)*vdir(2)+pnorm (3)*vdir(3)); z_int = (pnorm(1)*vdir(1)*vp(3)- pnorm(1)*vp(1)*vdir(3)+pnorm(1)*p(1)*vdir(3)+pnorm(2)*vdir(2)*vp(3)- pnorm(2)*vp(2)*vdir(3)+pnorm(2)*p(2)*vdir(3)+pnorm(3)*p(3)*vdir(3))/(pnorm(1)*vdir(1)+pn orm(2)*vdir(2)+pnorm(3)*vdir(3)); y_int = (pnorm(1)*vdir(1)*vp(2)- pnorm(1)*vp(1)*vdir(2)+pnorm(1)*p(1)*vdir(2)+pnorm(2)*p(2)*vdir(2)+pnorm(3)*vp(2)*vdir( 3)- pnorm(3)*vdir(2)*vp(3)+pnorm(3)*p(3)*vdir(2))/(pnorm(1)*vdir(1)+pnorm(2)*vdir(2)+pnorm( 3)*vdir(3)); x_int = (pnorm(1)*p(1)*vdir(1)+pnorm(2)*vp(1)*vdir(2)- pnorm(2)*vdir(1)*vp(2)+pnorm(2)*p(2)*vdir(1)+pnorm(3)*vp(1)*vdir(3)- pnorm(3)*vdir(1)*vp(3)+pnorm(3)*p(3)*vdir(1))/(pnorm(1)*vdir(1)+pnorm(2)*vdir(2)+pnorm( 3)*vdir(3)); %end vec_plane_int dir_calc.m function Direction = dir_calc(angle) X = cos(angle(2))*sin(angle(1)); Y = sin(angle(2)); Z = cos(angle(2))*cos(angle(1)); Direction = [X,Y,Z] XXVI

65 Intersection_pt.m function [xi, yi, zi] = Intersection_pt(V,N,w,h); den = dotprod(v,n'); xi = V(1)*((N(2)*w - N(3)*h))/den; yi = V(2)*((N(2)*w - N(3)*h))/den; zi =(V(1)*N(1)*h + V(2)*N(2)*h + V(3)*N(2)*w) / den; make_lookup.m function [pandes,tiltdes,val,row,col]=make_lookup(vp,walldir, desiredtarget,start_p,end_p,start_t,end_t) optimset(optimset,'maxfunevals',30); optimset(optimset,'display','off'); options=optimset; vguess=[0,0,1]; x=0; y=0; for pan=start_p:.5:end_p x=x+1; row(x,1)=pan*pi/180; y=0; for tilt=start_t:.5:end_t y=y+1; vdir=dir_calc([pi-pan*pi/180,tilt*pi/180]); [vec,val(x,y)]=fminsearch('find_reflected_doterr',vguess,options,vp,vdir,walldir,desiredtarget); n=vec/norm(vec); vguess=n; [xi,yi,zi] = vec_plane_int(vp,vdir,n,[0,0,0]); dist=sqrt(xi^2+yi^2+zi^2); % if(dist>2.55) % pandes(x,y)=-100; % tiltdes(x,y)=-100; % else a=angle_calc(n); pandes(x,y)=a(1); tiltdes(x,y)=a(2); % end end end y=0; for tilt=start_t:.5:end_t y=y+1; col(1,y)=tilt*pi/180; end unitvector.m XXVII

66 function V = unitvector(p1,p2); V(1) = P2(1) - P1(1); V(2) = P2(2) - P1(2); V(3) = P2(3) - P1(3); mag = sqrt(dotprod(v,v')); V = V/mag; GM9234S017_pantilt_lin.m with actual friction measurement % % linearized pan-tilt system about theta=0 thetadot =0 % % setup pan-tilt parameters GM9234S017_pantiltinit; IA11=I11_A; IA12=I12_A; IA13=I13_A; IA21=I21_A; IA22=I22_A; IA23=I23_A; IA31=I31_A; IA32=I32_A; IA33=I33_A; IB11=I11_B; IB12=I12_B; IB13=I13_B; IB21=I21_B; IB22=I22_B; IB23=I23_B; IB31=I31_B; IB32=I32_B; IB33=I33_B; IA=[IA11 IA12 IA13;IA12 IA22 IA23;IA13 IA23 IA33]; IB=[IB11 IB12 IB13;IB12 IB22 IB23;IB13 IB23 IB33]; ma=m_a; mb=m_b; pa=[lc_a1,lc_a2,lc_a3]; pb=[lc_b1,lc_b2,lc_b3]; N1=Ngear1; N2=Ngear2; Im1=Imj1; Im2=Imj2; % gravity g = 9.807; % get the linearized mass matrix theta1d=0;theta2d=0; M = massmatrix(ma,ia,pa,im1,n1,mb,ib,pb,im2,n2,theta1d,theta2d); XXVIII

67 % assume no friction for now d1=fv1; % viscuous friction for pan d2=fv2; % viscuous friction for tilt D = diag([d1 d2]); % get the linearized gravity term gradg=gravitylin(ma,ia,pa,im1,n1,mb,ib,pb,im2,n2,theta1d,theta2d); % define state space matrices A = [zeros(2,2) eye(2,2); -inv(m)*gradg -inv(m)*d]; B = [zeros(2,2) ; inv(m)]; C = [eye(2,2), zeros(2,2)]; D = zeros(2,2); G = ss(a,b,c,d); disp('*** G ***'); disp(['open loop transfer function linearized about (',... num2str([theta1d theta2d]),')']); tf(g) G11=tf(G(1,1)); G22=tf(G(2,2)); % % controller parameters % load Gc11; load Gc22_; % sampling period ts =.005; % 200 Hz GM9234S017_pantiltinit.m with actual friction measurement %Calculate the mass and inertia properties for body A clear all; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% % Calculate the position to the center of mass, the mass, and the inertia tensor for each body % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% % for the modified pan assembly XXIX

68 %Body A with Motor p_sm = [ ]'; m_sm = ; I_sm = [ ; ; ]; %Calculate the mass and inertia properties for body B %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%% % Define some key dimensions and properties of the individual bodies. % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%% m_p = 0.055; % mass of Mirror r_p =.0635; %radius of Mirror th_p =.0026; % thickness of Mirror %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% % Calculate the position to the center of mass, the mass, and the inertia tensor for each body % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% % for the tilt assembly with pulley & bar p_skel = [ ]'; m_skel = ; I_skel = [ ; ; ]; p_payload = [r_p/2 0 0]'; %CG of Mirror m_payload =m_p; I_payload = [ 1/2*m_p*(r_p)^2 0 0; /12*m_p*(3*(r_p)^2+th_p^2) 0 ; /12*m_p*(3*(r_p)^2+th_p^2)]; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Start off with the skeletal body and add the hub of the pulley to form a composite body % % with new mass center at p_sh, new mass of m_sh, and new inertia tensor about p_sh of I_sh. % % One by one, add the rest of the bodies until all of the bodies in body B are included. % % Note that the hole has been included as a negative mass. % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % add to form skeletal-hub-pulley-bar-mirror [p_sp, m_sp, I_sp] = compositebodies(p_skel, m_skel, I_skel, p_payload, m_payload, I_payload); % Initialization file for Pantilt XXX

69 Ts = 0.001; sampletime=.001; %Sampling time of controller running on ARCS %Define motor and amplifier parameters torque_sat =.43;%2.1; Kmtr = 4.59e-2;%2.32e-2; % Motor torque constant Nm/A motor1_igr=11.5; % Motor1 internal gear ratio motor1_nls = 44.4*motor1_igr; % Motor1 no load speed at rotor in Rad/s motor1_st = 2.1/motor1_igr; % Motor1 stall torque at rotor in Nm motor2_igr=11.5; % Motor2 internal gear ratio motor2_nls = 44.4*motor2_igr; % Motor2 no load speed at rotor in Rad/s motor2_st = 2.1/motor2_igr; % Motor2 stall torque at rotor in Nm theta1_start = 0.0; theta2_start = 0.0; thdot1_start = 0.0; thdot2_start = 0.0; % Define gravity constant g = 9.81; % acceleration due to gravity in m /(s^2) % Define Joint 1 Parameters %%PAN belt1_gr = 2; % belt gear ratio for joint 1 Ngear1 = belt1_gr*motor1_igr; % total gear ratio of internal and belt reduction Imj1 = 4.2e-6; % inertia of motor rotor in KG*m^2 as seen by motor reference fv1 = ; % viscous friction for joint in NmS/Rad as seen by encoder fc1 = 0.08; % coulomb friction for joint in Nm as seen by encoder % Define Joint 2 Parameters %Tilt belt2_gr = 4; % belt gear ratio for joint 2 Ngear2 = belt2_gr*motor2_igr; % total gear ratio of internal and belt reduction Imj2 = 4.2e-6; % inertia of motor rotor in KG*m^2 as seen by motor reference fv2 = ; % viscous friction for joint in NmS/Rad as seen by encoder fc2 = 0.18; % coulomb friction for joint in Nm as seen by encoder % Define Link A parameters m_a = m_sm; % mass of link A in Kg lc_a1 = p_sm(1); lc_a2 = p_sm(2); lc_a3 = p_sm(3); l_a = 0.0; % location of CG on link A in m I11_A = I_sm(1,1); I12_A = I_sm(1,2); I13_A = I_sm(1,3); I22_A = I_sm(2,2); I23_A = I_sm(2,3); I33_A = I_sm(3,3); % inertia of link 1 about CG in Kg*m^2 I21_A=I12_A;I31_A=I13_A;I32_A=I23_A; XXXI

70 % Define Link B parameters m_b = m_sm; % mass of link A in Kg lc_b1 = p_sp(1); lc_b2 = p_sp(2); lc_b3 = p_sp(3); %l_a = 0.0; % location of CG on link A in m I11_B = I_sp(1,1); I12_B = I_sp(1,2); I13_B = I_sp(1,3); I22_B = I_sp(2,2); I23_B = I_sp(2,3); I33_B = I_sp(3,3); % inertia of link 1 about CG in Kg*m^2 I21_B=I12_B;I31_B=I13_B;I32_B=I23_B; % Design the controller %Kp1 = 100; %Kd1 =.85; %Kp2 = 55; %Kd2 =.225; %K = [ Kp1 0 Kd1 0; 0 Kp2 0 Kd2 ]; load Gc11; load Gc22; XXXII

71 Appendix C XXXIII

72 XXXIV

73 Appendix D > x:='x'; y:='y'; z:='z'; plane:=an*(x-x1)+bn*(y-y1)+cn*(z-z1)=0; pendir:=[ap,bp,cp]; penpoint:=[xp,yp,zp]; dot:=[xd,yd,zd]; an:=cos(theta)*sin(phi); bn:=sin(theta); cn:=cos(theta)*cos(phi); > x:=ap*(z-zp)/cp+xp; y:=bp*(z-zp)/cp+yp; z_int:=solve(plane,z); z_int := ( cos( θ ) sin( φ) ap zp cos( θ ) sin( φ) xpcp + cos( θ ) sin( φ) x1cp + sin( θ) bp zp sin( θ) yp cp + sin( θ) y1 cp + cos( θ ) cos( φ) cp z1)/( cos( θ ) sin( φ) ap + sin( θ) bp + cos( θ ) cos( φ) cp) > x:='x'; y:='y'; z:='z'; plane:=an*(x-x1)+bn*(y-y1)+cn*(z-z1)=0; > x:=ap*(y-yp)/bp+xp; z:=cp*(y-yp)/bp+zp; y_int:=solve(plane,y); y_int := ( cos( θ ) sin( φ) ap yp cos( θ ) sin( φ) xpbp + cos( θ ) sin( φ) x1bp + sin( θ ) bp y1 + cos( θ ) cos( φ) yp cp cos( θ ) cos( φ) bp zp + cos( θ ) cos( φ) z1 bp)/( cos( θ ) sin( φ) ap + sin( θ) bp + cos( θ ) cos( φ) cp) > x:='x'; y:='y'; z:='z'; plane:=an*(x-x1)+bn*(y-y1)+cn*(z-z1)=0; > y:=bp*(x-xp)/ap+yp; z:=cp*(x-xp)/ap+zp; x_int:=solve(plane,x); x_int := ( cos( θ ) sin( φ) ap x1 + sin( θ) xpbp sin( θ) ap yp + sin( θ ) y1 ap + cos( θ ) cos( φ) xp cp cos( θ ) cos( φ) ap zp + cos( θ ) cos( φ) z1 ap)/( cos( θ ) sin( φ) ap + sin( θ ) bp + cos( θ ) cos( φ) cp) > x:='x'; y:='y'; z:='z'; plane:=an*(x-x1)+bn*(y-y1)+cn*(z-z1)=0; > y:=(-bn)*(x-xp)/(-an)+yp; z:=(-cn)*(x-xp)/(-an)+zp; XXXV

74 x_int2:=solve(plane,x); x_int2 := ( cos( θ) 2 sin( φ) 2 x1 + sin( θ) 2 xp sin( θ ) yp cos( θ ) sin( φ) + sin( θ ) y1 cos( θ ) sin( φ ) + cos( θ) 2 cos( φ) 2 xp cos( θ) 2 cos( φ) zp sin( φ) + cos( θ ) 2 cos( φ) z1 sin( φ) ) ( cos( θ ) 2 sin( φ) 2 + sin( θ) 2 + cos( θ) 2 cos( φ) 2 ) > x:='x'; y:='y'; z:='z'; plane:=an*(x-x1)+bn*(y-y1)+cn*(z-z1)=0; > x:=(-an)*(y-yp)/(-bn)+xp; z:=(-cn)*(y-yp)/(-bn)+zp; y_int2:=solve(plane,y); y_int2 := ( cos( θ) 2 sin( φ) 2 yp cos( θ ) sin( φ ) sin( θ) xp + cos( θ ) sin( φ) x1 sin( θ ) + sin( θ ) 2 y1 + cos( θ) 2 cos( φ) 2 yp cos( θ ) cos( φ) zp sin( θ) + cos( θ ) cos( φ) z1 sin( θ) ) ( cos( θ) 2 sin( φ) 2 + sin( θ ) 2 + cos( θ) 2 cos( φ) 2 ) > x:='x'; y:='y'; z:='z'; plane:=an*(x-x1)+bn*(y-y1)+cn*(z-z1)=0; > x:=(-an)*(z-zp)/(-cn)+xp; y:=(-bn)*(z-zp)/(-cn)+yp; z_int2:=solve(plane,z); z_int2 := ( cos( θ) 2 sin( φ) 2 zp cos( θ) 2 sin( φ ) cos( φ) xp + cos( θ ) 2 sin( φ) x1 cos( φ) + zp sin( θ) 2 sin( θ ) cos( θ ) cos( φ) yp + sin( θ) y1 cos( θ ) cos( φ) + cos( θ ) 2 cos( φ) 2 z1) ( cos( θ) 2 sin( φ) 2 + sin( θ ) 2 + cos( θ) 2 cos( φ) 2 ) > vec:=[x_int,y_int,z_int]-penpoint; para:=[x_int,y_int,z_int]-[x_int2,y_int2,z_int2]; perp:=vec-para; > reflected:=para+perp*(-1); >_reflected:=[reflected[1]/(sqrt(reflected[1]^2+reflected[2] ^2+reflected[3]^2)),reflected[2]/(sqrt(reflected[1]^2+refle cted[2]^2+reflected[3]^2)),reflected[3]/(sqrt(reflected[1]^ 2+reflected[2]^2+reflected[3]^2))]; > check_reflect:=dot-[x_int,y_int,z_int]; >u_check_reflect:=[check_reflect[1]/(sqrt(check_reflect[1]^ 2+check_reflect[2]^2+check_reflect[3]^2)),check_reflect[2]/ (sqrt(check_reflect[1]^2+check_reflect[2]^2+check_reflect[3 ]^2)),check_reflect[3]/(sqrt(check_reflect[1]^2+check_refle ct[2]^2+check_reflect[3]^2))]; > eq1:=u_reflected[1]-u_check_reflect[1]; > eq2:=u_reflected[2]-u_check_reflect[2]; XXXVI

75 > eq3:=u_reflected[3]-u_check_reflect[3]; > plane:=an*(x-x1)+bn*(y-y1)+cn*(z-z1)=0; pendir:=[ap,bp,cp]; penpoint:=[xp,yp,zp]; dot:=[xd,yd,zd]; > xp:=0;yp:=-6;zp:=12;ap:=.01;bp:=1;cp:=- 1.5;xd:=0;yd:=6;zd:=6;x1:=0;y1:=0;z1:=0; solve({eq1,eq2,eq3,eq4}); { an = , cn = , bn = }, { cn = , bn = , an = } > xp:='xp';yp:='yp';zp:='zp';ap:='ap';bp:='bp';cp:='cp';xd:=' xd';yd:='yd';zd:='zd';x1:=0;y1:=0;z1:=0; > solve({eq1,eq2,eq3},{an,bn,cn}); Warning, computation interrupted > xp:='xp';yp:='yp';zp:='zp';ap:='ap';bp:='bp';cp:='cp';xd:=' xd';yd:='yd';zd:='zd';x1:='x1';y1:='y1';z1:='z1'; > peq1:=xd=x_int+reflected[1]*t; peq2:=yd=y_int+reflected[2]*t; peq3:=zd=z_int+reflected[3]*t; > peq12:=simplify(peq1);peq32:=simplify(peq3);peq22:=simplify (peq2); > peq4:=sqrt(an^2+bn^2+cn^2)=1; solve({peq12,peq22,peq32},{theta,phi,t}); Warning, computation interrupted > XXXVII

76 Team Resumes: Appendix E XXXVIII

77 XXXIX

78 XL