VIEWS: 8 PAGES: 39 POSTED ON: 11/10/2010 Public Domain
Load Sway Compensation for Cranes Project Report Group 9 – The Swaying Shafts Jim Bevan Craig Monahan Dan Hoovler Eric Vander Zee Patrick Glaser EGR 345 November 13, 2003 1 Executive Summary This document discusses the concepts and design of the sway-controlling cart. A C program was written to control the position and compensate for the sway using the Motorola 68HC11 expanded Axiom board. The cart was made of mostly lightweight aluminum, which provided a stiff base while maintaining a minimum weight. A planetary motor with gears was used to power the cart. This allowed different configurations by switching gear ratios. Gears were used to transmit power to the wheel while also turning the encoder to provide position feedback. A potentiometer was attached to arms to determine the angle, which was read by A/D converter. The cart performed well, with scores above the average of other groups. Cost was around $140, less than the targeted value of $150. The weight is higher than the targeted value of 200g, at 320g. 2 Table of Contents Content Page Executive Summary 2 Design Description 4 Drawing summary 5 System block diagrams 6 Description of control scheme 7 Schematics 10 Calculations 11 Budgets and bill of materials 13 Weight inventory 14 Test Results 15 Simulation results 15 Comparison of sway 18 Formal Test Results 18 Overall score estimates 18 Appendix 21 Drawings 21 Stress calcuations 27 C program 28 Simulation program 33 Recepits and cost evidence 3 Design Description Mechanical Design Aspects The choice of our design aspects was based on many factors: functionality, weight/strength of the material, and ease of manufacture. In order for the cart to function correctly, it was imperative to use a drive system that was ridged and non- slipping. For this, gears wear chosen to transmit the power from the drive wheel. In this way we were able to include the motor, encoder, and drive wheel in one solid system. When the gears are properly aligned, it will be very difficult for the system to slip and lose position. The use of a wide rubber R/C car tire gave the cart excellent traction due to the large surface area in contact with the ground. The rear tires are designed to hold the rear of the cart parallel to the driving surface, and help keep the cart upright. Both front and rear tires are very light when compared to a solid plastic wheel of the same size. The side stability was also designed to be very light. There are no considerable forces seen on the side supports, so the supports do not have to be terribly strong. Therefore, small diameter rods were used to drop down off the sides of the cart. On the end of the rods are small hobby wheels, which guide the cart and prevent tipping. Aluminum 6061 was chosen for the cart material due to its strength, lightweight, and machinable properties. The use of this material enables us to design a platform for the cart that is stronger and lighter than commonly used plastic. The mounting brackets for the drive shaft, potentiometer, and the encoder were also made from aluminum because they could be easily machined and/or bent. The flat platform enabled us to drill holes for the brackets in the precise positions as the cart is assembled. Therefore, any dimensional design changes can be done on the fly. Electrical Design Aspects The electrical portion of the design consisted of the driver chip circuitry, wiring harness, and axiom board. A schematic of the driver chip circuitry is shown if figure 7. The L293 chip and wiring connections were permanently soldered to a circuit board to alleviate loose wires. A 40 pin wire harness was used to connect the 68HC11 to the driver chip. Also category 5 wire was used to connect the motor, encoder, and potentiometer from the cart to the circuit board. These types of connectors make it very easy to connect and disconnect the system. It also gives us a solid construction to help eliminate wiring problems 4 Modifications Our basic design had only minor dimensional changes. These dimensional changes include the position of the mounting brackets for the encoder on the potentiometer. As stated before, the brackets were fastened into position as the cart was assembled. This change is however, very minor. Another minor change to the mechanical design is the addition of two more guide wheels. The original design incorporated two guide wheels located at the back of the cart. Adding two in the front gave us the added stability we needed to keep the cart on track. The mounting pin for our drop arm was repositioned so that it connected to the encoder bracket instead of the side of the cart. This change developed due to the fact that the encoder position needed to be lower so that it would mesh with the motor and drive wheel gears. Due to this, there wasn’t enough room for the pin to fit through the side of the cart. Drawing Summary Figure 1 shows the fully assembled cart with all parts labeled. Figure 2 shows the cart in an exploded view. 5 Figure 1. Assembled cart. 6 Figure 2. Exploded view of cart. System Block Diagrams Figure 3 shows the hardware block diagram. It identifies which parts are connected. 7 Figure 3. Complete hardware system block diagram. The program was written to with the architecture shown in Figure 4. The user first has to enter the desired input, and then the motion profile will automatically be defined based on the desired input. The distance traveled is initially set to zero so by default when the user hits ‘S’ the cart will start moving according to the velocity profile. The distance is continually checked and the position of the cart is controlled solely by the motion profile until the distance traveled is greater then or equal to the desired distance. When the distance traveled is greater then or equal to the desired distance the angular position of the arm is compared to a minimum allowable angle. If the angle is less then the minimum allowable angle then the cart stays at its current position else the errors are calculated for both the linear and angular position and the desired position is set based on the errors. The desired velocity is passed through a deadband compensator and the angle is continually checked until the angle is less then the minimum allowable angle and the distance traveled is equal to the desired distance. Figure 4. Software block diagram. Description of Control Scheme Figure 5 shows the feedback control diagram. As the system goes through the diagram, the encoder feeds back position data while the potentiometer feeds back the arm angle through the A/D converter. 8 Figure 5. Feedback control block diagram. An example of the initial position profile for duration of 2 seconds and a length of 20 inches is found in Table 1. The position profile plot is shown in Figure 6. Table 1. Position Profile Example Duration = 2 seconds Length = 20 inches index point_master point_time point_position 0 0 0.0 0.00 1 24 0.2 0.48 2 95 0.4 1.90 3 206 0.6 4.12 4 345 0.8 6.90 5 500 1.0 10.00 6 655 1.2 13.10 7 794 1.4 15.88 8 905 1.6 18.10 9 976 1.8 19.52 10 1000 2.0 20.00 9 20 18 16 14 Position (inches) 12 10 8 6 4 2 0 0.00 0.25 0.50 0.75 1.00 1.25 1.50 1.75 2.00 Time (seconds) Figure 6. Position Profile 10 Schematics Figure 7 shows the wiring schematic used for the cart. The design is the same used in prior labs with the L293 chip but simply adding the A/D converter and encoder. Figure 7. Wiring diagram using 11 Calculations Figure 8. Free body diagrams of cart and mass. Figure 8 shows the free body diagrams for the cart and mass. The equations below describe the system mathematically. First, the forces are summed for the cart and the mass in equations (1), (2), and (3). Next, the differential equation for the motor is applied to the system, substituting in equations (2) and (3) to ultimately solve for x’’ in equation (5). F M P g sin L M Pl L x sin L M P 0 l L g sin L x cos L (1) 0.040 m L 9.81 sin x cos m s2 L L F FP M P g cos L L M Pl x sin L M P 0 2 FP M P g cos L L M Pl x sin L M P 2 (2) 1kg9.81 1kg0.040 m x sin 1kg m 2 s2 L L FX M C x FP sin L Fw 0 (3) 12 M C x M P g cos L L M Pl x sin L M P sin L Fw 0 2 0.3381kg x 1kg9.81m s 2 cos L L 1kg0.040m x sin L 1kg sin L Fw 0 2 K 2 K Fr w w VS w w JR JR J K J K2 Fw VS r R w w (4) rw R r w w given x wrw K J K 2 Fw VS r R x r 2 x r 2 R w w w K J K 2 M C x M P g cos L L M Pl x sin L M P sin L Vs r R x r 2 x r 2 R 0 2 w w w 0.3381kg x 1kg9.81 m s 2 cos L L 1kg0.040m x sin L 1kg sin L 2 K J K 2 r R x r 2 x r 2 R 0 Vs w w w K2 x M c 2 sin L M P cos L sin L M P g L M pl sin L Vs J 2 K x 2 0 2 r R rw rw R w M r 2 J sin L M P rw K2 cos L sin L M P g L M pl sin L Vs 2 2 2 K x c w 2 x 2 r R rw rw R w M p grw2 2 M pl sin L rw 2 x cos L sin L M r 2 J sin 2 M r 2 L M r 2 J sin 2 M r 2 cw L P w cw L P w Krw K2 Vs x R M r 2 J sin 2 M r 2 R M r 2 J sin 2 M r 2 c w L P w c w L P w 1kg9.81m s 2 rw2 2 1kg0.040msin L rw2 x cos L sin L L 0.338kgr 2 J sin 2 1r 2 0.338kgr 2 J sin 2 1r 2 w L w w L w Krw K2 Vs x R 0.3381kgr J sin 2 1kgr 2 2 w L w R 0.3381kgr 2 J sin 2 1kgr 2 w L w (5) 13 Budgets and Bill of Materials Table 2 shows the final bill of materials for the project. The estimated total price is $136.50, $13.50 below the targeted value of $150. The majority of the cost came from the Axiom 68HC11 board. The remainder of the cost was from the materials and smaller assembly pieces. Table 2. Bill of materials for cart. Bill of Materials Hardware Quantity ($/unit) Cost Total Hobby Gears (large) 2 $2.00 $4.00 Hobby Gears (small) 1 $1.00 $1.00 Potentiometer 1 $2.50 $2.50 Axiom 68HC11 (CME-11E9-EBVU-ED) 1 $89.00 $89.00 Motor/planetary gear set 1 $18.00 $18.00 Aluminum Stock 4 $1.50 $6.00 Drive wheel 1 $1.00 $1.00 Rear wheels (cost of material) 2 $0.50 $1.00 Side wheels 2 $0.25 $0.50 Encoder 1 $8.00 $8.00 Misc. screws, bolts, washers, nuts, etc 1 $3.50 $3.50 Wire Lot 1 $2.00 $2.00 Power supply 1 $0 $0 Total Cost: $136.50 14 Weight Inventory Each individual part was weighed prior to assembly. The total in table 3 gives the estimated total weight. Table 3. Table with estimated individual and total masses. Mass Table Items Quantity Weight (g) Total Comp. Weight Cart 1 70.4 70.4 Hanging Arms 2 30 60 Pot. with bracket 1 15.1 15.1 Encoder w/ bracket 1 18.8 18.8 Big Gears 2 6.2 12.4 Small Gear 1 2.4 2.4 Drive shaft brackets 1 10.5 10.5 Motor 1 73.4 73.4 Guide wheel/shaft 4 5 20 Drive Wheel 1 21 21 Rear Wheels 2 8 16 Total Weight 320.0 15 Test Results Simulations Results Motor Parameters The motor parameters for our planetary gear head motor were obtained by using a tachometer to track the position of the motor. The voltages generated by the tachometer were collected at different step inputs to the motor. These were recorded into an array for further calculations. The RPMs and the input voltage were also recorded to obtain a relationship between the steady state speed of the motor and the steady state voltage. Table 4 below shows the motor parameters that were calculated for the motor with the planetary gear system and the drive gears in place. The resistance of the motor (R) was measured using a DMM. The motor speed constant (K) was calculated using the relationship shown in equation 6. This relationship was used on a graph of steady state velocity vs. voltage. The moment of inertia (J) was calculated using equation 7. 1 SS V SS (6) K K2 J (7) 1 R Table 4: Motor Parameters Parameter Value Units Resistance (R) 1.3 ohms Motor Constant (K) 0.017366 Vs/rad Moment of Inertia (J) 0.00129 kg m^2 Figure 9 is a graph of the velocity vs time for the motor. This graph was used to find the time constant. The time constant was needed for equation 6 above to calculate the moment of inertia. Figure 10 is the steady state velocity vs the steady state voltage. The slope of this line was used to find the motor speed constant, using the relationship in equation 7 above. 16 35 30 25 Velocity (rad/s) 20 15 10 5 0 0 0.2 0.4 0.6 0.8 1 1.2 Time (s) Figure 9. Velocity vs. Time 29 y = 57.584x 28 27 Velocity (rad/s) 26 25 24 23 0.41 0.42 0.43 0.44 0.45 0.46 0.47 0.48 0.49 0.5 0.51 Voltage (v) Figure 10. Steady state velocity vs voltage The simulation data for our system is given below in table 5. This data is speculative because we know our cart will go 20 inches in less than 6 seconds. As our testing indicated, our final settling time is around 7 seconds. This comes very close to our theoretical settling time. 17 Table 5. Position vs Time data for the simulation Time (s) Position (in) 0.0013 0 0.3263 0.159932 0.6513 0.587881 0.9763 1.276903 1.3013 2.220388 1.6263 3.329511 1.9513 4.548709 2.2763 5.935061 2.6013 7.488231 2.9263 9.101706 3.2513 10.752987 3.5763 12.144033 3.9013 13.117985 4.2263 13.74852 4.5513 14.457203 4.8763 15.463309 5.2013 16.788571 5.5263 18.132489 5.8513 19.385337 6.0763 20.462137 Theoretical Settling Time 6 seconds Figure 11 below is the graph of the above data. The motion profile generated is very rough. This may be due to problems in the software. It may also be do to the sway compensation during the travel, which would cause the motor speed up or slow down. Our actual control system program generates a much smoother profile. These results are fairly inconclusive and unhelpful to us at this point. 18 Figure 11: Position vs time for the simulation Comparison with and without Sway Compensation When the cart is set to move 20 inches in two seconds, the mass settles and stops swaying in slightly less than 6 seconds. This test was done without sway compensation. A C program was written to compensate for the swaying, allowing the settling time to be less. Based on the time to settle without the sway compensation, it was estimated that the cart would move to its position and settle in less than 5 seconds when the desired position is 20 inches. Actual test results yielded total settling times of 4.5 to 4.8 seconds. Formal Test Results For the final test, the cart moved to 20 inches and settled in 2.15 seconds. This was the fastest time of all competing groups. 19 Overall Score The initial theoretical score is shown in table 6, which is 3680, based on the parameters listed below. 2 t score s 4 200 10 10 2 0.2 C B T m d ts the time to settle (s) =7.8 seconds C total cost of part ($) = $140 d distance moved in test (m) = guess, 20 in = 0.508 m B build quality score assigned by judges (0 to 1) = 0.1 T theory quality score assigned by judges (0 to 1) = 0.1 M mass of the apparatus (Kg) = 380g = 0.380 kg Table 6. Comparison of scores of all groups. Scoring Equation Item Variables Team 1 Team 2 Team 3 Team 4 Team 5 Team 6 Member (who I talked to) Carrie Randy Matt ---- Justin Bill The time to settle (s) Test 2 ts ---- 11 5.5 ---- ---- ---- Total cost of part ($) C 140 141 135.21 ---- 119 135 Distance moved in test (m) d 0.508 0.508 0.508 ---- 0.508 0.508 Build Quality score (0 to 1) B 0.1 0.1 0.1 ---- 0.1 0.1 Theory quality score (0 to 1) T 0.1 0.1 0.1 ---- 0.1 0.1 Mass of apparatus (kg) M 0.5712 0.365 0.429 ---- 0.290 0.255 Score: Infinity 6996.6 2096.2 ---- Infinity Infinity Scoring Equation Item Variables Team 7 Team 8 Team 9 Team 10 Team 11 Team 12 Member (who I talked to) Vern Sarah Jim ---- Brian Nate The time to settle (s) Test 2 ts 9.5 5.5 7.8 ---- ---- 30.0 Total cost of part ($) C 130 158 140 ---- 146 125 Distance moved in test (m) d 0.508 0.508 0.508 ---- 0.508 0.508 Build Quality score (0 to 1) B 0.1 0.1 0.1 ---- 0.1 0.1 Theory quality score (0 to 1) T 0.1 0.1 0.1 ---- 0.1 0.1 Mass of apparatus (kg) M 0.220 0.48 0.38 ---- 0.350 0.325 Score: 2925.4 2931.6 3680.11 ---- Infinity 40548.6 For the final test the cart edges and arms were trimmed to reduce weight to 320 grams. Steel nuts were replaced with nylon as well. The theory and build quality score was 0.5. Cost remained the same at $136.50. This reduced the score to 478, the highest score in the competition. Table 7 shows these final scores. 20 Table 7. Final comparison of competition scores. Times: Scores: team 1: 4.26s 1533 team 2: 3.69s 2245 team 3: 3.40s 748 team 4: 2.90s 900 team 5: 2.79s 969 team 6: 3.70s 939 team 7: 2.98s 575 team 8: 3.58s 1186 team 9: 2.15s 478 team 10: 2.71s 781 team 11: 9.85s 18562 team 12: 9.37s 8004 Conclusion Overall the results of the tests have been successful, proving an excellent design and planning. The one major drawback at this time is the weight, which is well above the targeted value of 200g. The cart itself seems to be larger than it needs to be, which is a large contributor to the weight. Even compared to the 2x4 beam, the cart seems very wide. This would be the major suggestion for improvement—reducing the size and weight of the cart. The main strength is the speed and power of the motor. The speed can be easily adjusted based on input voltage, which allows us to control the sway of the mass proportional to the velocity. 21 Appendix – Drawings 22 23 24 25 26 27 28 29 30 31 Appendix – Stress and other Similar Calculations Stress analysis was performed on components that are load bearing or had sufficient amounts of material removed. Three locations of the cart were analyzed: the potentiometer support on the cart, the connection of the mass to the arm, and the arms themselves (due to material removal). It was found that the stress induced in all of these locations was very low compared to the maximum material stress of the working material (Aluminum 6061, 265 MPa). Table 1 shows the stress in each component and the factor of safety associated with each. Component Stress (kPa) Safety Factor Potentiometer support 189 1402 Mass support 324 819 Arms (with removed 5.15 51460 mat.) The factor of safety is very high because the stresses in the components are so low. Due to the very high factor of safety, failure in the components should not be an issue. In fact, further weight reduction is possible. 32 Appendix – Controller C Program #include <hc11e9.h>/* 68HC11E9 I/O register defines */ #include <buffalo.h>/* Used for serial communication functions */ #include <pwm.h>/* PWM library functions */ #include <stdio.h> #define T 4 /* define as 4 ms, must divide by 1000 later*/ #define Kp 25 #define Ke 20 #define TABLE_SIZE 11 int new_pos; int current_pot; int init_pot; int integrate(int); void EncoderUpdate(void); int controller(int, int); int deadband(int c_wanted); void v_output(int v_adjusted); int init_table(); void update_table(); void get_setpoint(); void init(); int count =0; int c_wanted = 0; int c_stick_pos = 168; //Compensation for the deadband int c_stick_neg = 164; //Compensation for the deadband int c_max = 255; int c_min = 255; int e_sum; int Position; int point_master[TABLE_SIZE] = {0, 24, 95, 206, 345, 500, 655, 794, 905, 976, 1000}; int point_position[TABLE_SIZE]; int point_time[TABLE_SIZE]; int point_start_time; int point_index; int ticks; /* variables to keep a system clock count */ int point_current; /* a global variable to track position */ int duration; extern void __attribute__((interrupt)) RTI_ISR(void){ TFLG2 = RTIF; /*clear the interrupt flag*/ __asm("cli"); EncoderUpdate(); get_setpoint(); v_output(deadband(controller(point_current, Position))); count++; if(count>127) { count =0; } } int main(void) { *(unsigned *)0x00EC = (unsigned)RTI_ISR; init(); new_pos = 0; unsigned mode; char key; putstr(" Press 'q' to quit\n"); putstr(" Press '1-0' to set distance 0-20 in.\n"); putstr(" Press 'S' to move the motor\n"); putstr(" Press 'p' to print out position\n"); 33 duration = 0; mode = 0;/*Use polling loop update*/ do { key = input(); switch (key){ case 'r': case 'R': new_pos = -new_pos; duration = 1; break; case '1': new_pos = 13; duration = 1; break; case '2': new_pos = 27; duration = 1.2; break; case '3': new_pos = 39; duration = 1.4; break; case '4': new_pos = 53; duration = 1.7; break; case '5': new_pos = 66; duration = 2; break; case '6': new_pos = 79; duration = 2.3; break; case '7': new_pos = 92; duration = 2.6; break; case '8': new_pos = 105; duration = 2.8; break; case '9': new_pos = 118; duration = 3.0; break; case '0': new_pos = 131; duration = 3.3; break; case 's': putstr("time ="); outint16(duration); putstr("\n"); putstr("distance ="); outint16(new_pos); putstr("\n"); putstr("\n"); update_table(point_current, point_current +new_pos, duration); break; case 'p': case 'P': outint16(current_pot); putstr(", "); outint16(init_pot); putstr("\n"); break; case 'q': 34 case 'Q': mode = 1;/*quit program*/ break; } }while (mode == 0);/*Continue until quit*/ PWMDisable();/*Disable PWM generator*/ return 0; } int deadband(int c_wanted) { int c_adjusted; if(c_wanted == 0){ //turn off the output c_adjusted = 0; } else if(c_wanted > 0){ //a positive output c_adjusted = c_stick_pos + (unsigned)c_wanted * (c_max - c_stick_pos)/c_max; } else { //the output must be negative c_adjusted = -c_stick_neg - (unsigned)(-c_wanted) * (c_min - c_stick_neg)/c_min; } return c_adjusted; } void v_output(v_adjusted) { if(v_adjusted >= 0){ //set the direction bits to CW on, CCW off PORTD |= PD2; //CW on PORTD &= ~PD3; //CCW off if(v_adjusted >255){ RefSignal = 255; } else { RefSignal = v_adjusted; } } else { //need to reverse the output sign //set the direction bits to CW off, CCW on PORTD |= PD3; //CCW on PORTD &= ~PD2; //CW off if(v_adjusted < -255){ RefSignal = 255; } else { RefSignal = -v_adjusted; //flip sign } } } void init(){ /* initialize the register once */ /*Code for initializing PWM*/ PWMInit(); /*Initializing PWM*/ PWMEnable(); /*Enable PWM generator*/ DDRD |= (DDD2 | DDD3); /* set up the timed interrupt */ *(unsigned *)0x00EC = (unsigned)RTI_ISR; /* interrupt subroutine */ TMSK2 |= RTII; /*enable the interrupt */ __asm("CLI"); /*clear interrupts*/ /* setup the analog input */ 35 OPTION |= ADPU; /*turn on the analog input*/ ADCTL = SCAN | CA; /*scan the inputs continuously */ DDRD &= ~(PD4 | PD5); /* set pins 4 and 5 as inputs */ init_pot = ADR2; init_table(); } void EncoderUpdate(void){ /* interrupt driven encoder update */ /* Static variables for position calculation */ static unsigned char state = 0xFF; unsigned char new_state; new_state = (PORTD & (PD5 | PD4)) >> 4; /* Read encoder state */ /* Update position value */ if (state!=new_state) { switch (state) { case 0x00: if (new_state==0x01) Position++; else Position--; break; case 0x01: if (new_state==0x03) Position++; else Position--; break; case 0x03: if (new_state==0x02) Position++; else Position--; break; case 0x02: if (new_state==0x00) Position++; else Position--; break; } state = new_state; } } int integrate(int e){ e_sum += e * T; return e_sum; } int controller(int Cd, int Cf){ int Cw; int c_pot; int Cep; Cep=Cd-Cf; current_pot = ADR2 - init_pot; c_pot = current_pot*-Kp; if((current_pot < 4) && (current_pot > -4)){ Cw = Cep*Ke;} else{ Cw = Cep*Ke+c_pot;} return Cw; } 36 int init_table(void){ /* initialize the setpoint table */ ticks = 0; /* set the clock to zero */ point_current = 0; /* start the system at zero */ point_index = TABLE_SIZE; /* mark the table as empty */ return 0; } void update_table(int start, int end, int duration_sec){ unsigned i; point_time[0] = ticks + 10; /* delay the start slightly */ point_position[0] = start; for(i = 1; i < TABLE_SIZE; i++){ point_time[i] = point_time[0] + (unsigned long)i * duration_sec * 250 / (TABLE_SIZE - 1); point_position[i] = start + (long int)(end - start) * point_master[i] / 1000; } point_index = 0; } void get_setpoint(void){/* interrupt driven encoder update */ ticks++; /* update the clock */ if(point_index < TABLE_SIZE){ if(point_time[point_index] == ticks){ point_current = point_position[point_index++]; //outint16(point_current); } } } 37 Appendix – Simulation Program // // Simulation program // 11/20/03 // // System component values l = 0.4; // 40cm Mp = 1.0; // 1kg Mc = 0.39; // 390g g = 9.81; // good old gravity rw = 0.0254; // 1 in radius tire K = 0.017366; // motor speed constant R = 1.3; // motor resistance J = 0.00129; // rotor inertia // System state x0 = 0; // initial conditions for position v0 = 0; theta0 = 0.0; // the initial position for the load omega0 = 0.0; X=[x0, v0, theta0, omega0]; // The controller definition PI = 3.14159; ppr = 16; // encoder pulses per revolution; Kpot = 1.72; // the angle voltage ratio Vzero = 2.5; // the voltage when the pendulum is vertical Vadmax = 5; // the A/D voltage range Vadmin = 0; Cadmax = 255; // the A/D converter output limits Cadmin = 0; tolerance = 0.5; // the tolerance for the system to settle Kpp = 100; // position feedback gain Ksp = 10; // sway feedback gain Vpwmmax = 12; // PWM output limitations in V Cpwmmax = 255; // PWM input range Cdeadpos = 165; // deadband limits Cdeadneg = 165; function foo=control(Cdesired) Cp = ppr * X($, 1)/(2*PI*rw); Cpe = Cdesired - Cp; Cpc = Kpp * Cpe; VL = Kpot * X($,3) + 2.5; // assume the zero angle is 2.5V CL = ((VL - Vadmin) / (Vadmax - Vadmin)) * (Cadmax - Cadmin); if CL > Cadmax then CL = Cadmax; end // check for voltages over limits if CL < Cadmin then CL = Cadmin; end CLc = Ksp * (CL - (Cadmax + Cadmin) / 2); Cc = Cpc + CLc; Cpwm = 0; if Cc > 0.5 then // deadband compensation Cpwm = Cdeadpos + (Cc/Cpwmmax)*(Cpwmmax - Cdeadpos); end if Cc <= -0.5 then Cpwm = -Cdeadneg + (Cc/Cpwmmax)*(Cpwmmax - Cdeadneg); end foo = Vpwmmax * (Cpwm / Cpwmmax) ; // PWM output if foo > Vpwmmax then foo = Vpwmmax; end // clip voltage if too large if foo < -Vpwmmax then foo = -Vpwmmax; end endfunction // The motion profile generator function foo=motion(t_start, t_end, t_now, C_start, C_end) if t_now < t_start then foo = C_start; 38 elseif t_now > t_end then foo = C_end; else foo = C_start + (C_end - C_start) * (t_now - t_start ) / (t_end - t_start); end endfunction // define the state matrix function term1 = Mc*rw^2 + J; // Precalculate these terms to save time term2 = R*term1; Xd = 20; // the setpoint 10 turns == 160 pulses Cd = ppr * Xd / (2 * PI * rw) ; function foo=derivative(state,t, h) Vs=control(motion(0, 6, t($, 1), 0, Cd)); // Vs=control(Cd); term3 = cos(state($,3)); // precalculate some repeated terms to save time term4 = sin(state($,3)); term5 = term1 + Mp * (term4 * rw)^2; //printf("%f %f \n", Cd, Vs); v_dot = -state($,2)*(K^2) / (term5 * R) + term3*term4*Mp*g*rw^2 / term5+ state($,4)^2 * Mp*l*term4*rw^2 / term5 + Vs*K*rw / term5; foo = [ state($,2),v_dot, state($, 4), -g * term4 / l - state($, 2) * term3 / l ]; endfunction // Integration Set the time length and step size for the integration steps = 5000; // The number of steps to use t_start = 0; // the start time - normally use zero t_end = 6.50; // the end time h = (t_end - t_start) / steps; // the step size t = [t_start]; // an array to store time values for i=1:steps, t = [t ; t($,:) + h]; X = [X ; X($,:) + h * derivative(X($,:), t($,:), h)]; // first order end // Graph the values for part e) plot2d(t, [X(:,1) + l*sin(X(:,3))], [-2], leg="mass position"); //plot2d(t, [X(:,1), 10*X(:, 3)], [-2, -5], leg="position@theta (X 10)"); xtitle('Time (s)'); // printf the values over time intervals = 20; for time_count=1:intervals, i = int((time_count - 1)/intervals * steps + 1); xmass = X(i,1) + l*sin(X(i,3)); printf("Mass location at t=%f x=%f \n", i * h, xmass); // printf("Point at t=%f x=%f, v=%f, theta=%f, omega=%f \n", i * h, X(i, 1), X(i, 2), X(i, 3), X(i, 4)); end // find the settling time in the results array ts = 0; for i = 1:steps, xmass = X(i,1) + l*sin(X(i,3)); if xmass > (Cd + tolerance) then ts = i*h + t_start; end if xmass < (Cd - tolerance) then ts = i*h + t_start; end end printf("\nTheoretical settling time %f \n", ts); 39 * steps + 1); xmass = X(i,1) + l*sin(X(i,3)); printf("Mass location at t=%f x=%f \n", i * h, xmass); // printf("Point at t=%f x=%f, v=%f, theta=%f, omega=%f \n", i * h, X(i, 1), X(i, 2), X(i, 3), X(i, 4)); end // find the settling time in the results array ts = 0; for i = 1:steps, xmass = X(i,1) + l*sin(X(i,3)); if xmass > (Cd + tolerance) then ts = i*h + t_start; end if xmass < (Cd - tolerance) then ts = i*h + t_start; end end printf("\nTheoretic al settling time %f \n", ts); 39