segway report

Segway Project: Final Report

Grand Valley State University

School of Engineering

EGR 345: Dynamic System Modeling & Control

Team 2:Jayme Cook

Charlie DeCosterJohn WellesIan RudnikJen Brauss

Jacob Van’thof

1

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

Appendix F: Gantt Chart 65

2

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

3

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.

4

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.

5

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.

6

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.

7

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

8

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

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

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 directionApplied cart torque

Figure 1.3-2: Free Body Diagram for the Cart

14

cc gM sin

d

cc gM cos

cc xM cos

cc xM sin

ccF cosccdM

2ccdM

x

Mass of the wheel Force of contact

Force of friction due to the floor Angle of rotation of the wheel Angle of rotation of the cart

Torque Radius Direction of translation

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: (1)

Eq2: (2)

Eq3: (3)

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

15

cF RF floor

xM w wwJ

w

c

relationship needed to be developed. The angular relationship can be seen below

in Equation 4.

(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.

(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.

(7)

(8)

To find the force due to the motor, motor parameters where determined as seen in

equation 9.

16

(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.

(10)

(11)

(12)

(13)

1.4 Software Design

17

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)

18

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

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

19

PartPrice

($) QuantityTotal Cost

($)Mass/ Piece

(g) Mass (Kg) SourceAVR Board 37 1 37 89.5 0.0895 GVSUMechanical Encoder 2.58 2 5.16 12.8435 0.025687 GVSU

5V DC Motor 5.29 2 10.58 27 0.054Hobby

EngineeringMotor Supports 0.9 4 3.6 0.568 0.002272 GVSU9V Batteries 2 3 6 50 0.15 GVSU9V Battery Strap 0.9 3 2.7 0.568 0.001704 GVSUAA Batteries 1.5 2 3 25 0.05 Walgreens

Material for 6" Wheels (0.25 in Plastic) 10 2 20 33.5 0.067 GVSUPhoto Sensors 0.57 2 1.14 0.903 0.001806 GVSUGears 6.26 1 6.26 3.2 0.0032 ProbelayPotentiometer 3.61 1 3.61 12.8 0.0128 GVSUAluminum for Exterior 1.5 1 1.5 121.5 0.1215 GVSU

Gyro Motor 5 1 5 10 0.01 GVSUSteel for Gyro Dist 1.5 1 1.5 45 0.045 GVSUAluminum Rods 1.39 10 13.9 0.375 0.00375 Riders

Non-Skid 6 1 6 20 0.02Modern 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.0012House of Hobbies

Epoxy 3.49 1 3.49 2 0.002 RidersSolder 24.99 0.05 1.2495 453.6 0.02268 RidersSwitches 3.99 2 3.99 5 0.005 RidersMachining Extras 88 0.088 GVSUTOTALS $139.84 0.8211 Kg

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

2.0 Test Results

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

20

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

21

Meters

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

22

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

(12)

Time to complete the course (s) Total cost ($) Distance traveled (m) Build Quality (0=best, 1=worst) Theory Quality (0=best, 1=worst) Mass of Cart (Kg)

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

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

23

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.

24

Appendix A: Drawings

25

Segway Design: Bill of Materials

Item # Part Description Qty1 Batteries 32 Controller 13 Encoder 24 Encoder Brackets 25 Potentiometer Support 16 Gyro Bracket 1

7 Top 18 Bottom 19 Frame Assembly 110 Gyroscope Assembly 111 Handle Bar Assembly 112 Light Sensors 313 Motors (5V) 214 Exterior 215 Side Cover 216 Small Gear 217 Wheel Assembly 218 Rivets 1419 Grip Tape 12”x12” sheet20 Plumber’s Solder 36”

27

Appendix B: Electrical Schematic

44

Appendix C: Stress Strain Calculations

46

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.

Below is the FBD of the member:

47

Appendix D: C Program

51

//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 resistanceint 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){

e_sum2 += e2 *T;

52

return e_sum2;}

int diffR(int errorR){

int DR;

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

return DR;}

int diffL(int errorL){

int DL;

DL = errorL - VinitL; // Differentiate pot positionVinitL = errorL;

return DL;}

#define CLK_ms 10 // set the updates for every 10ms, same as CLK_ms

/***********************Controller Function********************///int Cd = 0; // Initialize desired positionint controllerL(int Cd, int Cf){

int Ce; // Position errorint 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;}

// Read the pot

53

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 timevolatile unsigned int CLK_ticks = 0; // the current number of msvolatile 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 = 10msTCNT0 = 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************************************/

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

54

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

55

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

}

56

/******************************End of PWM Stuff*******************************/

/*******************************Voltage Output********************************/int v_outputL(int v_adjustedL) /* call from the interrupt loop */{

int RefSignal; // the value to be returnedif(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 */if(v_adjustedR < -255) /* clip output below minimum */

57

{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 oncewhile((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 PA3ADCSRA = 0xC0; // turn on the ADC and set it to read oncewhile((ADCSRA & _BV(ADSC)) != 0x00){}sensor2 = (AD_read());

return sensor2;

58

ADCSRA = 0;

}

int sensor3_func() // reads sensor3 (left sensor){

ADMUX = 0x05; // set the input to channel 1 PA5ADCSRA = 0xC0; // turn on the ADC and set it to read oncewhile((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{

59

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

60

Appendix E: Scilab Program

61

// System component valuesl = 0.4; // 40cm length from the wheel axel to the load on top of the segwayMw = 1.0; // 1kg mass of wheelMc = 0.2; // 200g mass of segway and loadg = 9.81; // gravityrw = 0.0762; // 0.1524m diameter wheelsK = .5; // motor speed constantR = 7; // motor resistance// J = 0.005; // rotot inertiauf = 0.1; // floor friction

// System statex0 = 0; // initial conditions for positionv0 = 0;theta0 = 0.0; // the initial position for the load, cannot divide by zeroomega0 = 0.0;X = [x0, v0, theta0, omega0];

// The controller definitionPI = 3.14159;ppr = 16; // encoder pulses per revolutionKpot = 1.72; // the angle voltage ratioVzero = 2.5; //the voltage when the segway is verticalVadmax = 5; // the A/D voltage rangeVadmin = 0;Cadmax = 255; // the A/D converter output limitsCadmin = 0;tolerance = 0.5; // the tolerance for the system to settleKpp = 20; // position feedback gainKsp = 2; // sway feedback gainVpwmmax = 12; // PWM output limitations in VCpwmmax = 255; //PWM input rangeCdeadpos = 100; // deadband limitsCdeadneg = 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.5VCL = ((VL - Vadmin) / (Vadmax - Vadmin)) * (Cadmax - Cadmin);if CL > Cadmax then CL = Cadmax; end// check for voltages over limitsif CL < Cadmin then CL = Cadmin; endCLc = Ksp * (CL - (Cadmax + Cadmin) / 2);Cc = Cpc + CLc;Cpwm = 0;if Cc > 0.5 then// deadband compensation

Cpwm = Cdeadpos + (Cc/Cpwmmax)*(Cpwmmax - Cdeadpos);

endif Cc <= -0.5 then

Cpwm = -Cdeadneg + (Cc/Cpwmmax)*(Cpwmmax - Cdeadneg);

end

foo = Vpwmmax * (Cpwm / Cpwmmax); //PWM output

62

if foo > Vpwmmax then foo = Vpwmmax; end // clip voltage if too largeif foo < -Vpwmmax then foo = -Vpwmmax; end

endfunction

// The motion profile generatorfunction foo=motion(t_start, t_end, t_now, C_start, C_end)

if t_now < t_start thenfoo = C_start;

elseif t_now > t_end thenfoo = C_end;

elsefoo = C_start + (C_end - C_start)*(t_now -t_start) /

(t_end - t_start);end

endfunction

// define the state matrix functionXd = 10; // the setpoint 10 turns == 160 pulsesCd = 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 integrationsteps = 1000; // The number of steps to uset_start = 0; // the start time - normally use zerot_end = 10; // the end timeh = (t_end -t_start) / steps; // the step sizet = [t_start]; // an array to store time valuesfor i=1:steps,

t = [t ; t($,:) + h];X = [X ; X($,:) + h*derivative(X($,:), t($,:), h)]; // first order

end

// Graph the valuesplot2d(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];

63

// 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)');

64

Appendix F: Gantt Chart

65

segway report

Documents