Load Sway Compensation for Cranes by malj

VIEWS: 8 PAGES: 39

									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)
       1kg9.81                1kg0.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  1kg9.81m s 2 cos L    L 1kg0.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   1kg9.81 m s 2 cos L    L 1kg0.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 



                             1kg9.81m s 2 rw2             2       1kg0.040msin  L rw2  
x  cos L sin  L                                          L 
                        0.338kgr 2  J  sin  2 1r 2 
                                                                                                              
                                                                       0.338kgr 2  J  sin  2 1r 2 
                                  w                 L      w                   w               L        w 

                    Krw                                                    K2                             
Vs                                                        x                                                
     
    R 0.3381kgr  J  sin  2 1kgr 2
                  2
                  w             L          w                    
                                                           R 0.3381kgr 2  J  sin  2 1kgr 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

								
To top