Sei sulla pagina 1di 68

Segway Project: Final Report

Grand Valley State University School of Engineering

EGR 345: Dynamic System Modeling & Control Team 2: Jayme Cook Charlie DeCoster John Welles Ian Rudnik Jen Brauss Jacob Vanthof

Table of Contents
Executive Summary Design Description: Mechanical Description Electrical Design Stress Stain Calculations Equations of Motion & FBDs Software Design System Block Diagram System Control Diagram Cost & Weight Inventory Test Results: Simulation Pre-Test & Final Test Information Score Calculation Conclusions & Recommendations Appendices: Appendix A: Drawings Appendix B: Electrical Schematic Appendix C: Stress Strain Calculations Appendix D: C Program Appendix E: Scilab Program Appendix F: Gantt Chart 3 5 11 12 13 18 18 19 20 21 22 23 24 25 44 46 51 61 65

Executive Summary The purpose of the project was to design and build a 1/5th scale Segway cart. The cart was modeled after a two wheeled transportation device that uses sophisticated electronics to balance. The cart was designed to follow a line as fast as possible while still keeping a load balanced atop. The cart was limited to several maximums; a height of 6 inches, a mass of 1-kg, wheel diameters between 0.5 and 6 inches, and removable handlebars from 7-9 inches. The cart also had to support a cylindrical mass with specifications of, a mass up to 2-kg, and a diameter of up to 6-inch. The cost to build the cart was limited to $200.00. The cart was designed to rock on its wheels over a range of 60 degrees forwards and backwards as well as to follow a black electrical tapeline on a light colored floor. The cart was expected to be self-contained including the power source. With the above design constraints, the cart was then designed to be lightweight, structurally strong enough, inexpensive, and to follow the specified path. The cart was constructed of hollow aluminum tubing, which made up the frame. The tubing was soldered together. The cart used a spinning hanging mass attached to a potentiometer to sense the angle of tilt. By measuring the change in voltage in the potentiometer as the cart tilted, the balancing of the cart was regulated. Photo sensors were implemented for detecting the black electrical tape and to start the cart in motion. This was a difficult process, however it was successful. First of all the cart met all of the necessary specifications. Also the cart was successful in several of the test areas. The cart was self-balancing and attempted to support the mass. However if the compensation of the cart was too great it would tip. The photo sensors would read the

tape however a small change in lighting would throw the cart off path. Although not every aspect of the project was perfectly complete the project can easily be determined a success.

Design Description 1.1 Mechanical Design Frame: The major constraints on the frame design were weight and component accommodation. To deal with the issue of weight, aluminum was selected as the primary construction material. Aluminums low cost, high strength to weight ratio, availability, and versatility in joining made it the right choice. The selected aluminum pieces were hollow aluminum tubing. It was originally planned that the pieces be joined using a TIG welder, after experimentation it was determined to be too difficult and time consuming. The process of laser welding was researched. The process would be successful, however the high cost made it unattainable. The final joining choice ended up being the use of a brazing, or high temperature soldering process, using #31 Aladdin 3-in-1 rods. The soldering proved to be the best joining method. The size and shape of the frame was based upon the essential internal components. These components included motors, encoders, the AVR board, and the gyroscope. After construction of the frame it was discovered that the thickness of the tubing was overlooked. As a result of the problem the initial design of the AVR board placement had to be changed. The end result was the AVR board was placed on the underneath of the frame. A drawing of the frame can be seen if Figure 1.1-1.

Figure 1.1-1: Frame Isometric Wheels: The wheels were designed to carry the load of the robot itself and a mass placed atop. The purpose of building the wheels was to cut down on cost as well as the weight of the cart. The wheels were designed to the maximum allowable diameter of 6 in. The reason for the larger wheels was for component placement purposes. With larger wheels more of the components were able to be placed under the axles, thereby lowering the center of mass of the cart. With a lower center of mass the balancing of the cart would occur more naturally. The wheels can be seen in Figure 1.1-2.

Figure 1.1-2: Wheel Isometric Motors: The motors chosen were 5 volt gear motor. There was an initial issue with selecting a 5 volt motor, the amount of torque that it produced. The motor selected had a 120:1 gear ratio, with the added torque of the internal gears the motors were expected to turn the wheels without any problems. Another positive aspect to those motors became apparent in the mass calculation. Each motor had a mass of 20 grams. The motors were expected to account for a large proportion of the overall mass initially. However, the use of the motors below left more room for added mass in other areas. A sketch of the motor is shown in Figure 1.1-3 below.

Figure 1.1-3: Motor Isometric AVR Board: Initially the AVR board was to be placed on the inside of the front plate. However with the design overlook the board didnt fit. Also with the board placed on front the balancing function would have become more complex, due to the cart being top end heavy. The alternative was placing the AVR board underneath the bottom plate of the robot. Positioning the AVR board underneath the bottom plate allowed for easy access to wiring, connecting the serial cable, and resetting the AVR board. The AVR board being placed under the wheel axles also lowered the center of mass of the robot. Gyroscope: The gyroscope was created using a potentiometer, a DC motor, and a hanging mass. The hanging mass consisted of a plastic arm, a 3-volt DC motor, and a disk attached to the motor shaft. The arm was attached to the potentiometer with the DC motor secured into it. The spinning disk was a plastic gear with a

steel washer epoxied to it. The spinning disk was designed to counter the initial acceleration of the robot. As the robot tilted, the potentiometer measured the change in voltage. An assembly of the gyroscope can be seen if Figure 1.1-4.

Figure 1.1-4: Gyroscope Design Encoder Brackets: The brackets used to hold the encoders were made of aluminum. One small sheet of aluminum was cut into strips and then bent by hand to achieve the desired shape. The brackets were important because they allowed the axles to have a sturdy support. The added support was important because any bending could have caused large changes in the Segways motion. Also if the shafts had been bent it could have caused increased friction resulting in insufficient torque. The brackets also allowed easy mounting of the gyroscope. The brackets were simply mounted to the plastic shaft supports by rivets. The brackets can be seen in Figure 1.1-5. 9

Figure 1.1-5: Encoder Bracket Setup Battery Placement: The batteries were placed on the bottom plate of the robot to keep the center of mass of the robot as low as possible. The batteries were also positioned so that the mass was evenly distributed on the robot. The batteries were also kept clear of the swaying gyroscope in the middle of the frame. Top Plate: The top plate was designed to be load bearing, as well as to enclose the entire robot. It was necessary to have access to the internal components of the robot so the top and back plates where hinged. The front plate was secured to the frame and was attached to the top plate with a hinge, which was then attached to the back plate by another hinges. An exploded drawing of the entire cart can be seen below if Figure 1.1-6. 10

Figure 1.1-6: Exploded View of the Cart All individual components of the cart can be seen in full dimension in Appendix A. Also in Appendix A appears the complete assembly of the cart. 1.2 Electrical Design The circuit schematic designated how the components were connected to the control board. The schematic of the electrical circuitry can be seen in Appendix B. The first component shown was the potentiometer. The potentiometer was used to determine the angle of the cart with respect to the desired position. The desired position occurred when the bottom plate of the cart was parallel with the ground. The angle was determined by the output voltage and read by PA0. The two encoders were read by PB0-3. The encoders allowed the program to decipher the velocity that the wheels were experiencing. +5V was given to the encoders and the potentiometer by PD3, and the voltage was grounded by the common ground on the board. Variable resistors represented the light sensors because the resistance varied with the amount of light that each one saw. PA1-3 red the change in voltage due to the varying resistance. The voltage sources were connected to the board through the pins designated +24V and +7V. 11

One 9V battery was used to power the board through the +7V pin. The motors were driven through two 9V batteries connected in parallel and then attached through the +24V pin. Both voltage sources were grounded through the common ground. The ATMega32 chip connected to the L293 driver internally. The L293 chip drove the motors, and OUT1-4 connected the motors to the L293 board. 1.3 Calculations Stress Strain Calculations: Stress calculations were performed during the design process. A full overview of the calculations can be found in Appendix C. The Calculations consist of beam bending and shear analysis as well as column buckling calculations. The bending and shear analysis was performed for the longest loadbearing member in the frame design. Figure 1.3-1 below shows which members were analyzed and the assumed and actual loads for the frame. The reasoning was if the longest unsupported member could withstand the full load of the cart payload, the rest of the stronger members would as well. The calculations showed that the frame would be amply strong for the load it would have to carry. Specific safety factors were 1.69, 107.9, and 95.6 for bending, shear, and buckling stresses respectively. The bending safety factor may seem low at 1.69, but it was determined to be acceptable when the differences between the assumed loading used in the calculations and the actual loading were considered. In the calculations, it was assumed that the entire load would be placed on a single point at the center of one of the two longest members. In reality the load was divided

12

between both of the long top members, and was distributed. Therefore the assumed loading conditions were over estimated.

Figure 1.3-1: Loading and Member Diagram Equations of Motion: To determine the differential equations of motion for the cart first the appropriate components of the cart needed to be modeled. The selected components were the entire cart system and the wheel. Free body diagrams were created to account for all of the forces applied. The free body diagrams can be seen in Figures 1.3-2 and 1.3-3.

13

M c dc Fc cos c M c cos c x M c g sin c

d
c

2 M c d c

M c sin c x

d= Mc = Fc = g= c = x= c =

Distance between center of mass and axial or rotation Mass of the cart Force of contact at the axial Gravity Angle of rotation of the body Translational direction Applied cart torque Figure 1.3-2: Free Body Diagram for the Cart

M c g cos c

14

w
J ww

c
M w x
F floor R

Fc
M w = Mass of the wheel Fc = Force of contact F floor = Force of friction due to the floor w = Angle of rotation of the wheel c = Angle of rotation of the cart = Torque Radius R= x= Direction of translation Jw = Mass moment of Inertia Figure 1.3-3: Free Body Diagram for the Wheel

The forces in both free body diagrams were summed to get equations 1, 2, and 3. x Eq1: M c dc + Fc cos c + M c cos c M c g sin c c = 0 2 x Eq2: M c d c + M c sin c + M c g cos c = 0 Eq3: Fc = floor M w g + M w Rww + FMotor (1) (2) (3)

15

In the FBDs two angles are accounted for, the angle of rotation of the cart and the wheel. In order to simplify the equations in terms of one angle of rotation a relationship needed to be developed. The angular relationship can be seen below in Equation 4. x = w Rw x = cd w Rw = c d (4)

c =

Rw w d

Also there had to be a connection between the torque on the cart and the torque on the wheel. With that relationship a relationship between the torque and the These relationships can be seen below in

position x could be determined. equations 5 and 6.

c = M c d x = J w w

(5) (6)

Using the relationships as well as the sum of the forces equations the differential equations of motion were determined and can be seen below in equation 7 and 8.
R R R R w M c d w + ( M w + M c ) Rw cos w w + ( Floor M w g + FMotor ) cos w w + M c g sin w w + d d d d

R Mc J d = 0 w

(7)

16

R R sin w w + x 2 + g cos w w = 0 x d d

(8)

To find the force due to the motor, motor parameters where determined as seen in equation 9. K2 K FMotor Rw w + w JR = Vs JR J K d K2 w w FMotor = Vs R R R R w Rw w =
w w

K d K 2 FMotor = Vs R R w R w R R w w w

(9)

The motor parameter equation 9 was placed into the differential equations and simplified. Then the final equations were put into state variable form as shown below in equations 10-13. w = w R K 2 cos w w d w = w d Rw 2 M c Rw R + ( M w Rw + M c Rw ) Rw R cos w dw d R R ( Floor M w g + Vs K ) cos w w + ( M c g sin w w + M c R R R J d d w w d R M c Rw + ( M w Rw + M c Rw ) cos w w dw d (10)

d )

(11)

17

x=v R g cos w w 1 d v = v Rw Rw sin w sin w d d 1.4 Software Design

(12)

(13)

The system architecture diagram in Figure 1.4-1 was to control a Segway robot, driven with two 5-volt motors. There were two PWM outputs that drove the motors. As the cart moved it was to track a line using three optical line sensors. The angular position of the cart was to be detected using a simple mass hanging from a potentiometer with a spinning disk attached to a small 3-volt motor. The position/velocity of the wheels were verified using encoders for each wheel.

18

Figure 1.4-1: System Block Diagram The block diagram below, in Figure 1.4-2, shows a visual representation of the desired cart control. The cart began by reading the light sensors to detect the presence of a line. If a line was present, the cart was to move forward under

19

direction from the motion control functions. The motion control functions were set in the C code. These functions took inputs from PI (proportional and integral) control functions, which in turn took inputs from the encoders and the potentiometer (used as a gyro). The entire system was used to attempt the balancing of the robot as well as the guiding of the robot along an electrical tape path.

Figure 1.4-2: System Control Diagram The block diagrams in the above figures were used in assembling the C program. The program can be seen in Appendix D.

1.5 Cost & Weight Inventory

20

A constant inventory of the cost of the cart as well as the weight was kept. The purpose of this process was documentation, to ensure that the cost and weight were constantly being considered. Table 1.5 below shows the documentation. Table 1.5: Cost & Weight Inventory
Part AVR Board Mechanical Encoder 5V DC Motor Motor Supports 9V Batteries 9V Battery Strap AA Batteries Material for 6" Wheels (0.25 in Plastic) Photo Sensors Gears Potentiometer Aluminum for Exterior Gyro Motor Steel for Gyro Dist Aluminum Rods Non-Skid Plastic for Shaft Supports (in frame) Hinges Epoxy Solder Switches Machining Extras TOTALS Price ($) 37 2.58 5.29 0.9 2 0.9 1.5 Quantity 1 2 2 4 3 3 2 Total Cost ($) 37 5.16 10.58 3.6 6 2.7 3 Mass/ Piece (g) 89.5 12.8435 27 0.568 50 0.568 25 Mass (Kg) 0.0895 0.025687 0.054 0.002272 0.15 0.001704 0.05 Source GVSU GVSU Hobby Engineering GVSU GVSU GVSU Walgreens

10 0.57 6.26 3.61 1.5 5 1.5 1.39 6 1.5 0.29 3.49 24.99 3.99

2 2 1 1 1 1 1 10 1 2 4 1 0.05 2

20 1.14 6.26 3.61 1.5 5 1.5 13.9 6 3 1.16 3.49 1.2495 3.99 $139.84

33.5 0.903 3.2 12.8 121.5 10 45 0.375 20 22 0.3 2 453.6 5 88

0.067 0.001806 0.0032 0.0128 0.1215 0.01 0.045 0.00375 0.02 0.044 0.0012 0.002 0.02268 0.005 0.088 0.8211

GVSU GVSU Probelay GVSU GVSU GVSU GVSU Riders Modern Skate and Surf GVSU House of Hobbies Riders Riders Riders GVSU Kg

Receipts for the parts purchased can be seen in Appendix F.

2.0 Test Results

21

2.1 Simulation The Scilab program was used to verify the operation of the system using the equations of motion shown in equations 8-11. The graph of motion was expected to have some oscillation as the robot tried to maintain a balanced vertical motion during translation. The other line on the graph shows the angle of the wheels rotation, which will constantly be increasing. The graph of the simulation can be seen in figure 2.1-1.

Meters

Figure 2.1-1: Simulation Result Graph The entire Scilab program can be seen in Appendix E.

2.2 Pre-Tests

22

The robot was tested on both Monday November 15th and Friday November 19th. Neither test went well. Monday the robot was nonfunctional. It also had several newly discovered mechanical problems. One of the wheels had cracked the night prior so a new one needed to be made. The AVR board had been mounted on the front plate; the problem was that a dimension oversight caused the height of the robot to be too small to fit the AVR board. Finally the gyro construction needed to be altered. All of the mechanical issues needed to be fixed before the next test. Also, by the next test the robot needed to be semi functional. Fridays test went better. All of the mechanical problems had been fixed and the software was semi functional. The robot attempted to balance in the forward direction only but was unsuccessful. The robot did not yet move forward or follow a line but the balancing was in progress. 2.3 Final Results The final test was a success. The robot fit all of the necessary build and weight requirements. The robot also cost quite a bit less then the required amount. On the actual test day the robot made an excellent showing. Although the robot didnt accomplish every necessary test perfectly, it did accomplish some and attempt them all. The first test was the self balancing with no mass atop. The robot accomplished that test with flying colors. The next test was balancing with a mass atop. That test was more difficult the robot held up the mass and balanced for a while, however the mass became too much and the robot overcompensated.

23

The overcompensation caused the mass to fall. The next test was following a line without an applied mass. The robot saw the line and attempted to follow but got off track about a quarter of the way through. The anticipated cause was that the lighting in the testing area was much different then the lighting in the area that it was pre-tested. The final test was attempted but not accomplished; it was balancing a mass while following a line. The robot overcompensated and dropped the mass before it had a chance to see the line. Although all of the expectations were not completed perfectly, the robot did have all of the appropriate components to test perfectly. More tweaking and programming would have been necessary to get the robot to do every task. Despite that however the robot was seen as a success. 2.4 Score Estimation t score = s (4) 200 (10) B (10) T (2) 0.2 S 2 d t s = Time to complete the course (s) C = Total cost ($) d = Distance traveled (m) B = Build Quality (0=best, 1=worst) T = Theory Quality (0=best, 1=worst) M = Mass of Cart (Kg) S = Spillage in the mass (ml) Due to the above calculation shown in equation 12 the expected score is 4559309. The values that correspond are; time = 0.5s, cost = $139.84, distance = 0.5m, build quality = 0.3, theory quality = 0.1, mass = 0.8211Kg, and amount spilled = 200ml (all of the contents). 3.0 Conclusion
2 C M

(12)

24

The robot was a success overall. All of the physical requirements for the robot were completed effectively. The robot was under budget and under weight. The real issue holding the project back from total success was the time constraint. The program was not functioning up to its full potential, but given more time it could be manipulated to achieve all of the goals set forth and beyond. The Segway balanced itself, balanced the mass temporarily, and saw the line. All of this included the robot was a great accomplishment.

4.0 Recommendations There were many aspects of the project that could be improved upon. First, all of the necessary parts were not ordered until the final proposal was approved. The problem with this was that it only allowed a few days for delivery before the construction needed to be started. Many of the other groups began construction and ordering much sooner. This delay significantly decreased the amount of build time available. Also ordering extra parts would have increased the success of the robot. Specifically one of the motors didnt perform as well, and having and extra would have helped significantly. Another large issue is the double-checking of others work, to ensure it validity as well as its progress. If more double-checking had been done a lot of the last minute problems could have been corrected sooner.

25

Appendix A: Drawings

26

27

Segway Design: Bill of Materials


Item #
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20

Part Description
Batteries Controller Encoder Encoder Brackets Potentiometer Support Gyro Bracket Top Bottom Frame Assembly Gyroscope Assembly Handle Bar Assembly Light Sensors Motors (5V) Exterior Side Cover Small Gear Wheel Assembly Rivets Grip Tape Plumbers Solder

Qty
3 1 2 2 1 1 1 1 1 1 1 3 2 2 2 2 2 14 12x12 sheet 36

28

29

30

31

32

33

34

35

36

37

38

39

40

41

42

43

44

Appendix B: Electrical Schematic

45

46

Appendix C: Stress Strain Calculations

47

Frame Failure Calculations: The following calculations are to ensure that the frame is structurally sound. Bending and shear calculations are done for the longest member, which experiences these loads. The buckling calculations are done for the 3.5-inch vertical coulombs in the frame.

b :=

3 16

in

h :=
3

3 16

in

b1 :=

5 32
5

in

h1 :=

5 32

in

b h b1 h1 Ix := 12 12

Ix = 5.333 10

in

Below is the FBD of the member:

48

sum of forces in the x: F1+F2=20N sum of forces in the y: F3=0N sum of moments about point A: (F2)(3in)-(F1)(3in)=0 therefor: F1=F2=10N Using this information, shear fove diagram can be constructed for the beam Integrating the shear foce diagram produced the bending moment diagram for the beam. A diagram ov both follows:

Bending Moment and Sear force Plot


35 30 25 20 15 10 5 0 -5 0 -10 -15 Distance (in) 2 4 6 8 shear force (N) Bending Moment (N*in)

49

From these two diagrams, it can be seem that the maximum bending moment is at the center of the members length, and the maximum shear foce is at the end of the member. Their magnitudes are as follows: M max:= 30N in shear max:= 10N

From the material tables in the back of Mechanics of Materials, Fifth Edition : lbf yield := 20000 2 in Calculating bending stress in the member: Ybar := 3 32 in maxinum tensile stress will be at the bottom of the member cross section

x :=

Mmax Ybar Ix

x = 1.186 10

4 lbf

in

Nsafety :=

yield x

Nsafety = 1.687

Therefore the frame is safe in bending. Actually, this model assumes the total force of the load is carried on one member, when in the real design it will be equally distributed over two members. This means the bending safety factor above is half of what it will be in reality, making the design twice as safe as is calculated here. Also, the real load will be a distributed force over the entire beam as opposed to the point force shown above. Since a distributed force is less harsh than a point force, the frame will be safer still than calculated above.

Calculating the shear stress in the member:


2

max:=

shear max h 8 Ix

lbf max = 185.261 2 in

NsafetyShear :=

yield max

NsafetyShear = 107.956

Therefore the member is safe in shear.

50

Buckling calculations for virticle member: This calculation is for the frame's vertical columns which support teh ends of the beam analyzed above.

L := 3.5in

Ealuminum := 1000010 psi

Pcr :=

Ealuminum Ix 4 L
2

This formula models the member as a fixed free columb, which is the most conservative way to model the member This is the maximum load the member can support without buckling

Pcr = 107.41lbf Pactual := 1.12359 lbf

Pcr Nsafety_Buckling := Pactual

Nsafety_Buckling = 95.595

Therefore the member is safe in buckling

51

Appendix D: C Program

52

//345 project //Team #2 #include <avr/io.h> #include <avr/signal.h> #include <avr/interrupt.h> #include "sio.c" // Deadband Limits #define c_kinetic_pos 0x95 #define c_kinetic_neg 0xA4 #define c_static_pos 0xd0 #define c_static_neg 0xe6 #define c_kinetic_pos2 0x65 #define c_kinetic_neg2 0x90 #define c_static_pos2 0xc0 #define c_static_neg2 0xd6 #define c_max 255 #define c_min 255 #define T 4 #define Kp 5 #define Ki 5 #define Kd int db_correct = 1; int Angle_int; int Pdesired = 0x88; int sensor1; int sensor2; int sensor3; int motorL = 0; int motorR = 0; int VinitR = 0; int VinitL = 0; int e_sum1 = 0; int e_sum2 = 0; void delay(int ticks); void IO_update(void); int AD_read(); /* ATMEGA32 I/O register defines */

// Left Motor

// Right Motor

/* make the value positive */

5 // deadband correction is on by default // Desired vertical position of the pot // Change in voltage at each sensor due to a change in resistance // velocity of the motors

/**************************Integration**************************/ int integrateL(int e1) { e_sum1 += e1 * T; return e_sum1; } int integrateR(int e2) { e_sum2 += e2 *T;

53

return e_sum2; } int diffR(int errorR) { int DR; DR = errorR - VinitR; VinitR = errorR; return DR; } int diffL(int errorL) { int DL; DL = errorL - VinitL; VinitL = errorL; return DL; } #define CLK_ms 10 // set the updates for every 10ms, same as CLK_ms /***********************Controller Function********************/ //int Cd = 0; // Initialize desired position int controllerL(int Cd, int Cf) { int Ce; // Position error int Cw; // Error with gain compensation Ce = Cd - Cf; Cw = Kp * Ce + (Ki * integrateL(Ce) * CLK_ms/ 1000) + Kd*diffL(Ce) *(1000/CLK_ms); return Cw; } int controllerR(int Cd, int Cf) { int Ce; int Cw; Ce = Cd - Cf; Cw = Kp * Ce + (Ki * integrateR(Ce) * CLK_ms/ 1000)+ Kd*diffR(Ce) * (1000/CLK_ms); return Cw; } // Differentiate pot position // Differentiate pot position

// Read the pot

54

int pot_feedback() { ADMUX = 0x00; // set the input to channel 1 (PA0) ADCSRA = 0xC0; // turn on the ADC and set it to read once while((ADCSRA & _BV(ADSC)) != 0x00){} Angle_int = AD_read(); return Angle_int; ADCSRA =0; } /*******************************Clock Setup********************************/ unsigned int CNT_timer1; volatile unsigned int CLK_ticks = 0; volatile unsigned int CLK_seconds = 0; SIGNAL(SIG_OVERFLOW0){ // The interrupt calls this function (no change in function) CLK_ticks += CLK_ms; if(CLK_ticks >= 1){ // The number of interrupts between output changes CLK_ticks = CLK_ticks - 1; CLK_seconds++; IO_update(); } TCNT0 = CNT_timer1; } void CLK_setup(){ // Start the interrupt service routine //no change in function // the delay time // the current number of ms // the current number of seconds

TCCR0 = (0<<FOC0) | (0<<WGM01)| (0<<WGM00) | (0<<COM00)| (0<<COM01) | (1<<CS02) | (0<<CS01) | (1<<CS00); // use CLK/1024 prescale value // disable PWM and Compare Output Modes CNT_timer1 = 0xFFFF - CLK_ms * 8; // 8 = 1ms, 80 = 10ms TCNT0 = CNT_timer1; // start at the right point TIFR |= (1<<TOV0); TIFR &= ~(0<<OCF0); TIMSK |= (1<<TOIE0); TIMSK &= ~(0<<OCIE0); sei(); } /************************ end clock setup************************************/

// enable interrupts flag

/********************************Deadbands***********************************/ int deadbandL(int c_wanted) /* call this routine when updating */

55

{ int c_pos1; int c_neg1; int c_adjustedL; if(motorL > 0) { c_pos1 = c_kinetic_pos; c_neg1 = c_kinetic_neg; } else { c_pos1 = c_static_pos; c_neg1 = c_static_neg; } if(c_wanted == 0) /* turn off the output */ { c_adjustedL = 0; } else if(c_wanted > 0) /* a positive output */ { c_adjustedL = c_pos1 + (unsigned)(c_max - c_pos1) * c_wanted / c_max; if(c_adjustedL > c_max) c_adjustedL = c_max; } else { /* the output must be negative */ c_adjustedL = -c_neg1 - (unsigned)(c_min - c_neg1) * -c_wanted / c_min; if(c_adjustedL < -c_min) c_adjustedL = -c_min; } return c_adjustedL; } int deadbandR(int c_wanted) /* call this routine when updating */ { int c_pos2; int c_neg2; int c_adjustedR; if(motorR > 0) { c_pos2 = c_kinetic_pos2; c_neg2 = c_kinetic_neg2; } else { c_pos2 = c_static_pos2; c_neg2 = c_static_neg2; } if(c_wanted == 0) /* turn off the output */ { c_adjustedR = 0; } else if(c_wanted > 0) /* a positive output */ {

56

c_adjustedR = c_pos2 + (unsigned)(c_max - c_pos2) * c_wanted / c_max; if(c_adjustedR > c_max) c_adjustedR = c_max; } else { /* the output must be negative */ c_adjustedR = -c_neg2 - (unsigned)(c_min - c_neg2) * -c_wanted / c_min; if(c_adjustedR < c_min) c_adjustedR = -c_min; } return c_adjustedR; } /******************************End of Deadband********************************/

int AD_read() { return (ADCW); } /*******************************PWM Stuff*************************************/ void PWM_init() /* call this routine once when the program starts */ { DDRD |= (1 << PD5) | (1 << PD4); /* set PWM outputs */ DDRC |= (1 << PC0) | (1 << PC1) | (1 << PC2) | (1 << PC3); /* set motor direction outputs on port C*/ /*using OCR1*/ TCCR1A = _BV(COM1A1) | _BV(COM1B1) | _BV(WGM10); // turn on both PWM outputs on counter 1 TCCR1B = _BV(CS11) ; // set the internal clock to 1/8 clock } void PWM_updateL(int value) /* to update the PWM output for motor1*/ { if(value > 255) value = 255; if(value < 0) value = 0; OCR1A = value; // duty cycle motor 1 }

void PWM_updateR(int value2) /* to update the PWM output for motor 2*/ { if(value2 > 255) value2 = 255; if(value2 < 0) value2 = 0; OCR1B = value2; // duty cycle }

57

/******************************End of PWM Stuff*******************************/ /*******************************Voltage Output********************************/ int v_outputL(int v_adjustedL) /* call from the interrupt loop */ { int RefSignal; // the value to be returned if(v_adjustedL >= 0) { /* set the direction bits to CW on, CCW off */ PORTC = (PINC & 0xFC) | 0x02; /* bit 1 on, 0 off */ if(v_adjustedL > 255) /* clip output over maximum */ { RefSignal = 255; } else { RefSignal = v_adjustedL; } } else { /* need to reverse output sign */ /* set the direction bits to CW off, CCW on */ PORTC = (PINC & 0xFC) | 0x01; /* bit 0 on, 1 off */ if(v_adjustedL < -255) /* clip output below minimum */ { RefSignal = 255; /* the output must be negative */ } else { RefSignal = -v_adjustedL; /* flip sign */ } } return RefSignal; } //adjusting for negative voltages int v_outputR(int v_adjustedR) { int RefSignal; if(v_adjustedR >= 0) { PORTC = (PINC & 0xF3) | 0x04; if(v_adjustedR > 255) { RefSignal = 255; } else { RefSignal = v_adjustedR; } } else { PORTC = (PINC & 0xF3) | 0x08; if(v_adjustedR < -255) /* call from the interrupt loop */

/* set the direction bits to CW on, CCW off */ /* bit 3 on, bit 4 off*/ /* clip output over maximum */

/* need to reverse output sign */ /* set the direction bits to CW off, CCW on */ /* bit 3 off, bit 4 on */ /* clip output below minimum */

58

{ RefSignal = 255; } else { RefSignal = -v_adjustedR; /* flip sign */ } } return RefSignal; } //adjusting for negative voltages /*******************************End Of Voltage Output*******************************/ void IO_setup() { PWM_init(); CLK_setup(); ADCSRA=(ADCSRA|0x80); } //turn ADC on

void delay(int ticks) // ticks are approximately 1ms { volatile int i, j; for(i = 0; i < ticks; i++) { for(j = 0; j < 1000; j++){} } } //this function sucks and is never called

/*******************************Photo Sensors********************************/ int sensor1_func() /* reads sensor1 (right sensor)*/ { ADMUX = 0x01; // set the input to channel PA0 ADCSRA = 0xC0; // turn on the ADC and set it to read once while((ADCSRA & _BV(ADSC)) != 0){} sensor1 = (AD_read()); return sensor1; ADCSRA = 0; } int sensor2_func() /* reads sensor2 (middle sensor)*/ { ADMUX = 0x03; // set the input to channel 1 PA3 ADCSRA = 0xC0; // turn on the ADC and set it to read once while((ADCSRA & _BV(ADSC)) != 0x00){} sensor2 = (AD_read()); return sensor2;

59

ADCSRA = 0; } int sensor3_func() // reads sensor3 (left sensor) { ADMUX = 0x05; // set the input to channel 1 PA5 ADCSRA = 0xC0; // turn on the ADC and set it to read once while((ADCSRA & _BV(ADSC)) != 0x00){} sensor3 = (AD_read()); return sensor3; ADCSRA = 0; } void sensorcompare() { if(sensor2 <= 0x78) { motorL = 50; motorR = 50; if(sensor1 > 0x19e && sensor2 < 0x02b8) { motorL += 0x04; motorR -= 0x04; } else if(sensor2 < 0x02b8 && sensor3 > 0x02ce) { motorL -= 0x04; motorR += 0x04; } else { motorL = motorL; motorR = motorR; } } else { motorL = 0; motorR = 0; } } /*****************************End of Photo Sensors****************************/ void IO_update() // This routine will run once per interrupt for updates { // Compare the sensor values

60

sensor1_func(); sensor2_func(); sensor3_func(); sensorcompare(); PWM_updateL(v_outputL(deadbandL(motorL))); PWM_updateR(v_outputR(deadbandR(motorR))); pot_feedback(); int Pactual = Angle_int; PWM_updateL(v_outputL(deadbandL(controllerL(Pdesired, Pactual)))); PWM_updateR(v_outputR(deadbandR(controllerR(Pdesired, Pactual)))); } int main() { sio_init(); IO_setup(); sio_cleanup(); return 0; }

61

Appendix E: Scilab Program

62

// System component values l = 0.4; // 40cm length from the wheel axel to the load on top of the segway Mw = 1.0; // 1kg mass of wheel Mc = 0.2; // 200g mass of segway and load g = 9.81; // gravity rw = 0.0762; // 0.1524m diameter wheels K = .5; // motor speed constant R = 7; // motor resistance // J = 0.005; // rotot inertia uf = 0.1; // floor friction // System state x0 = 0; // initial conditions for position v0 = 0; theta0 = 0.0; // the initial position for the load, cannot divide by zero 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 segway 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 = 20; // position feedback gain Ksp = 2; // sway feedback gain Vpwmmax = 12; // PWM output limitations in V Cpwmmax = 255; //PWM input range Cdeadpos = 100; // deadband limits Cdeadneg = 95; 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

63

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; 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 Xd = 10; // the setpoint 10 turns == 160 pulses Cd = ppr * Xd / (2*PI*rw); function foo=derivative(state,t, h) term1 = rw*state($,3)/l; term2 = -asin(term1)*(state($,2))^2 - g*atan(term1); term3 = (Mc*R*rw^2)/cos(term1) + R*(rw^2)*(Mc + Mw) + l*R; term4 = (state($,2)^2)*K/(rw*R) - uf*Mw*g - Mc*g*tan(term1); term5 = (Mc*rw)/cos(term1) + rw*(Mc + Mw) - l/rw; foo = [state($,2), term2, state($,4), state($,4)*(K^2)/(term3) - term4/term5]; endfunction // Integration set the time length and step size for the integration steps = 1000; // The number of steps to use t_start = 0; // the start time - normally use zero t_end = 10; // 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 plot2d(t, [X(:,1)], [-2], leg="mass position"); plot2d(t, [X(:,1), X(:,3)], [-2, -5], leg="position@theta (X 10)"); xtitle('Time (s)'); // Integration set the time length and step size for the integration //steps = 1000; // The number of steps to use //t_start = 0; // the start time - normally use zero //t_end = 10; // the end time //h = (t_end -t_start) / steps; // the step size //t = [t_start]; // an array to store time values // Loop for integration //for i=1:steps, //

t = [t ; t($,:) + h];

64

// // // // // //end

F1 = h * f(X($,:), t($,:)); F2 = h * f(X($,:) + F1/2.0, t($,:) + h/2.0); F3 = h * f(X($,:) + F2/2.0, t($,:) + h/2.0); F4 = h * f(X($,:) + F3, t($,:) + h); X = [X ; X($,:) + (F1 + 2.0*F2 + 2.0*F3 + F4)/6.0];

// Graph the values //plot2d(t, X, [-2, -5, -7, -9], leg="mass position"); //xtitle('Time (s)');

65

Appendix F: Gantt Chart

66

67

68

Potrebbero piacerti anche