Final Report - Claymore - Grand Valley State University

Grand Valley State University
Padnos School of Engineering
Target Acquisition and Firing System Design Project
EGR 345 Dynamic Systems Modeling and Control
Instructor: Dr. Jack
Team 9 – “Young Gunners”
Brian Coleman
Steve Johnson
Coty Lindell
Bryan Masselink
Haley Nghiem
December 7, 2005

Executive Summary
The objective of this design project was to develop a device to read a signal,
locate a target, and fire a practice golf ball through the target autonomously in the least
amount of time possible. An ATMega32 microcontroller and L298P H-bridge driver
were used to control the device. Cost, weight, and size constraints were put under
consideration.
A stationary device with a rotating turret and launch mechanism was the optimal
design choice upon consideration of the design parameters. The launch mechanism was a
directed air nozzle contained within a 9 inch by 1.5 I.D. low-pressure PVC pipe. The
positioning of the launch mechanism was controlled by a high-torque stepper motor. The
stepper motor was fastened to an aluminum coupler, which was fastened to the launching
mechanism. The balls were fed to the launching mechanism using a gravity fed, helical
track wire hopper. The ATMega32 microcontroller was utilized to receive 5V inputs
from an external source. When a signal was received, the stepper motor aligned the
barrel with one of four targets and launched a practice golf to disable the target.
Testing was executed to determine how the device would perform in the final
competition. The test results revealed that air provided an adequate force to shoot the
balls at the targets. Furthermore, the stepper motor provided accurate positioning of the
barrel at an acceptable rate.
1

Table of Contents
1. Design Description......................................................................................................... 3
1.1 Design Specifications................................................................................................ 3
1.2 Mechanical Design.................................................................................................... 3
1.2.1 Design Summary ................................................................................................ 3
1.2.2. Mass and Budget of Materials .......................................................................... 4
1.3 Software Design........................................................................................................ 5
1.3.1 System Architecture ........................................................................................... 5
1.3.2 Control System Block Diagram ......................................................................... 6
1.4 Electrical Design....................................................................................................... 7
2. Free Body diagram and Differential Equations .............................................................. 8
2.1 Panning Angle Calculation ....................................................................................... 8
2.1.1 Differential Equations for Stepper Motor.......................................................... 8
2.1.2 Scilab Calculation for Panning Motor Selection.............................................. 9
2.2 Ball Characteristic Calculations.............................................................................. 11
2.2.1 Ball Velocity Before Launch ............................................................................ 11
2.2.2 Projectile Motion Derivation........................................................................... 13
2.2.3 Derivation for Individual Target Distance and Angle..................................... 15
3. Test Results.................................................................................................................. 16
3.1 Scilab Simulation Results ....................................................................................... 16
3.2 Estimated Design Score .......................................................................................... 17
4. Conclusions.................................................................................................................. 17
5. Recommendations......................................................................................................... 18
6. Digital Picture ............................................................................................................... 18
7. Appendices................................................................................................................... 19
7.1 Pro-E Drawings....................................................................................................... 19
7.1.1 Exploded Assembly View with BOM............................................................... 19
7.1.2 Orthographic Prints......................................................................................... 21
7.1.3 Pro-E drawings................................................................................................ 22
7.2 Gantt Chart.............................................................................................................. 30
7.3 C Code .................................................................................................................... 31
7.4 Scilab Code ............................................................................................................. 38
2

1. Design Description
1.1 Design Specifications
The target acquisition and firing system device must contain all electrical
and mechanical components except for the supplied power sources. The largest
dimension of the design must be less than 12 inches, but smaller designs will
receive more points. The device must have a mass less than 3 kg excluding any
external power sources. The device may be powered by an external AC/DC
voltage device or a pneumatic line or both. The equipment costs should be
minimized, with a goal of $200 or less. The device must shoot practice golf balls
approximately 1.5 inches in diameter at randomly activated targets 6 feet away.
An electrical line will provide a 5 volt signal to the ATMega32 microcontroller.
A different signal is provided for each of the four targets. The overall score of the
design will depend on the number of targets hit in 2 minutes, total cost, largest
dimension, build quality, theory quality, and mass of the device.
1.2 Mechanical Design
1.2.1 Design Summary
The assembled drawing, shown in Figure 1, displays the major
components of the target acquisition and firing device. The base of the machine
was made of black polycarbonate, which was lightweight with high machinability.
Four threaded rod supports were placed in the four corners of the base plate. The
pan angle was controlled by a stepper motor. The PVC pipe firing mechanism was
attached to the top of the stepper motor via a coupler and mounting bracket, and
was approximately nine inches long by 1.5 inches I.D.. Since the stepper motor
was mounted on the bottom of the base, the ATMega32 and the controller board
were attached to the back of the base with two bolts for easy connections. A steel,
sheet metal bracket, attached to the back of the barrel, was used to mount the air
nozzle used to fire the balls. The steel bracket was designed to the increase
airflow into the barrel. Thus, providing a larger force to the ball. When air
3

pressure was applied, the flap rotated approximately 90º, preventing multiple balls
from entering the barrel.
Figure 1: Assembled view of the fire mechanism
Appendix 7.1.1 shows the assembly exploded view of the mechanism,
appendix 7.1.2 includes the orthographic prints of the machined part and appendix
7.1.3 shows the pro-E drawings for all components used in the production of the
device.
1.2.2. Mass and Budget of Materials
The total budget of the device was not exceed $200. A budget inventory
was used to monitor the overall budget of the device. The mass of the device was
a factor in the overall design score. The total mass of the device excluding
external power sources must not exceed 3kg. The mass and budget of materials,
shown in Table 1, were updated on a regular basis. Table 1 includes the unit price
and mass for each item and where each item was purchased. The total money
4

spent was $97.96 and the total mass of the machine was 995 grams. All receipts
for purchased components are shown in Appendix 7.6.
Table 1: Mass and Budget Inventory
Quantity Unit
Part Total
Description
used Price Source of Unit Price Mass(g) Mass(g)
17Y series High Torque step
Anaheim automation
motor 1 $22.00 receipt 364 364
Machined Polycarbonate plate 1 $1.97 Mcmaster/carr pg.3314 55 55
(1) bottle Super Glue 1 $3.65 GVSU Book Store 0.01 0.01
(2) packs of wiffle balls 2 $4.95 Target 0 0
assorted hardware 1 $10.62 Godwin's hardware 20 46.49
(1) ATMega circuit board 1 $30.00 GVSU 35 35
2 feet, 1.5' diameter PVC pipe 1 $1.58 Menards 50 50
Surface savers 1 $1.49 Godwin's hardware 5 20
(8) U Joint 8 $0.29 Godwin's hardware 5 40
(4) 1' Threaded rod 4 $1.29 Godwin's hardware 30 120
1 ", 3/4" diameter aluminum rod 1 $0.59 Mcmaster/carr pp.3368 30 30
1" Aluminum C channel 1 $0.52 Mcmaster/carr pp.3367 15 15
1/2, 1 cubic foot sheet metal 1 $1.30 Mcmaster/carr pp.3379 10 10
Motor controlled board with wires 1 $15.00 GVSU 85 85
Air fitting 1 $1.47 Mcmaster/carr pp.238 20 20
Hopper wire (11') 1 $1.89 Godwin's hardware 9.5 104.5
Total Machine mass(g): 985
Total Machine cost ($): $98.60
1.3 Software Design
1.3.1 System Architecture
The system architecture of the device is shown in Figure 2. An
ATMega32 microcontroller combined with an L298P H-bridge driver was used to
control the air pulse and panning motor. The C program was written to control
the ATMega32, and L298P is located in Appendix 7.3. The stepper motor was
used to rotate the launching mechanism.
5

ATMega32
1 2 3 4
Target Lines
PWM
A0 A1 A2 A3
Figure 2: System Architecture
1.3.2 Control System Block Diagram
The control system block diagram was developed to control the target
acquisition and firing device as shown in Figure 3. When a target was activated,
the controller input port received a digital-high signal. When a target (1, 2, 3 or
4) was activated, a signal was sent to an input pin A0, A1, A2, or A3,
respectively. The C program, shown in Appendix 7.3, commanded the stepper
motor to turn to the desired target and deliver an air pulse to fire the ball. The
software utilized an interrupt sequence to monitor the status of each target. The
device launched balls until the target was deactivated. The sequence continues
upon activation of the next target.
Ci
Air Valve
H-bridge Vs Stepper
Motor
Pan
Angle
Air Valve
θp
Launch
Mechanism
Figure 3: System block diagram
6
w
Launch
Mechanism

Where,
Ci = Target input signal
θ = Panning Angle
p
1.4 Electrical Design
The electrical schematic of the device is shown in Figure 4. The electrical
schematic includes the ATMega32 microcontroller, a L298P H-bridge push-pull
four channel
driver chip, and a 4 volt power supply. Four 1 KΩ resistors were
used to reduce noise from the stepper motor.
Figure 4: Electrical Schematic
7

2. Free Body diagram and Differential Equations
The calculations shown in subsequent sections was used to determine the required
launch angle torque and launch speed needed to fire the ball the required distance. This
calculation was helpful in the selection process of motors.
2.1 Panning Angle Calculation
2.1.1 Differential Equations for Stepper Motor
τ Motor
N
FNR θ
Figure 5: FBD for the Turn Table
where, N= normal force (The hopper weight)
W(hopper) = weight of the hopper and its configurations
* *
J θ = mass moment of inertia
The differential equation for the launching angle from Figure 2.1 is
* *
∑ M : τ Motor − FNR = J θ .
*
Put (2.1) into state equation, let θ = ω
ω *
= τ
Motor −
J
W(hopper)
FNR
8
* *
J θ
(2.1)
, (2.2)

where, ω = angular velocity of the launcher motor, τ = torque motor give, J =
mass moment. The state equations above allow us to find the angular velocity the
motor gives. Once angular velocity is found, time is found by
dθ
ω = . (2.3)
dt
Integrate (2.3), and solve for t = time
t = θ / ω
(2.4)
2.1.2
Scilab Calculation for Panning Motor Selection
Figure 2.2: The torque and inertia in a basic motor model
The first-order differential equation can developed from motor properties using
some basic measurements.
Tm = KI
(2.6)
Tm
I = ( 2 .7)
K
where, T = motor torque, K= constant, and I = current. Then consider the
m
power in motor,
P m +
= V I = Tω
KIω
(2.8)
V m = Kω
(2.9)
V m
where, = voltage supply to motor, and P = power. The dynamics of the
rotating masses can be found by summing moments,
9

∑ ⎟ ⎞ ⎛ d
M = Tm
− TF
= J⎜
ω
(2.10)
⎝ dt ⎠
⎛ d ⎞
T m = J⎜
⎟ω
+ TF
(2.11)
⎝ dt ⎠
where, J= motor rotor, T = frictional torque, and w= motor speed. The current-
F
voltage relationship for the left hand side of the equation can be written and
manipulated
to relate voltage and angular velocity.
V Vm
I = (2.12)
R
s −
Tm Vs
− Kω
=
K R
⎛
d
dt
⎞
⎟ω
+ T
⎠
K
J⎜ F
⎝
(2.13)
Vs
− Kω
= (2.14)
R
2
⎛ d ⎞ ⎛ K ⎞ ⎛ K ⎞ TF
⎜ ⎟ω
+ ω = Vs
⎜ ⎟ +
dt ⎜
JR ⎟
⎝ ⎠ ⎝ ⎠ ⎝ JR ⎠ J
From section 2.1,
T F = F*N*R. The coefficients for the differential equation
(2.15) can be found for the motor in a dynamic case using steady state
velocities. At steady state (2.15) becomes
2 ⎛ K ⎞ ⎛ K
ω ⎜
⎟ = Vs
⎜
⎝ JR ⎠ ⎝ JR
K
V
ω
= .
s
⎞
⎟
⎠
Then J can be found because
K 2
(2.15)
(2.16)
(2.17)
τ =
(2.18)
JR
10

2
K
J =
τR
(2.19)
where, τ =time constant.
Time constant can be found by using 63% of steady
value from voltage vs time curve. The static torque value can be found using the
deadband limits,
ω = 0;
T < T
F
T ≠ ; T < 0 ω
2.2 Ball Characteristic Calculations
F
S
K
2.2.1 Ball
Velocity Before Launch
Figure 6 shows the pathway of the ball in hopper
feeder (slider), the arrow
indicates the direction of the ball.
Figure 6: Feeder pathway
To calculate the velocity from the top ( V t ) of feeder to the bottom ( Vb
), principle
of work and energy
is used,
w
here, ( t b)
Start
W +
( t→b ) = W(
t→b
) conservative
W(
t→b
) non−conservative
, (2.20)
W → = work from top to bottom of feeder . Conservative work includes
all work and energy that are path independent,
Finish
W = V + T,
(2.21)
11

where, V= potential energy, and T= kinetic energy. Work due to gravity (potential
energy) occurs when the ball is dropped through a feeder. Work due to gravity is
defined as
Vg = mgh , (2.22)
where V = work due to gravity, m = mass of an object, g = gravity, h= distance
from which the object is dropped, Figure 2.3. The ball is moving with kinetic
energy,
g
1 2
mv
T = , ( 2.23)
2
where, m = mass of an object, and v = object velocity. Non-conservative work is
work due to friction,
W = Fds , (2.24)
friction
sb
where W friction = non conservative work due to friction,
F= friction force as a
function of time, ds = change in distance the ball is dropped, S t = initial position
(2.24), then substitute them in (2.20),
∫
st
of the ball, and Sb
=final position of the ball. Combine (2.21), (2.22), (2.23) and
1
2
mv
2
t
s
b
1 2
+ mght
+ ∫ Fds = mvb
+ mghb
, (2.25)
2
st
where, vt
= initial velocity at top of feeder, v b = velocity of ball at the bottom of
feeder, h = the height at the top of feeder, and h = the height at the bottom of
t b
feeder. Figure
2.4 shows the free body diagram of the ball from feeder. The ball
starts at rest, with the height of h, as shown in Figure 2.3.
12

Figure 7: Free body diagram of the ball
Equation (2.25) is used to calculate the speed of the ball at the bottom of feeder
Based on the free body diagram, Figure 7,
mgy
t
sb
1
+ ∫ FNds =
2
st
mv
where, F = force of friction, and N = normal force of ball.
2.2.2 Projectile Motion Derivation
2
b
+ mgy , (2.26)
Figure 8: Projectile motion from launch position to targets.
Start out with the definition of a vector r in space,
r ( t)
= x(
t)
i + y(
t)
j , (2.27)
whe re r (t)
= vector in space, x ( t)
i = position of time in x direction, and y ( t)
j =
position of time in y direction. The acceleration in x direction is 0 ( a = 0 ),
therefore, the function of x direction with respect
to time is defined,
13
b
x

x i 0x
( t)
= x + V t , ( 2.28)
where, x(t)= position in x direction with respect to time, x = initial position,
V0x = initial velocity of the ball in x direction, and t = time. Whereas, acceleration
for the y direction is the gravity (
with respect to time is defined as,
1
2
a y
i
= −g
), therefore, the function of y direction
2
y( t)
= yi
+ V0
yt
− gt , (2.29)
where, y(t) = position in y direction with respect to time, = initial position in y
V 0 = initial velocity in the y direction, g = gravity, and t =
direction, y
time. From
Figure 8, assumed initial position is 0, ( x = y = 0 ), and
0
0
V x = V cosθ
, (2.30)
0
0
V = V sinθ
, (2.31)
0 y
where, V 0 = initial velocity, and
0
(2.30), (2.31) into (2.28) and (2.29) respectively to get
and
Solve (2.32) for t,
θ = the angle of launcher. Substitute value
into
x( t)
= V0
cosθ
( t)
, (2.32)
1
y( t)
− gt
2
2
= V0
sinθ
( t)
(2.33)
x
t = (2.34)
V cosθ
0
then substitute (2.34) back to (2.33),
14
y i

ut
2
x 1 ⎛ x ⎞
y ( x)
= V sinθ
− ⎜ ⎟
0
g
⎜ ⎟
(2.35)
2 2
V0
cosθ 2 ⎝V0
cos θ ⎠
cos
1 2
2
θ
Combine (2.35) and (2.36)
to get
(
= 1 + tan ( θ )
(2.36)
)
2 ( 1 tan θ )
2
gx
y ( x)
= x tanθ
− +
(2.37)
2
2V
If y(x) is z, then solve for V0
from (2.37);
V
0
=
0
2 ( 1+
tan ( ) )
2
gx θ
(2.38)
2(
x tan( θ ) − z
Equation (2.38) is used to find the desired initi al velocity base d on position
of
each target.
2.2.3 Derivation for Individual Target Distance and Angle
Figure 9: Launcher Position.
The triangle above has an angle ofγ . The
distance from the launcher to the
midpoint of the target is the adjacent
side of the triangle, “a”. The distance
between the midpoint to a specific target is the opposite side of the triangle, “o”.
From trigonometry, hypotenuse (“h” or the distance from launcher to a specific
target) of the triangle can be found by using Pythagorean theorems,
15

2 2 2
h = a + o , ( 2.39)
where, h= hypotenuse, a=adjacent, and o=opposite. Solve for h from (2.39),
2 2
h = a + o . (2.40)
The angle can be found be using the tangent equation,
o
tan γ = .
a
Then solve for γ from (2.41),
3. Test Results
(2.41)
−1⎛
o ⎞
γ = tan ⎜ ⎟ . (2.42)
⎝ a ⎠
3.1 Scilab Simulation Results
A Scilab program was used to simulate the motion of the panning motor.
The Scilab results are shown below in Figure 3.1. Figure 3.1 shows the position
and angular speed of the panning motor with respect to time. Scilab program is
shown in Appendix 7.4.
Figure 10: Scilab Simulation Results
16

3.2 Estimated Design Score
The design
score depends on the cost, mass, build quality, technical
quality, and number
of targets hit. The estimated contest score is shown in Table
2. The following equation was
used to asses the overall score for the design.
Score = H
Where,
−2
4)
H = targets hit in 2 minutes
C = total cost of
( 10)
parts
L = largest dimensions
(
c
80
−B
( 10)
(inches)
B = build quality (1 = best, 0 = worst)
T = Technical quality (1 = best, 0 = worst)
M = mass of machine (Kg)
−T
( 2)
L
( )
3
M
Table 2: Estimated contest score
H 143
C 98.6
L 12
B 1
T 1
M 0.985
Estimated Score
4
0.0000409205101
4. Conclusions
The machine that was designed and built for the purpose of target acquisition and
elimination,
performed as expected. When a target became active the stepper quickly
moved the barrel into position and a ball was fired at the target. Through the design,
build,
and implementation of this machine, certain conclusions were arrived upon.
17

1. The flap in front of the nozzle was sufficient at providing the ball with an
initial acceleration and holding the next ball back.
2. Mass was the most important factor in this project. By using light-weight
components for the base and hopper, the mass of the device was under a
kilogram causing a drastic effect on the score.
3. Launch velocity increased as the airflow through the barrel increased.
5. Recommendations
After the performance the device was analyzed through testing and competition.
The following are recommendations to improve the function of the mechanism:
1. The inside diameter of the barrel should be decreased to improve the
accuracy of the firing mechanism.
2. A method
to ensure that the stepper motor is consistently starting at the
3. Eliminate all loose connections by soldering wires directly onto the L298P
6. Digital
Picture
same position should be implemented.
motor driver.
Figure 11: Digital Photo of final Machine
18

7. Appendices
7.1 Pro-E Drawings
7.1.1 Exploded Assembly View with BOM
19

7.1.2 Orthographic Prints
21

7.1.3 Pro-E drawings
Figure 12: Machined base
Figure 14: Stepper motor
22

Figure 13: Barrel
Figure 15: Air nozzle bracket
23

Figure 16: Air fitting
Figure 17: Barrel bracket
24

Figure 18: Ball stopper flap
Figure 19: Ball stopper flap hinge
25

Figure 20: L_bracket (connect the barrel and ball stopper)
Figure 21: Stepper motor shaft
26

Figure 22: Rubber foot
Figure 23: Helical hopper (one of two wires)
27

Figure 24: Ball stopper
Figure 25: Supporting rod
28

Figure 26: Hopper U-joint
Figure 27: Hopper bar
Figure 28: Hopper slider
29

7.2 Gantt Chart
30

7.3 C Code
//Author: Brian Coleman
//Class: EGR 345
//Date: 11/29/05
//Include files
#include
#include
#include
#include "sio.c"
//Function prototypes
void CLK_setup(void);
void IO_setup(void);
void IO_update(void);
void move_step(int dir, int step_delay);
void write_step(int step);
int check_targets(void);
void delay(int ticks);
void stop(void);
//Global Constants
#define UP 1
#define DOWN 0
#define HIGH 1
#define LOW 0
#define Start 0 //Initial target
#define TARGET_1 -9 //Left-most target
#define TARGET_2 -4 //Inner-left target
#define TARGET_3 1 //Inner-right target
#define TARGET_4 6 //Right-most target
# define NUM_INTERRUPTS 10 //Number of interrupts per sec
# define DELAY_TIME 25 //1 is the maximum speed
#define FIRE_DELAY 200 //Air release time
//Variables
int
steps[] = {1, 2, 4, 8};
int
position = 0;
int last_target = 0;
31

int current_target = 0;
int destination = 0;
int current_step =
int i;
-1;
unsigned int CNT_timer1;
//The delay time
volatile unsigned int CLK_ticks
= 0; //The current number of ms since
last increment of CLK_seconds
volatile unsigned
since
start
int CLK_seconds = 0; //The current number of seconds
/*-------------------------------
Interrupt Stuff ---------------------
----------*/
SIGNAL(SIG_OVERFLOW1) / /The interrupt calls this function
{
CLK_ticks += (NUM_INTERRUPTS*10);
if(CLK_ticks >= 1000)
{
beginning
}
}
CLK_ticks -= 1000;
CLK_seconds++;
//Counts the number of seconds from the
IO_update();
TCNT1 = CNT_timer1;
void CLK_setup(void) //Starts the interrupt service
routine
{
TCCR1A = (0

}
CNT_timer1 = 0xFFFF - 80;
TCNT1 = CNT_timer1;
//Start at the right point
TIFR
&= ~(1

}
destination = current_target - position;
if(destination > 0)
{
}
{
}
{
}
for(i = 0; i < destination; i++)
delay(250);
delay(250);
move_step(UP,
DELAY_TIME);
PORTB |= 0x20; //Set PB5
delay(FIRE_DELAY);
PORTB &= ~0x20; //Clear PB5
delay(1500);
else if (destination < 0)
else
for(i = 0; i > destination; i--)
move_step(DOWN, DELAY_TIME);
PORTB |= 0x20; //Set PB5
delay(FIRE_DELAY);
PORTB &= ~0x20; //Clear PB5
delay(1500);
PORTB |= 0x20;
//Set PB5
delay(FIRE_DELAY);
PORTB &= ~0x20; //Clear PB5
delay(1500);
34

}
}
/*-------------------------------
Stepper Control ---------------------
-- ---* /
void move_step(int dir, int step_delay)
//step_delay determines the
rotational speed
{
}
if(dir == UP)
{
}
else
{
}
current_step++;
position++;
current_step--;
position--;
if(current_step > 3) current_step = 0;
else if(current_step
< 0) current_step = 3;
write_step(steps[current_step]);
delay(step_delay);
/*---------------------------------------------------------------------
-*/
void write_step(int step)
{
}
PORTB = step;
/*--------------------------------------------------------------------
-*/
int check_targets(void)
35

{
}
int target;
if((PINA & 0x01) !=0) //PA0
{
}
target = TARGET_1;
else if((PINA & 0x02) !=0) //PA1
{
}
else if
{
}
target = TARGET_2;
((PINA & 0x04) !=0)//PA2
else if ((PINA
& 0x08) !=0) //PA3
else target
= Start;
/* ---------------------------------------------------------------------
-*/
{
}
void
delay(int ticks) //ticks are approximately 1ms
{
}
{
}
target = TARGET_3;
target = TARGET_4;
return target;
volatile int i, j;
for(i = 0; i < ticks;
i++)
for(j = 0;
j < 465; j++);
/*-------------------------------
Main -------------------------------
*/
36

int main(void)
{
}
sio_cleanup();
sio_init();
IO_setup();
CLK_setup();
PORTD = 0x30; //Turn EA and EB to
for(;;);
return 0;
37
high

7.4 S cilab Code
//system component values
K=0.05 // motor constant
R= 20; //motor resistance
J= 1.35*10^10/25.4; //given from
pro-e
F= 407.437; //lbf
.25"diameter hose with 20 psia pressure
N= 100;
//sytem state
theta0= 0; //the initial position
for the motor (rad)
omega0=0;
//
X=[theta0, omega0];
moving=0;//th system is not in motion
//the controller deinition
Cd= 20; //setpoint
Kpot= 1.5; //the angle voltage ratio, slope of angle vs. voltage plot
Vzero= 0; //voltage when the penduleum is vertical
Vadmax= 5; //the A/D voltage range
Vadmin = 0;
Cadmax= 255; //the A/D converter output limits
Cadmin= 0;
Kp= 5;
Vpwmmax= 12; //PWM output limitaitons in V
Cpwmmax= 255; //PWM input range
Cdeadpos_static= 10; //deadband limits for static torque
Cdeadneg_static= 20;
Cdeadpos_dynamic= 10; //deadband limits for kinetic torque
Cdeadneg_dynamic= 20;
function foo=control(state,t)
38

VL= Kpot*state($,1)-Vzero; //estimate input voltage for pot.
Cp=(VL-Vadmin)/(Vadmax-Vadmin)*(Cadmax-Cadmin); //estimat A/D input
if Cp> Cadmax then Cp = Cadmax;
end //c heck for voltages over limits
if Cp< Cadmin then Cp=Cadmin;
end
Ce=Cd-Cp;
Cu=Kp* Ce;
if abs(state($ ,2))>0.01 then
Cdeadpos= Cdeadpos_dynamic;
Cdeadneg= Cdeadneg_dynamic;
else
Cdeadpos=Cdeadpos_static;
Cdeadneg=Cdeadneg_static;
end
Cpwm=0;
if Cu>0.99 then //deadnamd cp,[emsatopm
Cpwm =Cdeadpos + (Cu/Cpwmmax)*(Cpwmmax-Cdeadpos);
end
if Cu Vpwmmax then foo = Vpwmmax;
end //clip voltage if too
large
if foo < -Vpwmmax then foo = -Vpwmmax;
end
endfunction
39

define the state matrix function for a simple motor
function foo =derivative(state,t)
Vs= control(state,t);
foo= [state($,2), state($,2)*(-(K*K)/(J*R))+Vs*K/(J*R)+(F*N*R)/J];
endfunction
//Set
the time length and step size for the integration
steps= 100;
t_start=0;
t_end=2;
h=(t_end-t_start)/
steps;
t=[t_start];
//Loop for integration
for i=1:steps,
t=[t; t($,:)+h] ;
control(X($,:), t($,:));
X=[X; X($,:) + h*derivative(X($,:), t($,:))]; //first order
end
//print out the solution
intervals
=200;
for time_count =1:intervals,
i=int((time_count -1)/intervals *steps +1);
printf("%f\n", X(i,2));
end
// graph the values
plot2d(t,X,style = [-2,-5],leg="speed(rad/s@angle(rad))");
xtitle('Time(s)');
40