Segway Report

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 Van’thof

Table of Contents

Executive Summary 3

Design Description:

Mechanical Description 5

Electrical Design 11

Stress Stain Calculations 12

Equations of Motion & FBD’s 13

Software Design 18

System Block Diagram 18

System Control Diagram 19

Cost & Weight Inventory 20

Test Results:

Simulation 21

Pre-Test & Final Test Information 22

Score Calculation 23

Conclusions & Recommendations 24

Appendices:

Appendix A: Drawings 25

Appendix B: Electrical Schematic 44

Appendix C: Stress Strain Calculations 46

Appendix D: C Program 51

Appendix E: Scilab Program 61

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 didn’t 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

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.

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.

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 load-bearing 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

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.

=

d Distance between center of mass and axial or rotation

=

c

M Mass of the cart

=

c

F Force of contact at the axial

=

g Gravity

=

c

θ Angle of rotation of the body

=

x Translational direction

=

c

τ Applied cart torque

Figure 1.3-2: Free Body Diagram for the Cart

c c

g

M

sin

θ

d

g

M

cos

θ

x

M



cos

θ

x

M



sin

θ

F

cos

θ

d

M

θ



d

M

θ



x



=

w

M Mass of the wheel

=

c

F Force of contact

=

floor

F _{Force of friction due to the floor}

=

w

θ Angle of rotation of the wheel

=

c

θ Angle of rotation of the cart

= τ Torque = R Radius = x Direction of translation = w

J 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. Eq1: M_cdθ_c+F_ccosθ_c +M_cxcosθ_c −M_cgsinθ_c −τ_c =0 ₍₁₎

Eq2: 2 ₊ sin ₊ cos ₌0

c c c c c cd M x M g M θ  θ θ (2)

Eq3: Fc =−µfloorMwg+MwRwθw +FMotor (3)

c

F

F

R

x

M





J

θ





τ

θ

θ

In the FBD’s 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. w w c c w w c w w d R d R d x R x θ θ θ θ θ θ = = ∴ = = (4)

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 position x could be determined. These relationships can be seen below in equations 5 and 6. d x M_c c =  τ (5) τ =Jwθw (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.

+       +       + +           + +       w w c w w Motor w Floor w w w c w w c w d R g M d R F g M d R R M M d R d M θ µ θ θ

θ ( ) cos ( )cos sin

0 =     d J R M w c τ (7)

0 cos sin 2 _=      + +       w w w w d R g x d R x θ  θ ₍₈₎

To find the force due to the motor, motor parameters where determined as seen in equation 9. ∴ =     −     −     =       −       =     + w w w w w w w s Motor w Motor s w w R R K R d R R K V F J R F JR K V JR K θ ω ω ω ω ω    2 2     −     −     = R R K R d R R K V F w w w w w s Motor 2 θ θ  ₍₉₎

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 (10)                     − + +     +       +           + −                   − + +       − = w w w w c w w w c w c w w c w w w s w Floor w w w w w c w w w c w w w w d R d d R M R M R M d J R M d R g M d R R R K V g M d R R R d d R M R M R R M d R K θ τ θ θ µ θ θ ω ω cos ) ( ) sin ( cos ) ( cos ) ( cos 2 2  (11)

x =v (12)                         −                   − = w w w w w w d R d R g d R v v θ θ θ sin cos sin 1  ₍₁₃₎ 1.4 Software Design

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.

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

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.

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 Price ($) Quantity Total Cost ($) Mass/ Piece (g) Mass (Kg) Source

AVR Board 37 1 37 89.5 0.0895 GVSU Mechanical

Encoder 2.58 2 5.16 12.8435 0.025687 GVSU 5V DC Motor 5.29 2 10.58 27 0.054 EngineeringHobby Motor Supports 0.9 4 3.6 0.568 0.002272 GVSU 9V Batteries 2 3 6 50 0.15 GVSU 9V Battery Strap 0.9 3 2.7 0.568 0.001704 GVSU AA Batteries 1.5 2 3 25 0.05 Walgreens Material for 6"

Wheels (0.25 in

Plastic) 10 2 20 33.5 0.067 GVSU Photo Sensors 0.57 2 1.14 0.903 0.001806 GVSU Gears 6.26 1 6.26 3.2 0.0032 Probelay Potentiometer 3.61 1 3.61 12.8 0.0128 GVSU Aluminum for

Exterior 1.5 1 1.5 121.5 0.1215 GVSU

Gyro Motor 5 1 5 10 0.01 GVSU

Steel for Gyro Dist 1.5 1 1.5 45 0.045 GVSU Aluminum Rods 1.39 10 13.9 0.375 0.00375 Riders Non-Skid 6 1 6 20 0.02 Modern Skate and Surf Plastic for Shaft

Supports (in frame) 1.5 2 3 22 0.044 GVSU Hinges 0.29 4 1.16 0.3 0.0012 House of Hobbies Epoxy 3.49 1 3.49 2 0.002 Riders Solder 24.99 0.05 1.2495 453.6 0.02268 Riders Switches 3.99 2 3.99 5 0.005 Riders Machining Extras 88 0.088 GVSU

TOTALS $139.84 0.8211 Kg

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

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.

Figure 2.1-1: Simulation Result Graph

The entire Scilab program can be seen in Appendix E.

2.2 Pre-Tests

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 didn’t 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.

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 200 0.2 2 2 ) 2 ( ) 10 ( ) 10 ( ) 4 ( S d t score M T B C s _      = (12) = s

t 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).

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 didn’t perform as well, and having and extra would have helped significantly. Another large issue is the double-checking of other’s 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.

Segway Design: Bill of Materials

Item #

Part Description

Qty

1 Batteries 3 2 Controller 1 3 Encoder 2 4 Encoder Brackets 2 5 Potentiometer Support 1 6 Gyro Bracket 1 7 Top 1 8 Bottom 1 9 Frame Assembly 1 10 Gyroscope Assembly 1

11 Handle Bar Assembly 1

12 Light Sensors 3 13 Motors (5V) 2 14 Exterior 2 15 Side Cover 2 16 Small Gear 2 17 Wheel Assembly 2 18 Rivets 14

19 Grip Tape 12”x12” sheet

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 16in := h 3 16in := b1 5 32in := h1 5 32in := Ix b h 3 ⋅ 12 b1 h1⋅ 3 12 − := _{Ix 5.333 10}= × −5in4

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

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

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:

Mmax 30N in:= ⋅ _{shearmax 10N}:=

From the material tables in the back of Mechanics of Materials, Fifth Edition:

σ_yield 20000lbf in2

:=

Calculating bending stress in the member:

Ybar −3 32in

:= maxinum tensile stress will be at the bottom of the member cross section

σ_x −Mmax⋅Ybar Ix := σ_{x 1.186 10}× 4lbf in2 = 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:

τ_max shearmaxh 2 ⋅ 8 Ix⋅ := τ_{max 185.261}lbf in2 = NsafetyShear σ_τyield max := _{NsafetyShear 107.956}=

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}:= ⋅ 3psi

Pcr π

2

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

Pcr 107.41lbf= This is the maximum load the member can support without buckling

Pactual 1.12359lbf:=

Nsafety_Buckling Pcr Pactual

:= _{Nsafety_Buckling 95.595}=

//345 project //Team #2

#include <avr/io.h> /* ATMEGA32 I/O register defines */ #include <avr/signal.h>

#include <avr/interrupt.h> #include "sio.c"

// Deadband Limits

#define c_kinetic_pos 0x95 // Left Motor #define c_kinetic_neg 0xA4

#define c_static_pos 0xd0 #define c_static_neg 0xe6

#define c_kinetic_pos2 0x65 // Right Motor #define c_kinetic_neg2 0x90

#define c_static_pos2 0xc0 #define c_static_neg2 0xd6 #define c_max 255

#define c_min 255 /* make the value positive */ #define T 4

#define Kp 5 #define Ki 5

#define Kd 5

int db_correct = 1; // deadband correction is on by default int Angle_int;

int Pdesired = 0x88; // Desired vertical position of the pot

int sensor1; // Change in voltage at each sensor due to a change in resistance int sensor2;

int sensor3;

int motorL = 0; // velocity of the motors 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();

/**************************Integration**************************/ int integrateL(int e1)

{

e_sum1 += e1 * T; return e_sum1; }

int integrateR(int e2) {

return e_sum2; }

int diffR(int errorR) {

int DR;

DR = errorR - VinitR; // Differentiate pot position VinitR = errorR;

return DR; }

int diffL(int errorL) {

int DL;

DL = errorL - VinitL; // Differentiate pot position 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;

}

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; // the delay time

volatile unsigned int CLK_ticks = 0; // the current number of ms volatile unsigned int CLK_seconds = 0; // the current number of seconds 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

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(); // enable interrupts flag }

/************************ end clock setup************************************/

{ 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; }

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

/******************************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) /* call from the interrupt loop */ {

int RefSignal; if(v_adjustedR >= 0)

{ /* set the direction bits to CW on, CCW off */ PORTC = (PINC & 0xF3) | 0x04; /* bit 3 on, bit 4 off*/

if(v_adjustedR > 255) /* clip output over maximum */ { RefSignal = 255; } else { RefSignal = v_adjustedR; } }

else /* need to reverse output sign */

{ /* set the direction bits to CW off, CCW on */ PORTC = (PINC & 0xF3) | 0x08; /* bit 3 off, bit 4 on */

{

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){}

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() // Compare the sensor values {

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

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; }

// 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

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

// 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]; //end

// Graph the values

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