trajectory planning for the halobject - tu/e · trajectory planning for the halobject nagelmaeker,...

Trajectory Planning for theHalobject

P.M. Nagelmaeker

DCT 2005.105

Traineeship report

Coach(es): dr. ir. M.J.G. v.d. Molengraftdr. ir. G.W.M. Peters

Supervisor: prof. dr. ir. M. Steinbuch

Technische Universiteit EindhovenDepartment Mechanical EngineeringDynamics and Control Technology Group

Eindhoven, July, 2005

Contents

1 Introduction 4

2 Introduction to Motion Controllers 5

2.1 Globally . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52.2 Trajectory planning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6

2.2.1 Second order trajectory planning . . . . . . . . . . . . . . . . . . . . . . . . . . 62.2.2 Feedforward design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72.2.3 Third order trajectory planning . . . . . . . . . . . . . . . . . . . . . . . . . . 8

3 Trajectory Planning for the Walking Mechanism 10

3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103.2 Trajectory planning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

3.2.1 Second order trajectory planning . . . . . . . . . . . . . . . . . . . . . . . . . . 143.2.2 Third order trajectory planning . . . . . . . . . . . . . . . . . . . . . . . . . . . 143.2.3 Trajectory output turning motor . . . . . . . . . . . . . . . . . . . . . . . . . . 163.2.4 Trajectory output cantilever motor . . . . . . . . . . . . . . . . . . . . . . . . . 173.2.5 Examples from simulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19

4 Trajectory Planning for the Feelers Mechanism 20

4.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 204.2 Trajectory planning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20

4.2.1 Second order trajectory planning . . . . . . . . . . . . . . . . . . . . . . . . . . 234.2.2 Trajectory output . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23

5 Conclusion and recommendations 29

Bibliography 30

A States of the trajectory planning feelers 31

A.1 State: Scan Mode and State: Stop Scan Mode A . . . . . . . . . . . . . . . . . . . . . . 31A.2 State: Stop Scan Mode B . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33A.3 State: Start Pointing Mode B and State: Pointing Mode and State: Start Pointing Mode C 34A.4 State: Stop Pointing Mode . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35A.5 State: Start Pointing Mode B . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36

2

B C-source 37

B.1 Walking mechanism . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37B.1.1 TP2.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37B.1.2 TP3_C.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39B.1.3 TO2.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41B.1.4 TO3_C.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44

B.2 Feeler mechanism . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50B.2.1 FTP2.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50B.2.2 FTO2.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51

3

Chapter 1

Introduction

The VARA/NPS broadcasting associations challenged a group of artists to revive one of the halls of theNET3 building in Hilversum. An artist came up with the idea to create a moving sculpture, for nowcalled the Halobject. He turned to the University of Technology at Eindhoven (TU/e) with the requestto develop the underlying technical infrastructure. The TU/e accepted this and the Halobject becameone of the design projects for students to work on. Student groups studied on walking mechanisms,sensors, feelers and character for a year. Then a small group started the first realization phase of the’creature’. The complete framework, electrical system and operator panel were created. Furthermorethe basic control software was developed. Currently the second phase is in progress. This phase fo-cusses on creation of the needed software to let the Halobject walk autonomously in a space withoutbumping into stationary or moving objects. The final goal is a walking robot which interacts with itsenvironment via sensors (light, sound, distance).

This report will discussed the trajectory planning developed for the Halobject. The trajectories areused as reference path for the electric motor to follow.

4

Chapter 2

Introduction to Motion Controllers

2.1 GloballyThis chapter considers motion controllers and especially trajectory planning in a globally form. Itsmeant to give the reader a better understanding in the structure of a motion controller and to giveinsight in how trajectory planning works. Furthermore a simple design of a feedforward controllerwill be discussed. A combination of trajectory planning, feedback and feedforward control are oftentogether constituted as motion controller.

The global structure of a motion controller is graphicly depicted in figure 2.1.

Figure 2.1: General layout of motion controller

Feedforward control is a well known technique for high performance motion control problems. Itis, for instance, widely applied in robots.An example of a motion task can be to perform a motion from a position A to a position B, starting attime t.This task has to be performed by the motion controller which has several tasks:

• Trajectory planningThe calculation of an allowable trajectory with respect to given constraints for each actuationdevice; for example a trajectory an electric motor has to follow, maybe better known as referencesignal r;

• Feedforward controlThe calculation of a feedforward control signal on grounds of the calculated trajectory with theintention to obtain better tracking performance;

5

• Feedback control The processing of available measurements and calculation of a feedback con-trol signal for each actuation device to compensate for unknown disturbances and unmodelledbehavior.

In order to keep the design simple, these motion controller tasks are done for each actuator deviceseparately. In this case, each actuating device is considered to be acting on a single mass moving alonga single degree of freedom. The feedforward control problem is then to generate the force requiredto perform the acceleration of the mass according with the desired trajectory. However, the desiredtrajectory should be such that the required force is allowable from a mechanical point of view, and thatno actuator saturation occurs.The remaining part of this chapter will describe two different ways of trajectory calculation, namelythe second and a third order trajectory planning. More detailed information about trajectory planningcan be found in [1]

2.2 Trajectory planningConsider the simple motion system as in figure 2.2, denoting m the mass, d the damping and k thestiffness of the system.

Figure 2.2: Simple mass-damper-spring system

The equation of motion is as follows:

mx + dx + kx = F (2.1)

2.2.1 Second order trajectory planningWith a given bound on acceleration a, i.e. a bound on actuator force F , we want a transition fromcurrent position A to desired position B over distance x. The shortest time within the motion can beperformed is calculated as:

x = 2× 12at2 ⇒ ta =

√x

a⇒ tx = 2ta (2.2)

with ta denoting the constant acceleration time, tx denoting the total trajectory executing time. At thispoint, the trajectory consist of a constant acceleration phase (a), followed by a constant decelerationphase (−a).

6

In case of a given bound on the velocity, denoted as v, violation of v has to be checked by calculat-ing the maximal reached velocity.

v = ata (2.3)If v is not violated:

tx = 2ta (2.4)as in eq. 2.2.

If v is violated (v ≤ v):

ta =v

a⇒ xa = 2× 1

2at2a (2.5)

In order to reach the desired end position (x < xa) a constant velocity phase with duration tv has tobe a added:

tv =x− xa

v(2.6)

Now for the transition from A to B can be said:

tx = 2ta + tv (2.7)x = at2a + vtv (2.8)

When v is not violated, tv in equation 2.8 turns zero.

Resuming, with given design constraint (a and v), the second order trajectory algorithm calculatesan acceleration profile which has to be integrated twice to get the desired trajectory profile of theposition. See figure 2.3.

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9

−5

0

5

Second order trajectory profiles

a [r

ad/s

2 ]

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9

0

0.5

1

1.5

2

v [r

ad/s

]

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9

0

0.5

1

time [s]

x [r

ad]

Figure 2.3: Second order trajectory pro�les

2.2.2 Feedforward designBased on the calculated profiles a feedforward controller can easily be designed. As stated before thefeedforward force F is calculated as in equation 2.1, Fff = ma + dv + kx, and represents the forcewhich is needed to perform the desired trajectory. A typical feedforward design with PD-controllerscheme can be seen in figure 2.4.

7

Figure 2.4: Rigid-body feedforward

2.2.3 Third order trajectory planningCompared to the second order trajectory profiles (step in acceleration profile), third order profiles (stepin jerk [rad/s3]) have the advantage of being more smoothed. This implies a lower energy content athigher frequencies, which results in a lower high frequency content of the error signal and a decreaseof the positioning error level. Furthermore this reduces the chance of demanding a motion which isphysically impossible to perform by the given motion system (step in force not possible due to poweramp limitation such as time rise).

Now the third order trajectory planning will be discussed. We will assume that a trajectory mustbe planned over a desired distance x with respect to the given bounds on the jerk (j), the acceleration(a) and the velocity (v). The symmetrical trajectory is completely determined by three time phases: theconstant jerk phase (tj), the constant acceleration phase (ta) and the constant velocity phase (tv).

In the first place the constant jerk time is calculated, with no violation on a and v and assuming asymmetrical jerk profile:

x = 2jt3j ⇒ tj = 3

√x

2jand tx = 4tj (2.9)

The maximum reached velocity will be:v = jt2j (2.10)

If (v > v) the bound is violated and tj must be calculated as follows:

tj =√

v

j(2.11)

The maximum reached acceleration will be:

a = jtj (2.12)

If (a > a) the bound is violated and tj must be recalculated:

tj =a

j(2.13)

Now we have the maximal value for tj with no bounds violated. The next step in the algorithm is tocalculate ta. Assuming ta > 0 and v is not violated we can say:

x = 2jt3j + 3jt2j ta + jtjt2a ⇒ (2.14)

ta = −32tj +

12

√t2j

+4x

jtj(2.15)

8

The maximal reached velocity:v = jt2j + jtjta (2.16)

If (v > v) the bound is violated and tv must be recalculated as follows:

ta =v − jt2

j

jtj=

v

jtj− tj =

v

a− tj (2.17)

Now we have calculated tj and ta to be the time-optimal solution under restriction of the given bounds.To reache the desired x we finally have to add a constant velocity phase. We can calculate tv :

tv =x− 2jt3

j− 3jt2

jta − jtjt

2a

v(2.18)

and the total displacement x as function of the times tj ,ta and tv :

x = j(2t3j + 3jt2j ta + tjt2a + tjtatv) (2.19)

0.2 0.4 0.6 0.8 1

Third order trajectory profiles

t2

t3

t4

t5

t6

t7

0.2 0.4 0.6 0.8 1

0.2 0.4 0.6 0.8 1

0.2 0.4 0.6 0.8 1time [s]

Figure 2.5: Third order trajectory pro�les

Integrating the calculated jerk profile three times will give the trajectory profile from point 0 to thedesired end position. See figure 2.5.

9

Chapter 3

Trajectory Planning for the WalkingMechanism

3.1 IntroductionThe walking mechanism of the Halobject [2] is a so called hexapod containing six legs which can bedivided in three groups:

• The left legs.These two legs (left side, front and rear) always move synchronously and are driven with the leftelectric turning motor (70 Watt) and can only move for- and backwards.

• The right legs.Same as the left legs, only right.

• The cantilever leg (middle).The cantilever leg is driven by a 250 Watt electric motor. Turning this motor will result in liftingone side of the leg so the whole frame is lifted to one side. In lifted situation the Halobject isalways resting on three points, the left or right legs (front and rear) and the opposite cantileverleg (middle).

Figure 3.1: Frame construction of the walking mechanism

10

In figure 3.1 the walker mechanism is showed. The left-front and left-rear legs are interconnectedby a driving-belt. A cogwheel which is mounted on the beam is driven by the electric motor. By turningthis motor, the whole beam will turn around its middle axis and the left legs will make a forward orbackward translation. The same principle is applied for the right legs.

(a) (b) (c)

Figure 3.2: Straight forward walking

In figure 3.2 the principle of the walking mechanism is showed.

• (a) The walking mechanism is in homing position. This position is determined by a homingprocedure performed at each startup of the Halobject. All the six legs have contact with theground. By tilting the left side of the Halobject with the cantilever and moving the left legsforward (which lost contact with the ground) the Halobject will start move forward.

• (b) The cantilever is tilted max and the left legs are still moving forward. The right legs have stillcontact with the floor.

• (c) The cantilever is again in its neutral position and all six legs have contact with the floor again.The Halobject has made a half step forwards. By repeating this for the right legs (not depictedhere) the Halobject will move a next half step forward and return again in its homing position(a).

In the remaining part of the report those two steps (left and right, from homing position to homingposition) will be called a walking cyclus. By decreasing the forward stroke of one side, turning ispossible. When for example the forward stroke for the left turning motor is smaller than at the rightside, the Halobject will turn left.More detailed information about the walking mechanism can be found in [2].

3.2 Trajectory planningBasically, the trajectory planning software for the walking mechanism has to translate the demands ofthe character into the right reference paths for the three electric motors so a desired walk behavior isestablished. The software in question is written in C. This c-file is integrated in MATLAB SimuLink asan S-function. S-functions (system-functions) are a powerful mechanism for extending the capabilitiesof Simulink. This S-function has the following in- and outputs (see figure 4.3):

• Inputs

1. Start [1/0]If this input turns true, the Halobject starts walking. The walking move has always to startfrom its homing position, obtained from the earlier stated homing procedure. If start turnszero again the movement has to stop in its homing position. So the Halobject completes

11

first the walking cyclus before it will stops. If start is true the software uses input 2...7 tocalculate the three trajectories for each individual motor;

2. Step [rad]The variable step determines the stroke of the forward movement of the right and left turn-ing legs. Due to geometric restriction this variable is limited to 0.7 radians;

3. Step height [rad]The variable step height prescribes the maximum tilting height of the cantilever and has tobe expressed in radians. Due to geometric restriction this value is limited to 2.5 radians;

4. alphahal [deg]alphahal is the desired turning angle of the Halobject and represents the rotation of theHalobject after one walking cyclus and is expressed in degrees. In case of an unequalforward movement of both turning legs, the Halobject will turn. The value for the forwardstroke of the outer-leg is equal to input 2, step. The value for the forward stroke of the inner-leg (which has to be smaller) is calculated using the geometry of the walking mechanismand the value for step. First the desired difference in stroke is calculated:

4S = w tan(alphahal) (3.1)

with w the width of the walking mechanism. Now the desired step for the inner-leg can becalculated:

stepsmall = 2arcsin(sin(step

2)−4S) (3.2)

The variable alphahal is limited to -10 and +10 degrees to avoid contact of the right and leftturning leg beams in case the difference between step and stepsmall gets to large. At leasta positive value for alphahal will result in a turn right.

5. Bound on acceleration [rad/s2]Bound on acceleration is used for second order trajectory planning for the turning motorleft and right;

6. Bound on velocity [rad/s]Bound on velocity is used for second order trajectory planning for the turning motor leftand right;

7. trest [sec]To make slow walking behavior possible, a rest period can be added which is insertedbetween the calculated trajectories. This will be discussed in detail in the following subsec-tion.

8. Backward [1/0]If backward turns one the Halobject walks backward. If the Halobject is already walking, itwill first finish the current step.

9. Emergency Stop [1/0]If a emergency stop is needed (for example an instruction from the obstacle avoidancesoftware) the Halobject does not complete the walking cyclus but stops when the currentstep is finished.

10. Reset offset [1/0]In case of a emergency stop its possible that the Halobject has to stop outside the homingposition (namely at the opposite of the homing position). In that case, a internal flag is setwhich will force the software to wait for Reset offset to turn true. If so, the Halobject stepsback to reset this offset to return to the homing position.

12

• Outputs

1. Second order Trajectory turning motor left(position [rad], velocity [rad/s] and acceleration [rad/s2])

2. Second order Trajectory turning motor right(position [rad], velocity [rad/s] and acceleration [rad/s2])

3. Third order Trajectory cantilever motor(position [rad], velocity [rad/s], acceleration [rad/s2] and jerk [rad/s3])The cantilever has to deal with a relative large mass displacement (i.e. tilting the Halob-ject). To avoid problems caused by applying a step in the acceleration profile a third ordertrajectory is calculated (with step in jerk).

In figure 3.3 the global structure of the software can be seen with the IO’s as described above. Thesoftware is divided in two different parts, the trajectory planning and the trajectory output.

Figure 3.3: Global structure of the trajectory planning software

The trajectory planning calculates a acceleration profile for both the turning motors and outputsa vector with the corresponding switching times (9) for the acceleration profiles (at[9]) and the accel-eration values aleft and aright which only differ in case of cornering. The trajectory planning for thecantilever motor calculates a jerk profile and outputs the vector jt[31] with the 31 switching times togenerate the jerk profile. j[1] contains the corresponding jerk value. The trajectory output blocks forthe two turning motors and the cantilever motor aims to output the acceleration and jerk profile asfunction of time. By integrating these profiles the desired outputs are achieved, namely AVP (Accel-eration, Velocity, Position) for both turning motor and JAVP (Jerk, AVP) for the cantilever motor. To

13

0 1 2 3 4 5 6 7−2

−1.5

−1

−0.5

0

0.5

1

1.5

2

step

[rad

] / s

tep

heig

ht [r

ad]

time [sec]

LEFTRIGHTCANTILEVER

Figure 3.4: Typical trajectory for the walking mechanism during cornering

avoid drift, these integrators are resetted when the velocity or the position value equals zero.An example of calculated trajectories as used for the walking mechanism is showed in figure 3.4

In this example the reference path for the left turning motor is larger than the right reference pathso a right turn will be achieved. The value for alpha_hal was 10 degrees what means that the Halobjectexecuted a 10 degrees rotation within one walking cyclus (6,55 sec). Furthermore can be seen that thecantilever leg tilts the Halobject during one walking cyclus first on the left side, subsequently on theright side.The trajectory planning and output which will generate such reference paths will be discussed in detailin the following four subsections.

3.2.1 Second order trajectory planningThe c-file TP2.c represents the block "Second order trajectory planning Turning motor". Thisfile calculates a second order trajectory for the turning legs with respect to the desired step value, resttime t_rest and and cornering angle alpha_hal. Moreover the bounds on the acceleration and velocitymay not be exceeded. The calculated switching times needed for the acceleration profile are saved inthe vector at[9]. The scalar a[1] contains the corresponding value for the acceleration.If no cornering is desired, the value a[1] will be used for both a_left[1] and a_right[1]. If cornering isdesired, i.e. alpha_hal 6= 0, the value from a[1] is used for either a_left[1] or a_right[1] depending onwhich side is the outside. The outside leg which has to make the largest stroke, will use the value froma[1]. The stroke for the innerleg, which has to be smaller, is calculated with use of the desired corner-ing angle. As explained in the previous section the corresponding value for the inside forward stroke,step_small is calculated. Again with respect to the earlier mentioned input values (with exception forthe value for step) the switching times for the acceleration profile are calculated and saved in the cor-rect vector (a_left[1] or a_right[1]). To reduce the computers calculation time as much as possible, thesecalculation are only performed once when new input value is achieved.

3.2.2 Third order trajectory planningThe c-file TP3_C.c represents the block "Third order trajectory planning Turning motor" fromfigure 3.3 and calculates a third order trajectory for the cantilever motor. In contrast with the above-

14

mentioned trajectory planning, as being the time optimal solution with the given constraints, thecalculation of the trajectory for the cantilever differs from the standard method as described in chapter2.2.3. In principle, the movement of the cantilever can be derived from the calculated trajectory of theturning legs. The cantilever has to tilt the Halobject twice during one walking cyclus (figure 3.4). Onlythe time for one walk cyclus is needed and this end-time is already calculated in the previous subsec-tion. So this c-files uses the switching times information in vector at[9] as calculated in TP2.c. In factonly the last element in this vector is needed which holds the duration of one walking cyclus. Nowby dividing this duration time in a symmetrical jerk profile a third order trajectory can be calculatedsuch that it exactly matches the turning motor trajectory concerning the time-scale. In figure 3.5 anexample of a generated jerk profile is depicted. By integrating this profile, the remaining paths areachieved.

0 1 2 3 4 5 6 7−50

0

50

j [ra

d/s3 ]

0 1 2 3 4 5 6 7−5

0

5

a [r

ad/s

2 ]

0 1 2 3 4 5 6 7−2

0

2

v [r

ad/s

]

0 1 2 3 4 5 6 7−2

0

2

p [r

ad]

time [sec]

tj

tj t

j t

a t

v t

a

tj

ta

tj

tv

Figure 3.5: Trajectory planning for the cantilever

The jerk profile is generated as follows:When we consider only the first part of the trajectory, namely the transition from 0 to 2 radians (stepheight, t = 0...1.64 sec). This part consists of:

• one constant velocity phase tv ;

• one constant acceleration and one constant deceleration phase ta;

• two positive and two negative jerk phases tj .

Now by choosing:

tj = tend/72 (3.3)ta = 4tj (3.4)tv = 6tj (3.5)

the jerk profile as in figure 3.5 is generated such that the above mentioned velocity, acceleration anddeceleration phase are equal in duration.

15

The structure of the jerk profile is now determined. Only the value for the jerk has to be calculatedso the desired step height is achieved. Consider again the earlier mentioned first part of the trajectorywhich consist of the three equal phases tv and two times ta (so tv = ta ). First a second order trajectoryis calculated. The reached position of the first part is:

p =12a2t

2a + a2t

2v +

12a2t

2 = 2a2t2a ⇒ a2 =

p

2t2a(3.6)

with p equal to the desired step height and a2 the desired acceleration value to reach this step height witha second order profile. Now we make the extension to a third order profile. The reached velocity in thesecond order case is:

v2 = a2 · ta (3.7)and in the third order case:

v3 = 212a3 · tj + 4a3 · tj = 5a3 · tj (3.8)

To reach the same velocity (and so the same end-position) as in the second order case, the neededacceleration value in third order case has to be larger:

v2 = v3 ⇒ a2 · ta = 5a3 · tj ⇒ 6a2 · tj = 5a3 · tj ⇒ a3 =65a2 (3.9)

Finally, to reach a3 the jerk has to be:

j =a3

tj(3.10)

The switching times information is saved in vector jt[31] and the value for the jerk in scalar j. This in-formation is send to the block "Trajectory output cantilever motor" where the actual referencepaths are generated as a function of time (as in figure 3.5) and is discusses in section 3.2.5. Again,these calculations are only performed once when new input is received and new trajectories have tobe calculated.If input backward turns one, the Halobject has to walk backwards. To do so the trajectory for the can-tilever has to be multiplied with −1. Now the order of lifting each side is changed which will result ina backward walking movement.Finally, one remark about equation 3.3. With a running frequency of the software of 1000 Hz, theresolution for plotting the jerk profile is 1 msec. When the calculated value for tj is between two sam-ples rounding errors occur which will lead to a mismatch of the cantilever and the turning trajectoriesconcerning the time-scale.To avoid this problem, the value tend is rounded such that the division 1000∗tend

72 outputs a integer. Thisholds a maximum rounding error of 72/2 = 36 msec. Moreover this rounding occurs not in this c-filebut in the c-file TP2.c where tend is calculated. This means that the turning and cantilever trajectorywill match exactly and only a small difference occurs between the desired and calculated trajectoryconcerning the stroke. The maximal error in the position signal is 3..4e−3 radians which is acceptablefor the walking mechanism.

3.2.3 Trajectory output turning motorThe c-file TO2.c represents the blocks "Trajectory Output Turning motor Right" and "TrajectoryOutput Turning motor Left". This c-files uses the earlier calculated acceleration profile informa-tion (at[9], a_right or a_left) to output the acceleration reference signal for the right and left turningmotor. This signal is integrated once and twice to achieve respectively the velocity and position signal.It uses a counter triggered each sample time (1 msec) which is resetted at the end of each accelerationprofile, i.e. walking cyclus. By using counters, the software has time information needed to outputthe trajectory signals, but independently from the absolute running time. As depicted in figure 3.3 thetrajectory output software has to deal with three more inputs, that means ’start [1/0]’, ’emergency stop[1/0]’ and ’reset offset [1/0]’.

16

• start [1/0]If start [1/0] turns true the Halobject has to start walking. With use of (at[9] and a_right or a_left)the acceleration profile is generated as function of time with use of the counter. By integrationthe position and velocity signal are achieved and the AVP-signal is constructed which representsthe output of the TO2.c. If during walking new acceleration profile information is received, i.e.a new trajectory, the walking cyclus is first completed and the new trajectory is applied in thesucceeding walking cyclus. If start [1/0] turns zero again, the walk movement has to stop. Firstthe walking cyclus is finished so the Halobject will always stop in the homing position.

• emergency stop [1/0]As mentioned above, the Halobject will only stop walking after finishing the walking cyclus. Incase of an desired emergency stop ("soft" emergency stop) this might be a problem. When forexample an object is detected in the blind zone of the ultrasonic sensors, i.e. a object detectedwithin ± 60 cm of the Halobject, it is not advisable to finish the complete walking cyclus. In aworst-case-scenario the forward movement of the Halobject after receiving an emergency stopcan reach to 34 cm. Therefore the Halobject will in case of an estop not at all costs finish thewalking cyclus. The trajectory will be stopped as soon as the Halobject touches the ground withall his six legs, i.e at the half or at the end of the trajectory. In the second case the same situationoccurs as after a regular stop walking. However, in the first case additional software is neededthere the Halobject stopped outside the homing position. The movement now has stopped inthe opposite of the homing position (at t = 3.15 sec in figure 3.4). To deal with this offset fromthe homing position (for the turning legs) an additional input is created aiming to reset thisoffset, reset offset.Lastly, by introducing the emergency stop the worst-case-scenario forward movement is reducedto 34/2 = 17 cm.

• reset offset [1/0]In case of an offset to the homing position, the internal flag offset is set and by this the software isforced to wait for reset offset to turn true. If so, the last used trajectory is used to move the turninglegs back to the homing position to reset the offset. This movement has to be combined with acorresponding movement of the cantilever to tilt the Halobject so it a back-step is achieved. Thiswill be further discussed in the next section 3.2.3.So, without resetting the offset, no walking is allowed. The moment to reset this offset is adecision to be made for the central operating software (’character’). When reset offset alwaysequals one, an eventually offset after estop, is immediately resetted.

3.2.4 Trajectory output cantilever motorThe block "Trajectory output cantilever motor" from figure 3.3 is represented by the c-fileTO3_C.c. The jerk profile information jt[31] and j calculated in TP3_C.c is used to output the jerksignal as function of time using a counter. By integration, the JAVP signal is constructed which is theoutput of this file. Just like "Trajectory output turning motor" the same input signals have to beprocessed:

• start [1/0]If start [1/0] turns true the Halobject has to start walking. As mentioned above the JAVP signalsare generated as function of time. Just like "Trajectory Output Turning motor" the walkingcyclus is first finished after start turns false.

• emergency stop [1/0]As discussed earlier, after receiving an estop the trajectory stops only at the half or at the end ofthe walking cyclus.

17

• reset offset [1/0]In case the turning legs have an offset to the homing position after an estop, the software iswaiting for reset offset to turn true. If so the the cantilever has to tilt the Halobject. In fact thelast tilt move has to be repeated to let the Halobject step back and let the turning mechanismreturn in the homing position. In case the Halobject was already walking back when the estopwas received, automatically a step forward will be made.

18

3.2.5 Examples from simulationIn figure 3.6 (a) the AVP signals from the turning motor are showed during cornering. In figure 3.6(b) the JAVP signal is showed from the cantilever motor. In figure 3.7 (a) the three position paths areshowed. At t = 6 sec the value backward turns one. The current trajectory is finished and at t = 8.5sec the Halobject will start walking backwards.In figure 3.7 (b) again the three position paths are showed. Now at t = 10 sec an emergency stop isreceived. The current step is finished first and at t = 12.85 sec the Halobject has stopped. The offsetof the turning legs from the homing position is resetted at t = 15 sec when reset offset turns true andthe system is ready to walk again.

0 1 2 3 4 5 6 7 8 9−0.2

−0.1

0

0.1

0.2

a [r

ad/s

2 ]

0 1 2 3 4 5 6 7 8 9−0.2

−0.1

0

0.1

0.2

v [r

ad/s

]

0 1 2 3 4 5 6 7 8 9−0.8

−0.6

−0.4

−0.2

0

p [r

ad]

time [sec]

rightleft

(a)

0 1 2 3 4 5 6 7 8 9−20

0

20

j [ra

d/s3 ]

0 1 2 3 4 5 6 7 8 9−5

0

5

a [r

ad/s

2 ]

0 1 2 3 4 5 6 7 8 9−2

0

2

v [r

ad/s

]

0 1 2 3 4 5 6 7 8 9−2

0

2

p [r

ad]

time [sec]

(b)

Figure 3.6: (a) Turning motor (b) Cantilever motor

0 2 4 6 8 10 12 14 16 18−2

−1.5

−1

−0.5

0

0.5

1

1.5

2

p [r

ad]

time [sec]

LEFTRIGHTCANTILEVER

(a)

0 2 4 6 8 10 12 14 16 18 20−2

−1.5

−1

−0.5

0

0.5

1

1.5

2

p [r

ad]

time [sec]

LEFTRIGHTCANTILEVER

Received "Estop"

step finished

Received "Reset Offset"

(b)

Figure 3.7: (a) Transition from walking forwards to backwards (b) emergency stop succeeded byreset o�set

19

Chapter 4

Trajectory Planning for the FeelersMechanism

This chapter will discuss the motion controller as designed for the feelers mechanism on the Halob-ject. First of all a short overview will be given of the system which has to be controlled. In section 4.2the trajectory planning for the feeler mechanism will be described. Bla Bla.

4.1 IntroductionThe Halobject is equipped with two feelers which have to react on sounds from its surrounding.Therefore the Halobject is equipped with three microphone’s which are mounted in the mantle ofthe Halobject and are equally divided over the contour (with 120 degrees between them). Soundlocalization by means of a data-processing algorithm is possible. There interaction of the Halobjectwith its surrounding is desired, the central controlling software (i.e. the character) can act on thisinput in several ways. For example the Halobject can decide to be curious and moves towards thesound source. Concerning the feeler mechanism the Halobject can decide to scan the surroundingwith its feelers or actually point to a sound source.

Basically the feelers has to fulfill the following functions:

• The feelers are in scan mode and have to make a rotating movement to make the intension thatthe Halobject is scanning his surroundings;

• The feelers are in pointing mode and are pointing to a certain position in its surrounding;

• The feelers are in sleep mode. No action occurs.

During a previous internship, a two DOF mechanical system has been designed which can fulfillthese demands. This system makes use of two electric motors that are perpendicularly positioned so arotational movement can be achieved. The maximal allowable stroke from the midpoint is 18 degrees.In figure 4.1 and 4.2 the Halobject with the feelers and the feeler mechanism design is showed.

4.2 Trajectory planningElementarily, the trajectory planning software has to translate the demands of the character into theright reference paths for the two single axis motors so a desired movement is established (or no move-ment in case of the sleep mode). The software in question is written in C. This c-file is integrated

20

(a) (b)

Figure 4.1: (a) Halobject with feelers (b) Feeler mechanism

Figure 4.2: Realization of feeler mechanism

in MATLAB SimuLink as an S-function. S-functions (system-functions) are a powerful mechanism forextending the capabilities of Simulink. This S-function has the following in- and outputs (see figure4.3):

• Inputs

1. Scan mode [1/0]If scan mode is true, the scan movement will start. The scan movement will always startfrom position zero (x, y) = (0, 0). If scan mode is false, the scan movements stops andfeelers return to zero (x, y) = (0, 0). The scan mode uses second order trajectory planningwith the following three parameters (input 2, 3 and 4);

21

2. Deflection [degrees]This parameter is used in scan mode and represents the deflection in degrees of the feelersand is limited to 18 degrees due to the earlier stated mechanical restrictions. This signalis converted to radians and compensated for transmission of the worm - wormwheel set(1 : 25).

3. Bound on acceleration [rad/s2]Bound on acceleration is used for second order trajectory planning;

4. Bound on velocity [rad/s]Bound on velocity is used for second order trajectory planning;

5. Pointing mode [1/0]If pointing mode is true, the pointing move will start. The are three possible states fromwhere transition to pointing mode can take place:

(a) Sleeping mode. Feelers are in zero position (x, y) = (0, 0).(b) Scan mode. (

√x2 + y2 = delfection)

(c) Pointing mode, feelers are pointing to desired pointing angle. (x, y) = (xdes, ydes)and are waiting for new input.

If pointing mode turns false during pointing mode, the pointing will first finish the point-ing move (if it was not ready) and will return to position zero (x, y) = (0, 0). Pointingmode will always overrule scan mode, i.e. when pointing mode is true the software doesn’tlook for the value of scan mode. This means when pointing mode returns to zero, and scanmode is still true, the feelers will move from pointing position, via zero, to the rotating scanmove. All the used trajectories are second order.

6. Pointing angle [deg]Pointing angle is required in pointing mode and is translated in a corresponding value forthe x- and y-axis.

• Outputs

1. Second order Trajectory x-axis (position [rad], velocity [rad/s] and acceleration [rad/s2])

2. Second order Trajectory y-axis (position [rad], velocity [rad/s] and acceleration [rad/s2])

The above mentioned zero position of the feeler mechanism (x, y) = (0, 0) is determined by ahoming procedure during the startup of the Halobject. So (x, y) = (0, 0) represents the exact centerof the feeler position.

In figure 4.3 the global structure of te software can be seen with the IO’s as described above. Thesoftware is divided in two different parts, the trajectory planning and the trajectory output.

Figure 4.3: IO of the trajectory planning software

Both parts will be discussed in resp. subsection 4.2.1 and 4.2.2.

22

4.2.1 Second order trajectory planningThe c-file FTP2.c calculates a second order trajectory planning which will result in a scanning move-ment of the feeler. By generating a sinus reference for both x- and y-axis, but with 90 degrees delaybetween them, the feeler will describe a circle with a radius equal to the amplitude of the sinus. Thedisadvantage of applying a sine as reference signal is the startup of the movement. Taking the derativeof the sine-signal (position) will give a cosine signal for the velocity which will introduce a step at t0.To solve this problem the second order trajectory planning as described in chapter two is used. Firsta trajectory is calculated from zero to the desired amplitude with the given constraints for a and v.The switching times for the acceleration profile are saved in vector at_init with width four. The corre-sponding acceleration is equal to the value in scalar aa.These variables are the same for the x- and y-axis.

at_init = [t0 t1 t2 t3] [sec] (4.1)aa = a [rad/sec2] (4.2)

Subsequently, a reference signal for the scan mode is calculated. This signal has to start where theprevious trajectory stopped, namely at the desired amplitude. From there a second order approxima-tion of a sine has to start. The calculated switching times needed for the acceleration profile are savedin vector at. The corresponding acceleration is the same as for the initialization, namely aa.

at = [t0 t1 t2 t3 t4 t5] [sec] (4.3)

With these trajectories a smooth transition from zero into the scan mode is guaranteed. To keep thecomputer’s calculation time as small as possible, these calculation are only performed once when oneof the three inputs is changed.

4.2.2 Trajectory outputThe c-file FTO2.c uses the calculated acceleration profile information (at_init, at, aa) to output the ac-celeration reference signal. This signal is integrated once and twice to achieve respectively the velocityand position signal for the x- and y-axis. It uses two counters (for the x- and y-axis) triggered eachsample time (1 msec) which are resetted at the end of each acceleration profile. By using counters,the software has time information needed to output the trajectory signals, but independently from theabsolute running time.

As depicted in figure 4.3 the trajectory output software has to deal with three more inputs like ’scanmode [1/0]’, ’pointing mode [1/0]’ and ’pointing angle [deg]’. Therefore several states have been defined.In addition to the trivial states like scan mode and pointing mode, some extra states have been in-troduced to ensure that the desired behavior is emulated. The second order trajectory planning willguarantee a smooth movement of the feeler.

In figure 4.4 the statechart diagram is depicted which shows the sequence of states the softwaregoes through. Transitions (depicted as arrows between states) indicate that, in response to an event, anobject will go from one state to another and perform an action. The corresponding text-string containsthe event that triggers a transition. When in a given state, an object waits for an event to go to adifferent state. Below, the different states will be discussed. More detailed information can be foundin appendix A.

Initial state.To determine the neutral position of the feelers, a homing procedure is performed during thestartup of the Halobject. When this homing procedure is done (and the feelers are in homingposition (x, y) = (0, 0)), transition to next state takes place ('State: Waiting For Start').

23

State: Waiting For Start.The feelers are in homing position and are waiting for start, i.e. waiting for the booleans scanmode or pointing mode to turn true.

State: Scan Mode.Transition from 'State: Waiting For Start' when start scan == 1. This state contains fivesub-states:

• State: InitializationThis state realizes the transition from the x- and y-motor from zero to the desired de-flection value. When entering this state, first initialization of the x-axis will start usingthe acceleration switching times from vector at_init. When the trajectory is finished, andthe x-axis reached the desired deflection value, the flag initxdone is set and transition to'State: scan x-axis' takes place. The time to start the initialization of the y-axis isimportant to achieve a correct scanning movement. During the scan mode the criterium√

x2 + y2 = deflection must be satisfied. This means that the feeler is moving over acircle with a certain velocity ω. For the initialization state it means that the y-axis has tostart at a certain time, such that it reaches its maximum deflection when the x-axis passesthe zero (the x-axis is already in 'State: scan x-axis'). From this point the earlierstated circle-criterium is satisfied, the flag initydone is set and transition from the y-axis to'State: scan y-axis' occurs.

• State: Scan x-axisThis state realizes the scan movement of the x-axis after the flag initxdone is set. As ex-plained in section 4.2.1 it uses the switching times from vector at to output the accelerationprofile as function of counter x. By integration the velocity and position signal are obtained.

• State: Scan y-axisSame as State: Scan x-axis, but now for the y-axis (with initydone and counter y).

• State: Stop scan mode A x-axisDuring scan mode, the status of boolean start scan is only checked when the velocity (ofone axis) equals zero. If the start scan turns zero, the scan mode has to stop and the feelersmust return to zero. This state will take care for the transition from scan mode to thehoming position. A trajectory is calculated to let the x-axis return to zero.

• State: Stop scan mode A y-axisSame as for the x-axis. When both x and y-axis are returned to zero, transition to State:Waiting For Start takes place. In figures 4.5 and 4.6 the startup of the scan mode, thescan mode itself and stop scan mode is showed.

• State: Stop scan mode BDuring scan mode, the status of boolean pointing mode is only checked when the velocity(from x- or y-axis) equals zero. If pointing mode turns one, transition from scan to pointingmode has to occur (so the feelers are pointing to a certain angle). This state realizes thefirst part of it and aims to let the feeler stop on the scanning circle. From there transitionto State: Start Pointing Mode B takes place which realizes the second movement tothe desired pointing position. So the first part of the pointing mode occurs in this state. Infigure 4.7 the transition from scan mode to pointing mode is depicted. At t = 8 sec pointingmode turns true. At t = 9.4 sec, when the velocity of the first axis (y-axis) becomes zero, thevalue for pointing mode is checked. At this point transition to State: Stop scan modeB occurs. In this case the transition to this state first takes place for the y-axis becausethis axis reaches first the zero-velocity point. The axis which reaches State: Stop scanmode B first, will move to zero. The other will stay in scan mode until it also reaches thezero-velocity point. When the first axis, here y, reaches position zero, transition to the nextstate, State: Start Pointing Mode B occurs. When the second axis (x-axis) reaches itsmaximum amplitude (and so zero velocity) the State: Stop scan mode B is finished,and also transition to the next state takes place.

24

The reason for first returning to zero for the axis which enters first this state, is to en-sure that no trajectory is calculated which will result in a movement outside the desiredscanning radius (so

√x2 + y2 = deflection is not exceeded).

State: Start Pointing Mode B.This state succeeds the previous state and realizes the movement from a zero-velocity point (axisat zero or at maximum amplitude) to the desired endpoint with respect to the desired pointingangle. This desired pointing angle is converted to corresponding values for the x- and y-axis. Incase of the example (figure 4.7) first the x-axis reaches this state and a trajectory is calculated tothe desired end point. Same procedure is repeated when the y-axis enters this state. This stateis finished when both axis have finished their trajectory and the feeler has reached his desiredpointing position. Finally, this state transist to State: Pointing Mode.This state can also be reached from State: Waiting For Start when pointing mode turnsone and a movement from the homing position (x, y) = (0, 0) to the desired pointing positionhas to be established.

State: Pointing Mode.Feeler is in pointing position and is waiting for a new pointing angle or waiting for pointing modeto turn false (and pointing mode has to finish).

State: Start Pointing Mode C.In case a new pointing angle is received, State: Start Pointing Mode C calculates newtrajectories for x- and y-axis so movement to the new pointing position is realized. When bothtrajectories have finished transition back to State: Pointing Mode occurs. Additional soft-ware was needed to ensure that

√x2 + y2 = deflection is not exceeded, i.e. the calculated

trajectories will never result in a movement of the feeler outside the scanning radius. Basicallyit determines the quadrant of the scanning-circle for the current position and with respect tothe new pointing position it calculates the trajectories and determines which axis to start first.The second axis will start a certain adjustable period later. Figures 4.7 and 4.8 show the abovedescribed states.

State: Stop Pointing Mode.Transition to this state can only occur from State: Pointing Mode when pointing mode turnsfalse. Pointing mode has to end and this state realizes the movement from the current pointingposition to homing position (x, y) = (0, 0). When this position is reached transition to State:Waiting For Start occurs.

25

Figure 4.4: States chart

26

0 5 10 15−10

0

10

acce

lera

tion

[rad

/s2 ]

0 5 10 15−10

0

10

velo

city

[rad

/s]

0 5 10 15−10

0

10

posi

tion

[rad

]

0 5 10 150

5000

10000

coun

ter

[−]

time [sec]

x−axisy−axis

start scan = 0 start scan = 1

Figure 4.5: avp pro�les during scan mode with start at t =0 and stop at t =10 sec.

−8 −6 −4 −2 0 2 4 6 8−8

−6

−4

−2

0

2

4

6

8

x−axis [rad]

y−ax

is [r

ad]

Figure 4.6: position of feeler during scan mode with start from midpoint and return to midpointafter stop scan.

27

2 4 6 8 10 12 14 16 18 20 22−10

−5

0

5

10

acce

lera

tion

[rad

/s2 ]

2 4 6 8 10 12 14 16 18 20 22−10

−5

0

5

10

velo

city

[rad

/s]

2 4 6 8 10 12 14 16 18 20 22−10

−5

0

5

10

posi

tion

[rad

]

2 4 6 8 10 12 14 16 18 20 220

2000

4000

6000

coun

tet [

−]

time [sec]

Pointing mode = 1 & pointing angle = 35 deg

new pointing angle 195 deg

State: Scan Mode State: Stop Scan Mode B State: Start Pointing Mode B State: Pointing Mode State: Start Pointing Mode C State: Pointing Mode

State: Scan Mode State: Start Pointing Mode B

Figure 4.7: avp pro�les during scan mode with transition pointing mode (pointing mode =1 @ t= 8 sec).

−8 −6 −4 −2 0 2 4 6 8−8

−6

−4

−2

0

2

4

6

8

x−axis [rad]

y−ax

is [r

ad]

Figure 4.8: position of feeler during scan mode with start from midpoint and return to midpointafter stop scan.

28

Chapter 5

Conclusion and recommendations

The realized software was first tested in MatLab Simulink. Simulations turned out that the softwarewas capable of generating the correct trajectory with respect to the inputs. Subsequently the trajec-tory planning software was integrated with a controller and implemented on the PC104 system of theHalobject. Implementation-tests turned out that the software functioned correctly.Summarizing, the developed trajectory planning for the Halobject guarantees a safe use of the Halob-ject concerning the operation of the Halobject. Furthermore smooth movement of the walk and feelermechanisms is guaranteed.

In order to make the scanning movement of the feeler mechanism look more naturally, a randomfactor could be added which affects the rotational speed and / or the amplitude of the scanning move.

29

Bibliography

[1] P. F. Lambrechts Trajectory planning and feedforward design for electromechanical motion systems2003: Technical University Eindoven Report nr. DCT 2003-18

[2] R. H. M. Solberg, R.J.M de Jong Halobject manual Part 1 2005: Technical University EindovenReport nr. DCT 2005-30

[3] B. Kernighan, D. M. Ritchie C handboek 1990: Prenctice Hall, Acedemic Service Relations.

30

Appendix A

States of the trajectory planningfeelers

A.1 State: Scan Mode and State: Stop Scan Mode A

31

A.2 State: Stop Scan Mode B

33

A.3 State: Start Pointing Mode B and State: Pointing Modeand State: Start Pointing Mode C

34

A.4 State: Stop Pointing Mode

35

A.5 State: Start Pointing Mode B

36

Appendix B

C-source

B.1 Walking mechanism

B.1.1 Second order trajectory planning Turning motor/* Trajectory Planning Calculation* Walking Mechanism* Version 2.0* P.M. Nagelmaeker april '05**/

#define S_FUNCTION_NAME TP2#define S_FUNCTION_LEVEL 2#define stime 1000#define pi 3.1415927

#include "simstruc.h"#include <math.h>

static void mdlInitializeSizes(SimStruct *S){

ssSetNumSFcnParams(S, 0);if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)){

return;}

if (!ssSetNumInputPorts(S, 5)) return;

ssSetInputPortWidth(S, 0, 1); /*x*/ssSetInputPortWidth(S, 1, 1); /*vbound*/ssSetInputPortWidth(S, 2, 1); /*abound*/ssSetInputPortWidth(S, 3, 1); /*alfa hal*/ssSetInputPortWidth(S, 4, 1); /*trest*/ssSetInputPortDirectFeedThrough(S, 0, 1);ssSetInputPortDirectFeedThrough(S, 1, 1);ssSetInputPortDirectFeedThrough(S, 2, 1);ssSetInputPortDirectFeedThrough(S, 3, 1);ssSetInputPortDirectFeedThrough(S, 4, 1);

if (!ssSetNumOutputPorts(S,3)) return;ssSetOutputPortWidth(S, 0, 9);ssSetOutputPortWidth(S, 1, 9);ssSetOutputPortWidth(S, 2, 9);

ssSetNumRWork(S,5); /*Number of Real Workfactors*/ssSetNumSampleTimes(S, 1);ssSetOptions(S, SS_OPTION_EXCEPTION_FREE_CODE |

SS_OPTION_USE_TLC_WITH_ACCELERATOR);}

static void mdlInitializeSampleTimes(SimStruct *S)

37

{ssSetSampleTime(S, 0, INHERITED_SAMPLE_TIME);ssSetOffsetTime(S, 0, 0.0);

}

#define MDL_STARTstatic void mdlStart(SimStruct *S){

real_T *workR = ssGetRWork(S);workR[0]=0; /* xleft */workR[1]=0; /* v */workR[2]=0; /* a */workR[3]=0; /* xright */workR[4]=0; /* t rest */

}

static void mdlOutputs(SimStruct *S, int_T tid){

int_T i,delta_t,straight;real_T x,xs,xl,xleft,xright,vb,ab,a,alfa,t_1r,t_2r,tj,t_1,t_2;real_T t1,t2,t3,t4,t5,t6,t7,t8,t6des,trest;real_T at[9],aa[9],aa1[9];InputRealPtrsType in1 = ssGetInputPortRealSignalPtrs(S,0); /*x*/InputRealPtrsType in2 = ssGetInputPortRealSignalPtrs(S,1); /*vbound*/InputRealPtrsType in3 = ssGetInputPortRealSignalPtrs(S,2); /*abound*/InputRealPtrsType in4 = ssGetInputPortRealSignalPtrs(S,3); /*alfa hal*/InputRealPtrsType in5 = ssGetInputPortRealSignalPtrs(S,4); /*trest*/

real_T *y1 = ssGetOutputPortRealSignal(S,0);real_T *y2 = ssGetOutputPortRealSignal(S,1);real_T *y3 = ssGetOutputPortRealSignal(S,2);

real_T *workR = ssGetRWork(S);

stime=1000;pi=;

if (*in1[0] != workR[0] || *in2[0] != workR[1] || *in3[0] != workR[2] || *in4[0] != workR[3] || *in5[0] != workR[4]){xl=fabs(*in1[0]); /* abs values of input*/vb=fabs(*in2[0]);ab=fabs(*in3[0]);alfa=*in4[0]*pi/180; /* convert to radians*/trest=fabs(*in5[0])*stime;

xs=2*asin(sin(0.5*xl)-0.5*tan(fabs(alfa))); /* smallest forward stroke when cornering*/

if (alfa >= 0) { /* Determine innner- and outside during cornerin*/xleft=xl;xright=xs;}

else if (alfa < 0) { /* Right turn*/xright=xl;xleft=xs;}

if (xl == xs){ /* === Determine cornering yes/no */straight=1;}

else {straight=0;}

/* ============== Calculate trajectory with largest tend (== largest x) ========*//* ==== Calculating t_1 (Constant Acc phase) and t_2 (Constant Vel phase) ===== */t_1r=sqrt(xl/ab); /* Const Acceleration Time */if (vb<ab*t_1r) /* v bound violated ? */{

t_1r=vb/ab;ab=vb/t_1r;

}t_2r = (xl-ab*t_1r*t_1r)/vb; /* Const Velocity Time */ab = xl/( t_1r*t_1r + t_1r*t_2r ); /* New bound on acceleration*/

t_1=floor(t_1r*1000+0.5); /*rounding*/t_2=floor(t_2r*1000+0.5); /*rounding*/

/* ===== Calculating Acc. Profile ============================================ */t6=4*t_1+2*t_2; /*end time of cyclus*/tj=floor(t6/72+0.5);/*desired const acc time for kantelmotor. Needed to avoid rounding error in kantel motor calculation*/t6des=72*tj;/*desired end time with respect to rounding error of zero*/

38

delta_t=fabs(t6-t6des); /*difference*/

/*start routine to round t6 to t6des to avoid roundings problems*/if ( delta_t!=0 ) {

if (t6 < t6des) {t_1=t_1+delta_t/4;} /* enlarge const. acc. time */

else if (t6 > t6des) {t_1=t_1-delta_t/4;} /* shorten const. acc. time */

}

t6=4*t_1+2*t_2;delta_t=fabs(t6-t6des);if ( delta_t!=0 ) {

if (t6 < t6des) {t_2=t_2+delta_t/2;} /* enlarge const. acc. time */

else if (t6 > t6des) {t_2=t_2-delta_t/2;} /* shorten const. acc. time */

}

t1=t_1;t2=t1+t_2;t3=t2+t_1;t4=t3+trest;t5=t4+t_1;t6=t5+t_2;t7=t6+t_1;t8=t7+trest;

at[0]=0.0; at[1]=t1; at[2]=t2; at[3]=t3; at[4]=t4; at[5]=t5; at[6]=t6; at[7]=t7; at[8]=t8;aa[0]=ab; aa[1]=0.0; aa[2]=-ab; aa[3]=0.0; aa[4]=-ab; aa[5]=0.0; aa[6]=ab; aa[7]=0.0; aa[8]=0;

/*calculating trajectory for smallest x (xleft or xright)*//* only during cornering*/if (straight == 0) {

t_1=t_1/1000;t_2=t_2/1000;a=xs/(t_1*t_1+t_1*t_2);aa1[0]=a; aa1[1]=0.0; aa1[2]=-a; aa1[3]=0.0; aa1[4]=-a; aa1[5]=0.0; aa1[6]=a; aa1[7]=0.0; aa1[8]=0;

}else if (straight ==1) {

aa1[0]=ab; aa1[1]=0.0; aa1[2]=-ab; aa1[3]=0.0; aa1[4]=-ab; aa1[5]=0.0; aa1[6]=ab; aa1[7]=0.0; aa1[8]=0;}for (i=0; i<9; i++) { /* output*/

y1[i]=at[i];if (xleft >= xright) {

y2[i]=-aa[i];y3[i]=-aa1[i];}

else if (xleft < xright) {y3[i]=-aa[i];y2[i]=-aa1[i];}

}} /*end if (new data?) loop*/

/* ===== Storing old input ====================================== */workR[0]=*in1[0];workR[1]=*in2[0];workR[2]=*in3[0];workR[3]=*in4[0];workR[4]=*in5[0];

}

static void mdlTerminate(SimStruct *S){}

#ifdef MATLAB_MEX_FILE /* Is this file being compiled as a MEX-file? */#include "simulink.c" /* MEX-file interface mechanism */#else#include "cg_sfun.h" /* Code generation registration function */#endif

B.1.2 Third order trajectory planning Cantilever motor/* Trajectory Planning Calculation for Cantilever Motor* Version 2.0* P.M. Nagelmaeker april '05

39

**/

#define S_FUNCTION_NAME TP3_C#define S_FUNCTION_LEVEL 2#define stime 1000 /* sample frequency*/




return;}


ssSetInputPortWidth(S, 0, 1); /*stepheight*/ssSetInputPortWidth(S, 1, 9); /*t*/ssSetInputPortWidth(S, 2, 1); /*backwards?*/ssSetInputPortDirectFeedThrough(S, 0, 1);ssSetInputPortDirectFeedThrough(S, 1, 1);ssSetInputPortDirectFeedThrough(S, 2, 1);

if (!ssSetNumOutputPorts(S,2)) return;ssSetOutputPortWidth(S, 0, 31);ssSetOutputPortWidth(S, 1, 31);

ssSetNumRWork(S,11); /*number of real Workfactor*/

ssSetNumSampleTimes(S, 1);ssSetOptions(S, SS_OPTION_EXCEPTION_FREE_CODE |

SS_OPTION_USE_TLC_WITH_ACCELERATOR);}

static void mdlInitializeSampleTimes(SimStruct *S){

ssSetSampleTime(S, 0, INHERITED_SAMPLE_TIME);ssSetOffsetTime(S, 0, 0.0);

}


int_T i;real_T *workR = ssGetRWork(S);

for (i=0; i<11; i++) {workR[i]=0;}

}


int_T i,back;real_T x,a,a3,t_end,ta,tj,j,trest;real_T jt[31],jj[31];InputRealPtrsType in1 = ssGetInputPortRealSignalPtrs(S,0);InputRealPtrsType in2 = ssGetInputPortRealSignalPtrs(S,1);InputRealPtrsType in3 = ssGetInputPortRealSignalPtrs(S,2);

real_T *y1 = ssGetOutputPortRealSignal(S,0);real_T *y2 = ssGetOutputPortRealSignal(S,1);


/* ===== Getting Absolute value of Input =============================== */

if (*in1[0] != workR[0] || *in2[8] != workR[9] || *in3[0] != workR[10]){

x=fabs(*in1[0]); /* abs values of input*/t_end=*in2[3]/stime; /* end time of first half of cycles == at[3]*/

ta=t_end/6; /* values for 2nd order output */

40

a=x/(2*ta*ta);

a3=6*a/5; /* new max value for a with bound on jerk*/tj=ta/6;j=6*a/(5*tj); /* jerk [rad/s^3 with tj=ta/6 (and ta=/tend/12)*/

trest=(*in2[4]-*in2[3])/stime;

jt[0]=0.0; jt[1]= tj; jt[2]=jt[1]+4*tj; jt[3]=jt[2]+tj; jt[4]=jt[3]+ta; jt[5]=jt[4]+tj; jt[6]=jt[5]+4*tj;jj[0]=j; jj[1]=0.0; jj[2]=-j; jj[3]=0; jj[4]=-j; jj[5]=0; jj[6]=j;

jt[7]=jt[6]+tj; jt[8]=jt[7]+tj;jj[7]=-j; jj[8]=0;

jt[9]=jt[8]+4*tj; jt[10]=jt[9]+tj;; jt[11]=jt[10]+ta; jt[12]=jt[11]+tj; jt[13]=jt[12]+4*tj; jt[14]=jt[13]+tj;jj[9]=j; jj[10]=0.0; jj[11]=j; jj[12]=0; jj[13]=-j; jj[14]=0;

jt[15]=jt[14]+trest;jj[15]=-j;

jt[16]=jt[15]+tj; jt[17]=jt[16]+4*tj; jt[18]=jt[17]+tj; jt[19]=jt[18]+ta;; jt[20]=jt[19]+tj;jj[16]=0; jj[17]=j; jj[18]=0; jj[19]=j; jj[20]=0;

jt[21]=jt[20]+4*tj; jt[22]=jt[21]+tj;jj[21]=-j; jj[22]=j;

jt[23]=jt[22]+tj; jt[24]=jt[23]+4*tj; jt[25]=jt[24]+tj; jt[26]=jt[25]+ta; jt[27]=jt[26]+tj;; jt[28]=jt[27]+4*tj;jj[23]=0; jj[24]=-j; jj[25]=0; jj[26]=-j; jj[27]=0; jj[28]=j;

jt[29]=jt[28]+tj; jt[30]=jt[29]+trest;jj[29]=0; jj[30]=0;

if (*in3[0] == 0) { /* walking forward*/back=1;}

else if (*in3[0] == 1) { /* walking backward*/back=-1;}

for (i=0; i<31; i++){

y1[i]=floor(jt[i]*1000+0.5);y2[i]=back*jj[i];

}} /*end if (new data?) loop*/

/* ===== Storing old input ====================================== */workR[0]=*in1[0];workR[9]=*in2[8];workR[10]=*in3[0];

}



B.1.3 Trajectory output turning motor/* Trajectory Output Calculation for turning motor* Version 1.4* P.M. Nagelmaeker may '05**/

#define S_FUNCTION_NAME TO2#define S_FUNCTION_LEVEL 2#define stime 1000



ssSetNumSFcnParams(S, 0);

41

if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)){

return;}


ssSetInputPortWidth(S, 0, 9);ssSetInputPortWidth(S, 1, 9);ssSetInputPortWidth(S, 2, 1);ssSetInputPortWidth(S, 3, 1);ssSetInputPortWidth(S, 4, 1);ssSetInputPortDirectFeedThrough(S, 0, 1);ssSetInputPortDirectFeedThrough(S, 1, 1);ssSetInputPortDirectFeedThrough(S, 2, 1);ssSetInputPortDirectFeedThrough(S, 3, 1);ssSetInputPortDirectFeedThrough(S, 4, 1);

if (!ssSetNumOutputPorts(S,3)) return;ssSetOutputPortWidth(S, 0, 1); /* acc*/ssSetOutputPortWidth(S, 1, 1); /* vel*/ssSetOutputPortWidth(S, 2, 1); /* pos*/

ssSetNumIWork(S,6); /* count[1] + ready [1] + start[1] + estop[1] + offset[1] + reset[1] */ssSetNumRWork(S,29); /* 18 + 3 == 0..8=not used + in2[9] + aold[1] + v[1] + x[1] + in1[9]*/

ssSetNumSampleTimes(S, 1);

ssSetOptions(S, SS_OPTION_EXCEPTION_FREE_CODE |SS_OPTION_USE_TLC_WITH_ACCELERATOR);

}



}


int_T i;int_T *workI = ssGetIWork(S);real_T *workR = ssGetRWork(S);

for (i=0; i<6; i++) {workI[i]=0;}

for (i=0; i<29; i++) {workR[i]=0;}

}


int_T i,stime,tend,tenddiv2,count,ready,start,estop,offset;real_T a,aold;

InputRealPtrsType in1 = ssGetInputPortRealSignalPtrs(S,0); /*at [0..9] */InputRealPtrsType in2 = ssGetInputPortRealSignalPtrs(S,1); /*aa [0..9] */InputRealPtrsType in3 = ssGetInputPortRealSignalPtrs(S,2); /*start[0]*/InputRealPtrsType in4 = ssGetInputPortRealSignalPtrs(S,3); /*estop[0] */InputRealPtrsType in5 = ssGetInputPortRealSignalPtrs(S,4); /*reset[0]*/

real_T *y1 = ssGetOutputPortRealSignal(S,0); /* Acc,real_T *y2 = ssGetOutputPortRealSignal(S,1); /* vel*/real_T *y3 = ssGetOutputPortRealSignal(S,2); /* pos*/

int_T *workI = ssGetIWork(S); /* workfactors integer*/real_T *workR = ssGetRWork(S); /* workfactors real*/

count=workI[0];ready=workI[1];start=workI[2];estop=workI[3];offset=workI[4]; /* ==1 if emergency stop activated at first half of cylcus*/aold=workR[18];

42

stime=1000;

tend=workR[8+21]; /* end time cylcus in counts */tenddiv2=workR[3+21]; /* half of cyclus in counts */

if (ready ==1){ready=0;}

if (count == 0) {ready=1;}

if (ready == 1 && offset ==0 ) { /*Getting new input once a cycle*/for (i=0; i<9; i++) /* Getting New Input Values */{

workR[i+21] =*in1[i]; /* = at[9]*/workR[i+9]=*in2[i]; /* = aa[9]*/

}workI[2]=*in3[0]; /* getting value for start*/start=workI[2];

}

if (ready == 1 || count == tenddiv2) { /* getting value for estop*/workI[3]=*in4[0];estop=workI[3];if (count == tenddiv2 && estop == 1) {

offset=1;workI[4]=1;}

}

if (start == 1 && estop == 0 && offset == 0) /*Start walk*/{/* ======= Calculating Acc as functions of counts ========================== */if ( count < workR[1+21] )

{ a=workR[9];} /* a= ab*/else if ( count >= workR[1+21] && count < workR[2+21] )

{ a=workR[10];} /* a= 0*/else if ( count >= workR[2+21] && count < workR[3+21] )

{ a=workR[11];} /* a= -ab*/else if ( count >= workR[3+21] && count < workR[4+21] )

{ a=workR[12];} /* a= 0 if trest !=0 */else if ( count >= workR[4+21] && count < workR[5+21] )

{ a=workR[13];} /* a= -ab*/else if ( count >= workR[5+21] && count < workR[6+21] )

{ a=workR[14];} /* a= 0*/else if ( count >= workR[6+21] && count < workR[7+21] )

{ a=workR[15];} /* a= ab*/else if ( count >= workR[7+21] && count-1 <= workR[8+21])

{ a=workR[16];} /* a= 0 */

/* ============= integrators ================================================= */workR[19]=workR[19]+aold/stime;workR[20]=workR[20]+*y2/stime;

/* ========== Reset Integrators ==============================================*/if (count == 0 || count == tenddiv2 ) /*Reset Integrator velocity*/{workR[19]=0;}if (count == 0) /*Reset Integrator position*/{workR[20]=0;}

/* ======= genereren outputs ================================================ */*y1=a;*y2=workR[19];*y3=workR[20];

workR[18]=a; /*Storing old value of a*/

/* ======= ophogen Counts ================================================ */if ( count<tend )

{ (workI[0])++;}if ( count>=tend )

{ workI[0]=0;}} /* end if start == 1*/

else if (start == 0 || estop == 1 && count == 0) { /*ESTOP received*/*y1=0;*y2=0;

43

*y3=0;}

if (offset == 1) {if (*in5[0] == 1) { /* request for reset ?? */

workI[5] = 1;} /* reset =1*/if (workI[5] ==1) { /* if reset == 1*/

if ( count >= workR[4+21] && count < workR[5+21]){ a=workR[13];} /* a= -ab*/

else if ( count >= workR[5+21] && count < workR[6+21] ){ a=workR[14];} /* a= 0*/

else if ( count >= workR[6+21] && count-1 <= workR[7+21] ){ a=workR[15];} /* a= ab*/

/* ============= integrators ================================================= */workR[19]=workR[19]+aold/stime;workR[20]=workR[20]+*y2/stime;

/* ========== Reset Integrators ==============================================*/if (count == 0 || count == tenddiv2 ) /*Reset Integrator velocity*/

{workR[19]=0;}if (count == 0) /*Reset Integrator position*/

{workR[20]=0;}

/* ======= genereren outputs ================================================ */*y1=a;*y2=workR[19];*y3=workR[20];

workR[18]=a; /*Storing old value of a*/

/* ======= ophogen Counts ================================================ */if ( count<tend ){

(workI[0])++;}if ( count>=tend ){

workI[0]=0; /* count =0 */workI[4]=0; /* offset =0*/workI[5]=0;} /* reset =0 */

} /*end of reset period*/}

} /* end of mdlOutputs function*/



B.1.4 Trajectory output cantilever motor/* Trajectory Output Calculation* Version 1.7* P.M. Nagelmaeker mei '05**/

#define S_FUNCTION_NAME TO3_C#define S_FUNCTION_LEVEL 2#define stime 1000




return;}

44


ssSetInputPortWidth(S, 0, 31); /* #0..30 jt */ssSetInputPortWidth(S, 1, 31); /* #31..61 jj */ssSetInputPortWidth(S, 2, 1); /* #62 start */ssSetInputPortWidth(S, 3, 1); /* #63 estop */ssSetInputPortWidth(S, 4, 1); /* #64 reset offset? */ssSetInputPortDirectFeedThrough(S, 0, 1);ssSetInputPortDirectFeedThrough(S, 1, 1);ssSetInputPortDirectFeedThrough(S, 2, 1);ssSetInputPortDirectFeedThrough(S, 3, 1);ssSetInputPortDirectFeedThrough(S, 4, 1);

if (!ssSetNumOutputPorts(S,4)) return;ssSetOutputPortWidth(S, 0, 1);ssSetOutputPortWidth(S, 1, 1);ssSetOutputPortWidth(S, 2, 1);ssSetOutputPortWidth(S, 3, 1);

ssSetNumIWork(S,6); /* count[1] + ready[1] + start[1] + emergency stop [1] + offset[1] + reset[1] */ssSetNumRWork(S,67); /* 31 + 31 + 4 == jt[31] + jj[31] + jold[1] + a[1] + v[1] + x[1] +store[1]*/



}



}



workI[0]=0; /* count*/workI[1]=0; /* ready bit*/workI[2]=0; /* start bit*/workI[3]=0; /* emergency stop bit*/workI[4]=0; /* offset*/workI[5]=0; /* reset*/

for (i=0; i<67; i++) {workR[i]=0;}

}


int_T i,stime,tend,tenddiv2,tend3div4,tenddiv4,count,ready,start,estop,offset;real_T j,jold;

InputRealPtrsType in1 = ssGetInputPortRealSignalPtrs(S,0); /*jt[31]*/InputRealPtrsType in2 = ssGetInputPortRealSignalPtrs(S,1); /*jj[31]*/InputRealPtrsType in3 = ssGetInputPortRealSignalPtrs(S,2); /*start*/InputRealPtrsType in4 = ssGetInputPortRealSignalPtrs(S,3); /*estop*/InputRealPtrsType in5 = ssGetInputPortRealSignalPtrs(S,4); /*reset offset */real_T *y1 = ssGetOutputPortRealSignal(S,0); /*jerk*/real_T *y2 = ssGetOutputPortRealSignal(S,1); /*acceleration*/real_T *y3 = ssGetOutputPortRealSignal(S,2); /*velocity*/real_T *y4 = ssGetOutputPortRealSignal(S,3); /*position*/

int_T *workI = ssGetIWork(S);real_T *workR = ssGetRWork(S);

count=workI[0];ready=workI[1];start=workI[2];estop=workI[3];offset=workI[4];jold=workR[62];stime=1000;

45

tend=workR[30]; /* end time cylcus in counts */tenddiv4=workR[7];tenddiv2=workR[14];tend3div4=workR[22];

if (ready ==1) /*Reset of Ready-bit*/{ready=0;}

if (count == 0 ) /* Set Ready-bit*/{ready=1;}

if (ready == 1) { /*Getting new input once a cycle*/workI[2]=*in3[0];start=workI[2];for (i=0; i<31; i++) /* Getting New Input Values */{

workR[i]=*in1[i]; /* = jt[31]*/workR[i+31]=*in2[i]; /* = jj[31]*/

}}

if (ready == 1 || count == tenddiv2) { /*Checking for ESTOP*/workI[3]=*in4[0];estop=workI[3];

if (estop == 1 && count == tenddiv2) {offset=1;workI[4]=offset;}

}

if (start == 1 && estop == 0 && offset==0) /*option 1*/{

/* ======= Calculating Acc as functions of counts ========================== */if ( count < workR[1] ) /* j=j*/

{ j=workR[0+31];workR[63]=workR[63]+jold/stime;}

else if ( count >= workR[1] && count < (workR[2]) ) /* j= 0*/{ j=workR[1+31];workR[63]=workR[63]+jold/stime;workR[66]=workR[63];} /*store a for calc V*/

else if ( count >= workR[2] && count < workR[3] ){ j=workR[2+31]; /* j= -j*/workR[63]=workR[63]+jold/stime;}

else if ( count >= workR[3] && count < workR[4] ){ j=workR[3+31]; /* j= 0 */workR[63]=0;}


else if ( count >= workR[5] && count < workR[6] ){ j=workR[5+31]; /* j= 0*/workR[63]=workR[63]+jold/stime;}

else if ( count >= workR[6] && count < workR[7] ){ j=workR[6+31]; /* j= j*/workR[63]=workR[63]+jold/stime;}


else if ( count >= workR[8] && count < workR[9] ){ j=workR[8+31]; /* j= 0 */workR[63]=workR[63]+jold/stime;}


else if ( count >= workR[10] && count < workR[11] ){ j=workR[10+31]; /* j= 0*/workR[63]=0;}




else if ( count >= workR[14] && count < workR[15] ) /*== trest*/{ j=workR[14+31]; /* j=0*/

46

workR[63]=workR[63]+jold/stime;}else if ( count >= workR[15] && count < workR[16] )

{ j=workR[15+31]; /* j= -j*/workR[63]=workR[63]+jold/stime;}














else if ( count >= workR[29] && count < workR[30]+1 ) /* ==trest*/{ j=workR[29+31]; /* j= 0*/workR[63]=workR[63]+jold/stime;}

/* ======= genereren outputs ================================================ */*y1=j;*y2=workR[63];

/* ======= integrators ====================================================== */workR[64]=workR[64]+*y2/stime; /* velocity*/*y3=workR[64];workR[65]=workR[65]+*y3/stime; /* position*/*y4=workR[65];

if (count ==0 || count == tenddiv4 || count == tenddiv2 || count == tend3div4 ){

workR[64]=0; *y3=workR[64]; /*Reset Integrator velocity*/}

if (count == 0 ) /*Reset Integrator position*/{ workR[63]=0; *y2=workR[63];

workR[65]=0; *y4=workR[65];}

/* ======= ophogen Counts ================================================ */workR[62]=j; /*storing old value*/

if ( count<tend ){ (workI[0])++;}

if ( count>=tend ){ workI[0]=0;}

}else if (start == 0 || estop == 1 && offset == 0) { /*no offset of keermotor...Waiting for start*/ /*option 2*/

47

*y1=0;*y2=0; workR[63]=0.0;*y3=0; workR[64]=0.0;*y4=0; workR[65]=0.0;}

else if (offset == 1) { /*offset of keermotor...Waiting for resetting offset*/ /*option 3*/if (*in5[0] == 1) { /* request for reset ?? */

workI[5] = 1;} /* reset =1*/if (workI[5] ==0 ) { /* if reset == 0*/

*y1=0;*y2=0; workR[63]=0.0;*y3=0; workR[64]=0.0;*y4=0; workR[65]=0.0;jold=0; workR[62]=0;}

else if (workI[5] == 1) { /* Resetting offset*/if ( count >= workR[14] && count < workR[15] )


else if ( count >= workR[15] && count < workR[16] ){ j=workR[15+31]; /* j= -j*/


{ j=workR[16+31]; /* j= 0*/workR[63]=workR[63]+jold/stime;}

else if ( count >= workR[17] && count < workR[18] ){ j=workR[17+31]; /* j= j*/



else if ( count >= workR[19] && count < workR[20] ){ j=workR[19+31]; /* j= j*/



else if ( count >= workR[21] && count < workR[22] ){ j=workR[21+31]; /* j= -j*/


{ j=workR[22+31]; /* j= j*/workR[63]=workR[63]+jold/stime;}

else if ( count >= workR[23] && count < workR[24] ){ j=workR[23+31]; /* j= 0*/








{ j=workR[28+31]; /* j= j*/workR[63]=workR[63]+jold/stime;}

else if ( count >= workR[29] && count < workR[30]+1 ) /* ==trest*/{ j=workR[29+31]; /* j= 0*/

workR[63]=workR[63]+jold/stime;}

/* ======= genereren outputs ================================================ */*y1=-j;*y2=-workR[63];

/* ======= integrators ====================================================== */workR[64]=workR[64]+*y2/stime; /* velocity*/*y3=workR[64];workR[65]=workR[65]+*y3/stime; /* position*/*y4=workR[65];

if (count ==0 || count == tenddiv4 || count == tenddiv2 || count == tend3div4 ){ workR[64]=0; *y3=workR[64];} /*Reset Integrator velocity*/

if (count == 0 ) /*Reset Integrator position*/{ workR[63]=0; *y2=workR[63];

workR[65]=0; *y4=workR[65];}

48

/* ======= ophogen Counts ================================================ */workR[62]=j;

if ( count<tend ){ (workI[0])++;}

if ( count>=tend ){ workI[0]=0;workI[4]=0; /* offset =0 */workI[5]=0;} /* reset =0 */

} /* end of resetting offset*/

}

} /*end of output function*/



49

B.2 Feeler mechanismB.2.1 Second order trajectory planning Feelers/* Trajectory Planning Calculation Feelers* Version 2.0* P.M. Nagelmaeker mei '05***/

#define S_FUNCTION_NAME FTP2#define S_FUNCTION_LEVEL 2#define PI 3.14159265358979#define stime 1000




return;}


ssSetInputPortWidth(S, 0, 1); /* desired deflection (max value = 18 deg) */ssSetInputPortWidth(S, 1, 1); /* vbound*/ssSetInputPortWidth(S, 2, 1); /* abound*/ssSetInputPortDirectFeedThrough(S, 0, 1);ssSetInputPortDirectFeedThrough(S, 1, 1);ssSetInputPortDirectFeedThrough(S, 2, 1);

if (!ssSetNumOutputPorts(S,3)) return;ssSetOutputPortWidth(S, 0, 6); /*at[6]*/ssSetOutputPortWidth(S, 1, 6); /*aa[6]*/ssSetOutputPortWidth(S, 2, 4); /*at_init[4]*/

ssSetNumRWork(S,3); /*Set number of real workfactors*/



}


ssSetSampleTime(S, 0, INHERITED_SAMPLE_TIME);

ssSetOffsetTime(S, 0, 0.0);}


real_T *workR = ssGetRWork(S);workR[0]=0; /* desired deflection (max value = 18 deg) == *in[0] */workR[1]=0; /* vbound == *in[1] */workR[2]=0; /* abound == *in[2] */

}


int_T i,tv_init,ta,tv;real_T ampl,vb,ab,tar,tvr,tv_initr,t1,t2,t3,t4,t5;real_T at[6],aa[6],at_init[4];InputRealPtrsType in = ssGetInputPortRealSignalPtrs(S,0);real_T *y1 = ssGetOutputPortRealSignal(S,0); /*at[6]*/real_T *y2 = ssGetOutputPortRealSignal(S,1); /*aa[6]*/real_T *y3 = ssGetOutputPortRealSignal(S,2); /*at_init[14]*/

50


if (*in[0] != workR[0] || *in[1] != workR[1] || *in[2] != workR[2]){ampl=fabs(*in[0])*PI*25/180; /* abs values of input and converted to radians and compensated for transmission*/vb=fabs(*in[1]);ab=fabs(*in[2]);

/* ==== Calculating t_1 (Constant Acc phase) and t_2 (Constant Vel phase) ===== */tar=sqrt(ampl/ab); /* Const Acceleration Time */if (vb<ab*tar) /* v bound violated ? */{

tar=vb/ab;ab=vb/tar;

}vb=ab*tar;tvr = (2*ampl-ab*tar*tar)/vb; /* Const Velocity Time */tv_initr=(ampl-ab*tar*tar)/vb; /* Const Velocity Time for init */ta=floor(tar*1000+0.5); /*rounding*/tv=floor(tvr*1000+0.5);tv_init=floor(tv_initr*1000+0.5);

t1=ta;t2=t1+tv;t3=t2+2*ta;t4=t3+tv;t5=t4+ta;

at[0]=0.0; at[1]=t1; at[2]=t2; at[3]=t3; at[4]=t4; at[5]=t5;aa[0]=-ab; aa[1]=0.0; aa[2]=ab; aa[3]=0.0; aa[4]=-ab; aa[5]=0.0;at_init[0]=0.0; at_init[1]=ta; at_init[2]=ta+tv_init; at_init[3]=ta+tv_init+ta;

for (i=0; i<6; i++) {y1[i]=at[i];y2[i]=aa[i];}

for (i=0; i<4; i++) {y3[i]=at_init[i];}

} /*end if (new data?) loop*/

/* ===== Storing old input ====================================== */workR[0]=*in[0];workR[1]=*in[1];workR[2]=*in[2];

}



B.2.2 Trajectory output Feelers/* Trajectory Output Calculation* Version 2.1* P.M. Nagelmaeker mei '05***/

#define S_FUNCTION_NAME FTO2#define S_FUNCTION_LEVEL 2#define PI 3.14159265358979#define stime 1000 /* sample frequency*/#define ampl 7.4 /* desired ampl used in pointing mode */

51

#define ab 6.0 /* Bound on acc. used in pointing mode */#define vb 4.0 /* Bound on vel. used in pointing mode */#define DIV 4 /* tend pointing mode divided by DIV is starttime for second axis*/




return;}

if (!ssSetNumInputPorts(S, 6)) return;ssSetInputPortWidth(S, 0, 6); /*at[6]*/ssSetInputPortWidth(S, 1, 6); /*aa[6]*/ssSetInputPortWidth(S, 2, 4); /*at_init[4]*/ssSetInputPortWidth(S, 3, 1); /*scan mode*/ssSetInputPortWidth(S, 4, 1); /*startpointingx mode*/ssSetInputPortWidth(S, 5, 1); /*startpointingx angle*/ssSetInputPortDirectFeedThrough(S, 0, 1);ssSetInputPortDirectFeedThrough(S, 1, 1);ssSetInputPortDirectFeedThrough(S, 2, 1);ssSetInputPortDirectFeedThrough(S, 3, 1);ssSetInputPortDirectFeedThrough(S, 4, 1);ssSetInputPortDirectFeedThrough(S, 5, 1);

if (!ssSetNumOutputPorts(S,6)) return;ssSetOutputPortWidth(S, 0, 1); /*ax*/ssSetOutputPortWidth(S, 1, 1); /*vx*/ssSetOutputPortWidth(S, 2, 1); /*px*/ssSetOutputPortWidth(S, 3, 1); /*ay*/ssSetOutputPortWidth(S, 4, 1); /*vy*/ssSetOutputPortWidth(S, 5, 1); /*py*/

ssSetNumIWork(S,22);/* count_x[1] + startpointingx[1] + + start homing[1] + homing x done[1] +at_init[4] + count_y[1] + homing y done[1]+start homing y[1] + stop2halfx[1] + stop2halfy[1] + startpointingy[1] +spxdone[1] + spydone[1] + pxdone[1] + pydone[1]*/ssSetNumRWork(S,21);/* in1[6] + in2[6] + aold_x[1] + v_x[1] + x_x[1] +aold_y[1] + v_y[1] + x_y[1] + pold_x[1] +pold_y[1] +alpha_des old[1]== 0..5 + 6..11 + 12+13+14 + 15+16+17 + 18+19 +20*/



}



}



workI[0]=0; /* count_x */workI[1]=0; /* startpointingx 1 == startpointingx mode is acive...Transition from scan mode to zero (0,0) */workI[2]=0; /* start x 1 == starting init + scan for x-axis*/workI[3]=0; /* initxdone 1 == initialisation for x-axis done*/workI[4]=0; /* at_init[0] */workI[5]=0; /* at_init[1] */workI[6]=0; /* at_init[2] */workI[7]=0; /* at_init[3] */workI[8]=0; /* count_y */workI[9]=0; /* initydone 1 == initialisation for y-axis done*/workI[10]=0; /* start y 1 == starting init + scan for y-axis*/workI[11]=0; /* stop2halfx 1 == received stop @ second half of cylcus x-axis*/workI[12]=0; /* stop2halfy 1 == received stop @ second half of cylcus y-axis*/workI[13]=0; /* startpointingy 1 == startpointingy mode is acive...Transition from scan mode to zero (0,0) */workI[14]=0; /* pointingmode 1 == pointing mode is active == feelers are pointing */

52

workI[15]=0; /* stoppointing 1 == start stoppointingmode to return from pointingpos to zero */workI[16]=0; /* pointingxdone 1 == pointing done ==> x-axis @ xdes */workI[17]=0; /* pointingydone 1 == pointing done ==> y-axis @ ydes */workI[18]=0;

/* startpointingxfirst 1 == set when first startpointing x is set to 1...First axis moves to zero .. second axis moves to max ampl. */

workI[19]=0;/* startpointingyfirst 1 == set when first startpointing y is set to 1...

First axis moves to zero .. second axis moves to max ampl. */workI[20]=0;

/* startpointingx2 1 == set when part 2 of pointing starts == from standard pos. (zero or max ampl.) to xdes */workI[21]=0;

/* startpointingy2 1 == set when part 2 of pointing starts == from standard pos. (zero or max ampl.) to ydes */

for (i=0; i<21; i++){workR[i]=0;}

}


int_T i,tend,tenddiv2,tstarty,count_x,count_y,initxdone,initydone,startx,starty,stoppointing;int_T stop2halfx,stop2halfy,startpointingx,startpointingy,pointingmode,pm_tendx,pm_tendy;int_T ta,tv,at[3],pointingxdone,pointingydone,startpointingxfirst,startpointingyfirst,startpointingx2,startpointingy2;real_T a,ax,ay,aold_x,aold_y,tstarty_r,tar,tvr,xdes,ydes,v;

InputRealPtrsType in1 = ssGetInputPortRealSignalPtrs(S,0); /*at[6]*/InputRealPtrsType in2 = ssGetInputPortRealSignalPtrs(S,1); /*aa[6]*/InputRealPtrsType in3 = ssGetInputPortRealSignalPtrs(S,2); /*at_init[4]*/InputRealPtrsType in4 = ssGetInputPortRealSignalPtrs(S,3); /*scan mode? [1]*/InputRealPtrsType in5 = ssGetInputPortRealSignalPtrs(S,4); /*pointing mode [1]*/InputRealPtrsType in6 = ssGetInputPortRealSignalPtrs(S,5); /*pointing angle? [1]*/

real_T *y1 = ssGetOutputPortRealSignal(S,0); /*output acceleration x*/real_T *y2 = ssGetOutputPortRealSignal(S,1); /*output velocity x*/real_T *y3 = ssGetOutputPortRealSignal(S,2); /*output position x*/real_T *y4 = ssGetOutputPortRealSignal(S,3); /*output acceleration y*/real_T *y5 = ssGetOutputPortRealSignal(S,4); /*output velocity y*/real_T *y6 = ssGetOutputPortRealSignal(S,5); /*output position y*/

int_T *workI = ssGetIWork(S);real_T *workR = ssGetRWork(S);

count_x=workI[0];count_y=workI[8];startx=workI[2];starty=workI[10];initxdone=workI[3];initydone=workI[9];stop2halfx=workI[11];stop2halfy=workI[12];startpointingx=workI[1];startpointingy=workI[13];pointingmode=workI[14];stoppointing=workI[15];pointingxdone=workI[16];pointingydone=workI[17];startpointingxfirst=workI[18];startpointingyfirst=workI[19];startpointingx2=workI[20];startpointingy2=workI[21];

aold_x=workR[12];aold_y=workR[15];

if (pointingmode ==0) {

if (initxdone == 0 && initydone ==0 && startx == 0 && startpointingx ==0 && startpointingy == 0) {workI[2]=*in4[0]; startx=workI[2]; /* Wait for start scan mode */if (*in5[0] == 1) { /* start pointing mode from position zero*/

workR[20]=*in6[0]; /* Require desired pointing angle and save it*/workI[1]=1; startpointingx=1;workI[13]=1; startpointingy=1;workI[20]=1; startpointingx2=1;workI[21]=1; startpointingy2=1;}

}

if (initxdone ==0 && startx == 1 && initydone == 0 && startpointingx ==0) { /*== init procedure for x-axis. Starts first. */

53

if (count_x == 0 ) {for (i=0; i<6; i++) { /* Getting New Input Values */

workR[i]=*in1[i]; /* = at[6] */workR[i+6]=*in2[i];} /* = aa[6] */

for (i=0; i<4; i++) {workI[i+4]=*in3[i];} /* = at_init[0..3] => workI[4..7] */

}

if (count_x < workI[5] ){a=-workR[6];} /* a=ab*/

else if (count_x >= workI[5] && count_x < workI[6]){a=workR[7];} /* a= 0*/

else if ( count_x >= workI[6] && count_x-1 <= workI[7] ){a=-workR[8];} /* a=-ab*/

else if ( count_x >= workI[7] ){a=0.0;} /* a=-ab*/

/* ============= integrators ================================================= */workR[13]=workR[13]+aold_x/stime;workR[14]=workR[14]+*y2/stime;

/* ========== Reset Integrators ==============================================*/if (count_x == 0 || count_x == workI[7] ) /*Reset Integrator velocity*/

{workR[13]=0;}if (count_x == 0) /*Reset Integrator position*/

{workR[14]=0;}

/* ======= genereren outputs ================================================ */*y1=a;*y2=workR[13]; /* v */*y3=workR[14]; /* p */workR[12]=a; /*Storing old value of a*/

/* ======= ophogen count_x ================================================ */

if ( count_x<workI[7] ){ (workI[0])++;}

if ( count_x>=workI[7] ){ workI[0]=0; count_x=0; /*count_x=0*/

a=0.0; workR[12]=a; *y1=a;workI[3]=1; initxdone=1;}

} /* end homing x procedure*/

if (initydone == 0 && (startx == 1 || starty ==1) && startpointingy == 0 ) { /*== init procedure for y-axis. Starts second. */

tstarty_r=workR[1]+0.5*(workR[2]-workR[1]); /*ta+0.5tv == time [countsx] to start init y and count_y*/tstarty=floor(tstarty_r+0.5);if (count_x == tstarty) {

starty=1; workI[10]=starty;}

if (starty ==1) {if (count_y < workI[5] )

{a=-workR[6];} /* a=ab*/else if (count_y >= workI[5] && count_y < workI[6])

{a=workR[7];} /* a= 0*/else if ( count_y >= workI[6] && count_y-1 <= workI[7] )

{a=-workR[8];} /* a=-ab*/else if ( count_y >= workI[7] )

{a=0.0;} /* a=-ab*/

/* ============= integrators ================================================= */workR[16]=workR[16]+aold_y/stime; /* v_y*/workR[17]=workR[17]+*y5/stime; /* p_y*/

/* ========== Reset Integrators ==============================================*/if (count_y == 0 || count_y == workI[7] ) /*Reset Integrator velocity*/

{workR[16]=0;}

/* ======= genereren outputs ================================================ */*y4=a; /*ay*/*y5=workR[16]; /*vy*/*y6=workR[17]; /*py*/workR[15]=a; /*Storing old value of a*/

/* ======= ophogen count_y ================================================ */

if ( count_y<workI[7] ){ (workI[8])++; }

if ( count_y>=workI[7] )

54

{ workI[8]=0; count_y=0; /*count_y=0*/a=0.0; workR[15]=a; *y4=a;workI[9]=1; initydone=1;}

} /* end starty ==1*/} /* end init y procedure*/

if ( initxdone ==1) { /*start scan for x-axis*/

tend=workR[5]; /* end time cylcus in count_xs */tenddiv2=floor((workR[5]/2)+0.5); /* +0.5 == rounding */

if (count_x == 0 || count_x == tenddiv2) {if (*in4[0] ==0 ) {

startx=0; workI[2]=startx;} /*Stop scan mode if *in4[0] = 0 */if (*in5[0] ==1 ) {

startpointingx=1; workI[1]=startpointingx;workR[20]=*in6[0]; /* Require desired pointing angle and save it*/if (startpointingy == 0) {

startpointingxfirst=1; workI[18]=1;} /* x-axis starts pointing first and x-axis will return to zero*/else if (startpointingy ==1) {

count_x=0; workI[0]=0; /* reset counter*/workR[18]=workR[14];} /* saving current amplitude in memory*/

} /*Stop scan mode and start pointing mode */}

if (startx == 0 || (startpointingx ==1 && startpointingxfirst ==1)) { /* stop scan mode and return to zero (0,0)*/

if (count_x == tenddiv2) {workI[0]=0; count_x=0; /*count_x=0*/stop2halfx=1; workI[11]=1;}

if (count_x < workI[5] ){a=-workR[6];} /* a=ab*/

else if (count_x >= workI[5] && count_x < workI[6]){a=workR[7];} /* a= 0*/

else if ( count_x >= workI[6] && count_x-1 <= workI[7] ){a=-workR[8];} /* a=-ab*/

else if ( count_x >= workI[7] ){a=0.0;} /* a=-ab*/


/* ========== Reset Integrators ==============================================*/if (count_x == 0 || count_x == workI[7] ) /*Reset Integrator velocity*/

{workR[13]=0;}

/* ======= genereren outputs ================================================ */if (stop2halfx == 0) {

a=-a;}

*y1=a;*y2=workR[13]; /* v */*y3=workR[14]; /* p */workR[12]=a; /*Storing old value of a*/

/* ======= ophogen count_x ================================================ */

if ( count_x<workI[7] ){ (workI[0])++;}

if ( count_x>=workI[7] ){ workI[0]=0; count_x=0; /*count_x=0*/

a=0.0; workR[12]=a; *y1=a; /* a=0*/workR[13]=0.0; *y2=0.0; /* v=0*/workR[14]=0.0; *y3=0.0; /* p=0*/workI[3]=0; initxdone=0;workI[11]=0; stop2halfx=0;}

} /*END startx=0 == stop scan*/

else if (startx ==1 && startpointingx == 0) { /*START scan mode*//* ======= Calculating Acc as functions of count_xs ========================== */if ( count_x < workR[1] )

{ ax=workR[6];} /* a= -ab*/else if ( count_x >= workR[1] && count_x < workR[2] )

{ ax=workR[7];} /* a= 0*/else if ( count_x >= workR[2] && count_x < workR[3] )

{ ax=workR[8];} /* a= ab*/else if ( count_x >= workR[3] && count_x < workR[4] )

{ ax=workR[9];} /* a= 0*/

55

else if ( count_x >= workR[4] && count_x-1 <= tend){ ax=workR[10];} /* a= -ab*/

/* ============= integrators x-axis ================================================= */workR[13]=workR[13]+aold_x/stime; /* v_x*/workR[14]=workR[14]+*y2/stime; /* p_x*/

/* ========== Reset Integrators ==============================================*/if (count_x == 0 || count_x == tenddiv2 ) /*Reset Integrator v_x*/

{workR[13]=0;}

/* ======= genereren outputs ================================================ */*y1=ax;*y2=workR[13];*y3=workR[14];workR[12]=ax; /*Storing old value of ax*/

/* ======= ophogen count_x ================================================ */if ( count_x<tend )

{ (workI[0])++;}if ( count_x>=tend )

{ workI[0]=0;}} /*end startx ==1*/

} /*end initxdone ==1*/

if (initydone ==1) { /*start scan Y axis*/

if ((count_y == 0 || count_y == tenddiv2) && initxdone ==1 && startpointingy ==0) {if (startx == 0) {

starty=0; workI[10]=starty;} /*Stop scan mode if *in4[0] = 0 */if (*in5[0] ==1) {

startpointingy=1; workI[13]=startpointingy;workR[20]=*in6[0]; /* Require desired pointing angle and save it*/if (startpointingx == 0) {

startpointingyfirst=1; workI[19]=1;} /* y-axis starts pointing first and y-axis will return to zero*/else if (startpointingx ==1) {

count_y=0; workI[8]=0; /* reset counter*/workR[19]=workR[17];} /* saving current amplitude in memory*/

} /*Stop scan mode and start pointing mode */}

if (starty == 0 || (startpointingy ==1 && startpointingyfirst ==1)) { /* stop scan mode and return 2 zero*/

if (count_y == tenddiv2) {workI[8]=0; count_y=0; /*count_y=0*/stop2halfy=1; workI[12]=stop2halfy;}

if (count_y < workI[5] ){ay=-workR[6];} /* a=ab*/

else if (count_y >= workI[5] && count_y < workI[6]){ay=workR[7];} /* a= 0*/

else if ( count_y >= workI[6] && count_y-1 <= workI[7] ){ay=-workR[8];} /* a=-ab*/

else if ( count_y >= workI[7] ){ay=0.0;} /* a=-ab*/

/* ============= integrators y-axis ================================================= */workR[16]=workR[16]+aold_y/stime; /* v_y*/workR[17]=workR[17]+*y5/stime; /* p_y*/

/* ========== Reset Integrators ==============================================*/if (count_y == 0 || count_y == tenddiv2 ) /*Reset Integrator v_y*/

{workR[16]=0;}

/* ======= genereren outputs ================================================ */if (stop2halfy == 0) {

ay=-ay;}

*y4=ay;*y5=workR[16];*y6=workR[17];workR[15]=ay; /*Storing old value of ay*/

/* ======= ophogen count_y ================================================ */

if ( count_y<workI[7] ){ (workI[8])++;}

if ( count_y>=workI[7] ){ workI[8]=0; count_y=0; /*count_y=0*/

56

ay=0.0; workR[15]=ay; *y4=ay; /* a=0*/workR[16]=0.0; *y5=0.0; /* v=0*/workR[17]=0.0; *y6=0.0; /* p=0*/workI[9]=0; initydone=0;workI[12]=0; stop2halfy=0;}

} /*END stop scan loop*/

else if (starty == 1 && startpointingy == 0) { /*START scan mode*//* ======= Calculating Acc as functions of count_y ========================== */if ( count_y < workR[1] )

{ ay=workR[6];} /* a= -ab*/else if ( count_y >= workR[1] && count_y < workR[2] )

{ ay=workR[7];} /* a= 0*/else if ( count_y >= workR[2] && count_y < workR[3] )

{ ay=workR[8];} /* a= ab*/else if ( count_y >= workR[3] && count_y < workR[4] )

{ ay=workR[9];} /* a= 0*/else if ( count_y >= workR[4] && count_y-1 <= tend)

{ ay=workR[10];} /* a= -ab*/


/* ========== Reset Integrators ==============================================*/if (count_y == 0 || count_y == tenddiv2 ) /*Reset Integrator v_y*/

{workR[16]=0;}

/* ======= genereren outputs ================================================ */*y4=ay;*y5=workR[16];*y6=workR[17];workR[15]=ay; /*Storing old value of ay*/

/* ======= ophogen count_y ================================================ */if ( count_y<tend )

{ (workI[8])++; }if ( count_y>=tend )

{ workI[8]=0;}} /*START scan mode*/

} /*END initydone == 1*/

if (startpointingx == 1 && startpointingy == 1) {

if (initxdone ==0 || initydone ==0) {if (startpointingxfirst == 0) {

initxdone=0; workI[3]=0;} /*initydone has been resetted in end of 'returning 2 zero' loop*/if (startpointingyfirst == 0) {

initydone=0; workI[9]=0;} /*initxdone has been resetted in end of 'returning 2 zero' loop*/}

if (startpointingxfirst == 0) {startpointingx2=1; workI[20]=1; /*x-axis @ max ampl. AND y-axis @ zero*/if (startpointingyfirst ==1 && initydone ==0) {

startpointingy2=1; workI[21]=1;}}

if (startpointingyfirst == 0) {startpointingy2=1; workI[21]=1; /*x-axis @ zero AND y-axis @ max ampl.*/if (startpointingxfirst ==1 && initxdone ==0) {

startpointingx2=1; workI[20]=1;}}

if (startpointingx2 ==1 && pointingxdone == 0) { /*start transition from standard pos. to desired angle*/

xdes=ampl*sin(fabs(workR[20])/360*2*PI)-workR[18]; /*workR[20]=alpha_des and workR[18]=current ampl. */

/* ==== Calculating ta (Constant Acc phase) and tv (Constant Vel phase) ===== */tar=sqrt(fabs(xdes)/ab);

if (vb<ab*tar) /* v bound violated ? */{tar=vb/ab;}

v=ab*tar;tvr=(fabs(xdes)-ab*tar*tar)/v; /* Const Velocity Time */

ta=floor(tar*1000+0.5); /*rounding*/tv=floor(tvr*1000+0.5);

57

at[0]=ta; at[1]=ta+tv; at[2]=ta+tv+ta;

if (count_x < at[0] ){a= ab;} /* a=ab*/

else if (count_x >= at[0] && count_x < at[1]){a=0.0;} /* a= 0*/

else if ( count_x >= at[1] && count_x-1 <= at[2] ){a=-ab;} /* a=-ab*/

else if ( count_x >= at[2] ){a=0.0;} /* a=-ab*/


/* ======= genereren outputs ================================================ */if (xdes < 0) {

a=-a;}


/* ======= ophogen count_x ================================================ */

if ( count_x<at[2] ){ (workI[0])++;}

if ( count_x>=at[2] ) {workI[0]=0; count_x=0; /*count_x=0*/a=0.0; workR[12]=a; *y1=a; /* a=0*/workR[13]=0.0; *y2=0.0; /* v=0*/workR[18]=workR[14]; /* saving current ampl.*/pointingxdone=1; workI[16]=pointingxdone;}

} /*END if startpointingx2 ==1*/

if (startpointingy2 ==1 && pointingydone == 0) { /*start transition from standard pos. to desired angle*/

ydes=ampl*cos(fabs(workR[20])/360*2*PI)-workR[19];

/* ==== Calculating ta (Constant Acc phase) and tv (Constant Vel phase) ===== */tar=sqrt(fabs(ydes)/ab);


v=ab*tar;tvr=(fabs(ydes)-ab*tar*tar)/v; /* Const Velocity Time */


at[0]=ta; at[1]=ta+tv; at[2]=ta+tv+ta;

if (count_y < at[0] ){a= ab;} /* a=ab*/

else if (count_y >= at[0] && count_y < at[1]){a=0.0;} /* a= 0*/

else if ( count_y >= at[1] && count_y-1 <= at[2] ){a=-ab;} /* a=-ab*/

else if ( count_y >= at[2] ){a=0.0;} /* a=-ab*/


/* ======= genereren outputs ================================================ */if (ydes < 0) {

a=-a;}

*y4=a;*y5=workR[16];*y6=workR[17];workR[15]=a; /*Storing old value of ay*/

/* ======= ophogen count_y ================================================ */

58

if ( count_y<at[2] ){ (workI[8])++;}

if ( count_y>=at[2] ) {workI[8]=0; count_y=0; /*count_x=0*/a=0.0; workR[15]=a; *y4=a; /* a=0*/workR[16]=0.0; *y5=0.0; /* v=0*/workR[19]=workR[17]; /* saving current amplitude in memory*/pointingydone=1; workI[17]=pointingydone;}

} /*END if startpointingx2 ==1*/} /*END if startpointingx && startpointingy ==1*/

if (pointingxdone ==1 && pointingydone ==1) {pointingxdone=0; workI[16]=0;startx=0; workI[2]=0;starty=0; workI[10]=0;startpointingx=0; workI[1]=0;startpointingx2=0; workI[20]=0;startpointingxfirst=0; workI[18]=0;pointingydone=0; workI[17]=0;startpointingy=0; workI[13]=0;startpointingy2=0; workI[21]=0;startpointingyfirst=0; workI[19]=0;pointingmode=1; workI[14]=1; /*set pointing mode */

}

} /*END pointingmode==0 loop*/

else if (pointingmode ==1) { /*pointing mode....Wait for new pointing angle*/

if (startpointingx2 ==0 && startpointingy2 ==0) {/* saving new alpha_des only once @ start of pointingmode==1 loop*/if (*in5[0] ==0) { /*stop pointingmode*/

stoppointing=1; workI[15]=1;startpointingx2=1; workI[20]=1;startpointingy2=1; workI[21]=1;}

else if (workR[20] != *in6[0]) { /* new pointing angle ??*/workR[20]=*in6[0];ydes=ampl*cos(fabs(workR[20])/360*2*PI); /* new desired absolute y-pos*/

if (workR[19] >= 0.0) { /* current y-pos. >0 ?? */if (ydes >= workR[19]) { /* current y=pos*/

startpointingxfirst=1; workI[18]=1; /* ynew >= current y => Start moving x-axis first*/startpointingx2=1; workI[20]=1;} /* Start x*/

if (ydes < workR[19]) { /* current y=pos*/startpointingyfirst=1; workI[19]=1; /* Start moving y-axis first*/startpointingy2=1; workI[21]=1;} /* Start y*/

}else if (workR[19] < 0.0) { /* current y-pos. < 0 ?? */

if (ydes >= workR[19]) { /* current y=neg*/startpointingyfirst=1; workI[19]=1; /* Start moving y-axis first*/startpointingy2=1; workI[21]=1;} /* Start y*/

if (ydes < workR[19]) { /* current y=pos*/startpointingxfirst=1; workI[18]=1; /* ynew >= current y => Start moving x-axis first*/startpointingx2=1; workI[20]=1;} /* Start x*/

} /*END*/} /* END if new pointing angle*/

} /*END spx2==0 && spy2==0*/

if (startpointingx2 ==1 && pointingxdone == 0 && stoppointing ==0) {/*start transition from old pos. to new desired angle*/

xdes=ampl*sin(fabs(workR[20])/360*2*PI)-workR[18];/*relative new x-pos.. workR[20]=alpha_des and workR[18]=current ampl. */





at[0]=ta; at[1]=ta+tv; at[2]=ta+tv+ta;pm_tendx=floor(at[2]/DIV+0.5);

if (count_x < at[0] )

59

{a= ab;} /* a=ab*/else if (count_x >= at[0] && count_x < at[1])

{a=0.0;} /* a= 0*/else if ( count_x >= at[1] && count_x-1 <= at[2] )

{a=-ab;} /* a=-ab*/else if ( count_x >= at[2] )

{a=0.0;} /* a=-ab*/



a=-a;}


/* ======= ophogen count_x ================================================ */


if ( count_x>=at[2] ) {workI[0]=0; count_x=0; /*count_x=0*/a=0.0; workR[12]=a; *y1=a; /* a=0*/workR[13]=0.0; *y2=0.0; /* v=0*/workR[18]=workR[14]; /* saving current ampl.*/pointingxdone=1; workI[16]=pointingxdone;}

} /*END if startpointingx2 ==1*/

if (startpointingy2 ==1 && pointingydone == 0 && stoppointing ==0) { /*start transition from old pos. to new desired angle*/

ydes=ampl*cos(fabs(workR[20])/360*2*PI)-workR[19];





at[0]=ta; at[1]=ta+tv; at[2]=ta+tv+ta;pm_tendy=floor(at[2]/DIV+0.5);







a=-a;}


/* ======= ophogen count_y ================================================ */


60


} /*END if startpointingy2 ==1*/

if (startpointingxfirst == 0 && startpointingy2 ==1 && stoppointing ==0) {/* CASE 1 =Determine when x-axis starts (y-axis already running)*/

if (count_y == pm_tendy) {startpointingx2=1; workI[20]=1;}

}else if (startpointingyfirst == 0 && startpointingx2 ==1 && stoppointing ==0) {

/* CASE 2= Determine when y-axis starts (x-axis already running)*/if (count_x == pm_tendx) {

startpointingy2=1; workI[21]=1;}}

if (pointingxdone ==1 && pointingydone ==1 && stoppointing ==0) { /*both axis ready with pointing move; Reset flags*/pointingxdone=0; workI[16]=0;startpointingx=0; workI[1]=0;startpointingx2=0; workI[20]=0;startpointingxfirst=0; workI[18]=0;pointingydone=0; workI[17]=0;startpointingy=0; workI[13]=0;startpointingy2=0; workI[21]=0;startpointingyfirst=0; workI[19]=0;}

if (stoppointing ==1) { /* Start transition from pointing pos. to zero */

if (startpointingx2 ==1 && pointingxdone == 0) { /*start transition for x-axis*/

xdes=0.0-workR[18];





at[0]=ta; at[1]=ta+tv; at[2]=ta+tv+ta;pm_tendx=floor(at[2]/DIV+0.5);

if (count_x < at[0] ){a= ab;} /* a=ab*/

else if (count_x >= at[0] && count_x < at[1]){a=0.0;} /* a= 0*/

else if ( count_x >= at[1] && count_x-1 <= at[2] ){a=-ab;} /* a=-ab*/

else if ( count_x >= at[2] ){a=0.0;} /* a=-ab*/



a=-a;}


/* ======= ophogen count_x ================================================ */


if ( count_x>=at[2] ) {

61

workI[0]=0; count_x=0; /*count_x=0*/a=0.0; workR[12]=a; *y1=a; /* a=0*/workR[13]=0.0; *y2=0.0; /* v=0*/workR[18]=workR[14]; /* saving current ampl.*/pointingxdone=1; workI[16]=pointingxdone;}

} /* END if startpointingx2 ==1 && pointingxdone == 0 */

if (startpointingy2 ==1 && pointingydone == 0 ) { /*start transition for y-axis*/

ydes=0.0-workR[19];





at[0]=ta; at[1]=ta+tv; at[2]=ta+tv+ta;pm_tendy=floor(at[2]/DIV+0.5);







a=-a;}


/* ======= ophogen count_y ================================================ */



} /*END if startpointingy2 ==1*/

if (pointingxdone ==1 && pointingydone ==1) { /*transition to zero ready -> Stop pointing mode and reset flags*/pointingmode=0; workI[14]=0;stoppointing=0; workI[15]=0;startpointingx2=0; workI[20]=0;startpointingy2=0; workI[21]=0;pointingxdone=0; workI[16]=0;pointingydone=0; workI[17]=0;

}

} /* END if stoppointing == 1*/} /*END else if pointingmode == 1 */

} /*END mdlOutput*/

/* Function: mdlTerminate =====================================================* Abstract:* No termination needed, but we are required to have this routine.

62

*/static void mdlTerminate(SimStruct *S){}


63

trajectory planning for the halobject - tu/e · trajectory planning for the halobject nagelmaeker,...

Documents