Creation and choreography of robotic bird

Creation and choreography of robotic bird

Alexander Larsson Supervisor: Lars Asplund Examiner: Lars Asplund

Creation and choreography of robotic bird

Alexander Larsson Supervisor: Lars Asplund Examiner: Lars Asplund

In this thesis a dancing swan robot is made utilizing already existing components. We built the neck from stacking servo-actuators on top of each-other. The legs are a beam construction with 1 degree of freedom. The wings have 4 degrees of freedom, of which 3 in the shoulder joint. From the shoulder joint a beam protrudes outwards to an actuator in the elbow joint which in turn has a beam attached representing the outer wing. Communications are handled by USB transceivers making a regular personal computer capable of directly interfacing with the actuators. In order to choreograph the motion a recording solution was implemented utilizing the sensors in the servo-actuators to periodically sample the position of the joints. A control program is developed to handle both playback of recorded motion and a prerecorded audio track. While recording the control program periodically polls the engines about their current position and stores the time and position in a table. When playing the animation the control program periodically polls the table to see if any samples are due, in which case it instructs the actuator to move towards the sample point.

Contents

I Introduction 3

A Problem description . . . 3 B Related works . . . 4

B.1 Stem Structure for Flower Robot Using Shape Memory Alloy (SMA) Actuators . . . 4 B.2 Construction and evaluation of a robot dance system . . . 4 B.3 Nao . . . 4

B.4 Scen och sinnesproduktion . . . 4

II Mechanics 5 A Servo actuators . . . 5

B Set point function . . . 5

C Regulator . . . 7 D Neck . . . 8 D.1 Plates . . . 8 D.2 SMA . . . 10 D.3 Serial of servos . . . 12 D.4 Solution . . . 13 E Wings . . . 14

E.1 Plastic girders . . . 14

E.2 Metal rods . . . 14

E.3 Solution . . . 15 F Legs . . . 15 III Programming 18 A Movement Programming . . . 18 A.1 Requirements . . . 18 A.2 Solution . . . 19 B Controller . . . 20 B.1 Record thread . . . 21 B.2 Control thread . . . 22 B.3 Audio thread . . . 23

IV Electronics 24 A Serial communication . . . 24 B Solution . . . 27

V Conclusion 27

VI Appendix 31

A Controller source code 31

B Spring position optimization 42

C AVR-bridge source code 45

I. Introduction A. Problem description

The task is to build a robot to "`dance"' in conjunction with music. As such a suitable robot frame should be built and eventually decorations could be applied.

The wings should have four degrees of freedom each, the legs should have at least one degree of freedom.

The neck should have at least three degrees of freedom.

A movement programming solution should be developed that allows recording of motions performed and editing of recorded movements.

Playback of recorded programs should be accompanied and synchronized to a previously recorded audio le.

B. Related works

B.1 Stem Structure for Flower Robot Using SMA Actuators

In Development of Stem Structure for Flower Robot Using SMA Actuators [1] SMA is utilized as actuators to create a bendable stem for a robotic ower. SMA is a material that when heated creates a force to return to it's initial conguration. Shape Memory Alloys -Unconventional Actuators describes the material in greater detail [2].

B.2 Construction and evaluation of a robot dance system

In Construction and evaluation of a robot dance system [3] the authors utilize a pre-built robot. The motion is generated by creating a database of valid dance steps and then concatenating moves from the database. By this method several dances could be realized. B.3 Nao

Project Nao [4] was started in 2005 with the goal of making available a humanoid robot to the general public at an aordable cost.

The robot is equipped with among other things two cameras, a sonar and an inertia sensor. The robot is programmed by connecting boxes representing actions and ow control, and with the right combinations dancing could be achieved.

B.4 Scen och sinnesproduktion

In the borderline between the digital and the living Åsa Unander-Scharin creates a har-mony between dierent art-forms which characterizes the work of Scen- och sinnesproduction

1_.

Past projects include a singing tree titled Ombra mai fù (2007), a mecatronic dancer

titled Petrusjkas tårar2 _{and a solo choreography on a ABB industrial robot titled Orfeus}

Klagan3_.

II. Mechanics

The mechanics of the robot is modular with reusability in mind. The construction kit FAC-system [5] forms the core upon which all other components are tted. Should a part be in need of moving all that is required is to unfasten the component and then fasten the component in its new position. This is of course limited by available holes.

A. Servo actuators

The servo actuators we utilize are from the Robotis Dynamixel series.

All engines receive instructions over a half-duplex multi-drop bus which allows us to control the actuators and read the state of the actuators. As it is a multi-drop bus we may freely add as many devices as we please. Figure 1 displays a couple of devices connected on the same bus. The conguration is however not a daisy-chain, as both slots are internally connected.

Enumerating all the dierent parameters to control would be rather much, as such we limit ourselves to the parameters we actually use, which are presented in table I.

B. Set point function

We observed that when we changed Mm from zero to anything above zero while k was

set the engine would make a fast turn towards the last angle it was instructed to go to, regardless of moving speed.

Therefore we believe the name moving speed does not refer to the maximum allowed speed, but rather it is an argument to the set-point function.

2 _{Petrusjkas tears}

Fig. 1

Three devices connected through a shared multi-drop bus. TABLE I

Engine parameters utilized

Symbol Field Description

α Target angle The actuator will try to attain this angle k Moving speed The slope of set-point function

Mm Maximum torque The maximum torque the engine will produce

Bit rate The bit rate on the bus

β Output shaft angle The present angle of the output shaft C Compliance margin Accepted deviation from α P Punch Minimum current supplied to engine K Compliance Slope Length of regulator slope

Therefore we formed the hypothesis that at every time t there is a reference value r(t) which can be described using equation 1. The function k(t) represents the speed setting found in table I at the time t.

In order to corroborate our hypothesis we did two simple tests. In our rst test we instructed the engine to move to an arbitrary position at an arbitrary nonzero speed. Once it had stopped moving we set Mm to zero and then proceeded to manually change the

position of the actuator. Now if we were to change Mm to a nonzero value we would expect

the actuator to move at its maximum speed towards the position we originally instructed it to move to. When turning on torque we observed that the engine did move very fast towards the position we originally told it to go to.

In our second test we instructed the engine to move to an arbitrary position. Once it had stopped moving we set Mm to zero and then proceed to manually change the position

of the actuator. According to our hypothesis if we now read the current angle β and tell the engine to go to beta at speed zero (immediately) the actuator would stay in position and apply a torque if necessary to stay in that position.

When we turned torque back on we observed that the actuator made no movement and trying to change its position resulted in the actuator creating a torque to remain in position. Based on these two experiments we believe that our hypothesis is correct and equation 1 accurately describes the behavior of the set-point function.

r0(t) = k(t) (α(t) > r(t)) r0(t) = −k(t) (α(t) < r(t)) r0(t) = 0 (α(t) = r(t)) r(t) = α(t) (k(t) = 0) r(0) = β(0) (1) C. Regulator

In our system we have r(t) representing the position we want our engine to be in and β(t) representing the position it is in currently. From these we can determine the error at time t according to equation 2.

The internal regulator in the actuator is a proportional regulator 3 with a compliance margin, no regulation is done if the error is less than this value. When the error leaves the compliance margin the output torque jumps up to the punch value and as the error increases the output torque increases in a linear fashion until the maximum torque is reached. Should the error continue to grow beyond this point the actuator will continue to output the maximum torque until it shuts o due to overheating.

e(t) = β(t) − r(t) (2) M (t) = e(t) × P (3) M (t) = P (t) + (e(t) − C(t)) × Mm(t) K(t) (e(t) > C(t)) M (t) = −P (t) − (e(t) + C(t)) × Mm(t) K(t) (−e(t) > C(t)) M (t) = Mn(t) (e(t) > C(t) + K(t)) M (t) = −Mn(t) (e(t) < −C(t) − K(t)) M (t) = 0 (|e(t)| < C(t)) (4) (5) D. Neck D.1 Plates

By making a stack of neck segments these segments can be rotated independently of each-other to achieve a bendable neck.

In the rst test a series of segments were stacked and a wire was utilized to pull down a side of the plate, thus bending the stack.

Fig. 2

Early neck part iteration. The top is somewhat rounded.

Early iterations consisted of a rounded tube, see gure 2. However as these had problems rotating the construction moved towards a rounded top and a rounded bottom, such as gure 3

However we had problems with the stack not bending evenly. Most bending was done at the place where the resistance was the least until that section hit the end of it's span, then the bending continued at the joint where the least resistance was encountered barring the places that could not move further.

Figure 4 illustrates the wanted behavior of the stack that all sections rotate the same, while gure 5 illustrates the observed eect that most rotation was in sections.

Fig. 3

Late neck part iteration. Both the top and bottom are rounded.

A solution to this could have been to introduce springs between segments as regulators. As segments rotated in relation to each-other the restorative force from the springs would change the resistance to rotation causing the rotation to be better distributed over the segments.

D.2 SMA

SMA is as mentioned before a material that can be deformed while it is cold and tries to return to its original shape once heated. [2] So by utilizing SMA wires similar to pull a side of a hose representing the neck we would get a bendable neck. This is the same approach used

Fig. 4

Intended behavior, all parts rotating the same.

Fig. 5

Fig. 6

The distance l between the output shaft and the center of mass is considered to be the distance between the output shafts.

in [1]. However, as [2] claims the typical actuation range of SMA to about 4 % elongation the solution was discarded due to not allowing sucient actuation range. Should SMA with a bigger span somehow be procured, the problem of hysteresis and slow response time could be used to motivate why this approach is unsuitable for this problem.

The main advantage for using this method to create a bend in an object would be the uniform force distribution, creating a very smooth bend on a suitable continuous surface. D.3 Serial of servos

Another approach could be to utilize a series of servo engines, similar to the robot snakes already built.

If we accept the notation that actuators are indexed, with 0 donating the top of the stack and ascending numbers being beneath. Furthermore it should also be noted that the bottom actuator rotates the stack and its force is perpendicular to the force created by the mass of the above actuators. While this actuator will still need to counteract the weight of all actuators above it the restorative force will come from the actuator frame rather than

TABLE II

Distance between output shafts

RX to RX 6.7 cm RX to AX 6.7 cm AX to AX 9.5 cm the engine.

The actuators have a mass m and and its center of mass is located at a distance l from the output shaft of the below actuator. We consider the center of mass to be located on the output shaft, see gure 6. The mass of frames bolts and cabling are neglected in these calculations. We measured the distance on all our servo-actuators and the distance is listed in table II. This allows us to calculate the external torque Me acting on every actuator k bar

the bottom actuator is according to equation 6. Note that this equation is in a worst case scenario where the neck is held perpendicular to the gravity vector.

Mek = k X i=1 mi× li (6) D.4 Solution

In our solution we made a series of servo-actuators. The stronger and heavier RX-28 servo-actuators were on the bottom because they needed to provide the most torque. At the top of the neck where the need for forces were not so great lighter and weaker servo-actuators were utilized.

The nal neck has a length of 0.5 m and consist of four RX-28 actuators at the bottom and three AX-12 actuators on the top. A picture of the neck can be found at gure 7.

The advantage of this solution is that we get many degrees of freedom and good control over the movement. The disadvantage is however the weight of all actuators as well as the current consumption and bus trac to control all actuators.

Fig. 7

The final neck implementation.

E. Wings

The targeted wingspan of the constructed bird is 2.0 m , the lower limit of the wingspan on the wikipedia entry [6] . The lower limit was targeted in order to minimize the momentum required by the wing servos. As the width of the body frame is about 0.3 m the external torque Me of one wing with a uniformly distributed mass m is according to equation 7.

Me = m ×

0.200 − 0.30

4 = m × 0.425 (7)

E.1 Plastic girders

The rst idea was to build the wings out of a frame of free-formed plastic girders around 0.3 m long. However this solution was later discarded due to the low stiness of plastic. E.2 Metal rods

Another approach would be to build a frame out of metal rods. Aluminium would be a prudent choice considering it is hard and light, Wikipedia lists aluminium with a density ρ of 2700 kg/m3_{, thus using a rod with a radius r and length l will yield a mass m according}

Fig. 8

The final wing implementation.

to equation 8.

m = ρ × r2× l (8)

E.3 Solution

In our solution we use two aluminium rods connected with a thin aluminium sheet. At the body base plate 3 servoactuators are attached providing 3 degrees of freedom. At the end of this stack the aluminium rod aggregate are fastened. At the end of the that another servoactuator is attached, which in turn is connected to a nal rod aggregate. A picture of this may be found in gure 8

F. Legs

The mass of the body was at the last measurement before legs were assembled 1.9 kg. With the projected one meter leg length divided on two parts the estimated torque required in the joints would be as to equation 9, where Me represents external torque applied and α

represents the angle of the joint, with 0 being standing upright. 1.9kg × 9.82N/kg × 0.5m × sinα

This function peaks at 90 degrees yielding approximately 4.66 Nm. 1.9kg × 9.82N/kg × 0.5m × 1

2 ≈ 4.66N m (10)

Our rst attempt at an solution was to utilize a standing lamp with three Degrees of Freedom (DOF), one of them being for the lamp. By removing the lamp and wires a link mechanism with two degrees of freedom was created.

In order to reduce the power requirements on the engines we looked into utilizing elon-gation and torsional springs for load balancing.

The lamp structure had two sets of springs from start and four fasteners. By calculating the expected external torque from the body at 20 sampling points equally spaced from zero to 90 degrees and comparing it to the expected torque applied by a spring at the same angle we tried to come up with a good combination springs and fasteners.

In our method we held the fasteners on the arm xed and the spring rest length xed and varied the spring constant and non-xed fastener position. We used an iterative algorithm that moved the fastener in one direction for as long as the change resulted in improvement, when the change made the solution worse than the previous iteration the direction was changed. The metric used for how good a particular solution was to take the dierence between the torque exerted by the spring and the torque exerted by the body squared and sum this up over all sample points. Lower values are better. See equation 11 where Λ is the score of the solution, Me is the external torque applied at sample i and Ms is the spring

torque applied at sample i.

Λ = 20 X i=1 (Mei− Msi) 2 ₍₁₁₎

We found out that this method was quite poor due to nding local minimum values of S. The result could vary quite a bit depending on the starting conditions. The solutions that we did nd were either were very poor due to having a large S, or being very poor due to

being very far away, the furthest solution placing the fastener several kilometers away from the leg. For completeness the source code for this can be found in appendix B.

We also looked into using torsional springs. These springs yield a restorative force that is proportional to the angular displacement. The maximum deviation between a linear function and equation 9 assuming same starting and ending points were graphically found to be approximately 20 %.

In order to simplify the problem of analytically nding the maximum deviation we normalize the external weight to one, thus yielding.

sinα ≈ 2a

π (12)

These two will be furthest apart from each-other when their derivatives are equal. d

dαsinα = cosα = 2

π (13)

Which yields the result in equation 14

α ≈ 0.880rad (14)

And thus the dierence in this point sin acos2

π −

2acos_π2

2 ≈ 0.2105 (15)

A deviation of 21% of maximum external torque is acceptable.

The lamp solution was later discarded in favor of a single link due to some problems encountered. The primary reason was that the top of the structure did not have a constant angle in relation to the ground plane. Going for a linked structure to eliminate the need for a 3rd actuator was the primary reason for going with the lamp in the rst place.

Fig. 9

Final leg mechanism. The body is lowered by rotating the lower joints.

• High weight, weight of structure exceeded engine limit • Bulky and therefore dicult to transport

• Dicult placing of parts due to links obscuring the good fastening points • Low stiness of links

We nally settled on the link mechanism displayed in gure 9 with one degree of freedom.

III. Programming A. Movement Programming

A.1 Requirements

The requirements for the movement programming interface is given in the below table. Lines in parentheses are not part of the specications but are implied from it.

• playback movement during capture on already dened joints • playback captured data

• (serial port communication) • allow editing of captured data

• playback audio track while recording or playing data • work on both PC and Macintosh

The rst iteration in movement programming was a simple spreadsheet. While functional it was very hard to work with. So we looked into utilizing some already existing software.

Poser/Daz3D is a animation studio that lets you create What You See Is What You Get (WYSIWYG) movies by manipulating characters. So by importing a model of the robot into any of these programs we would have a WYSIWYG editor for the movement, provided the data could be extracted.

Blender [7] is an opensource animation studio. It has a built in curve editor and built-in script support for python.

A.2 Solution

The solution used to solve this program is to utilize Blender [7]. As Blender software has a built in curve editor we used that for editing motion curves, along with a WYSIWYG animation interface. Playback of an audio track may be done synchronized to playing back an animation in the program. The remainder was solved by the built-in python [8] scripting interface.

In order to communicate easily the python serial communications library pyserial was utilized, while sound was being handled pyaudio. What we had to implement in python was:

• Control code for engines • Algorithm for playback

• Algorithm for recording • Audio playback

B. Controller

The rst controller was a micro-controller connected between a personal computer and the servo-actuators. When the position of a joint should be recorded the computer sends a message to subscribe to periodic updates of the joint position. Actuation is done by periodically instructing the actuators to reach a position. We observed that in order for the movement to look smooth approximately 10 to 20 move instructions needed to be issued every second for every actuator.

We rst used a ATMega128 micro-controller connected with a bit rate of 56700 bits per second to a computer, the connection to the actuators were on 1 000 000 bits per second. The computer sent commands to the microcontrol to subscribe to events or to forward commands to actuators. This particular solution worked ne with low amount of actuators, however as the amount of actuators grew this could no longer be kept up.

As a response the control algorithm was reworked to work on movement speed instead, only sending instructions at breakpoints of rotating speed. Even with this modication the throughput was too low.

In order to increase throughput the micro-controller was bypassed to allow direct control from a personal computer. The code found in appendix C was used to turn the micro-controller into a bridge. The control algorithm was split into two threads. One thread for initializing and recording, and one thread for control. The control thread was later split into one control thread for every bus to achieve even higher throughput.

Fig. 10

Recording thread flowchart

B.1 Record thread

The actions of the recording thread are displayed in gure 10. In order to conserve space and simplify the owchart actions whose order is insignicant or obvious are aggregated into one action.

The thread starts of by initializing all resources that will be used. Then proceeds to take a time-stamp in order to know when the recording (or playback started) Following this the thread starts the control thread and audio playback thread. This thread then proceeds

Fig. 11

Control thread flowchart

to periodically poll the actuators for their present position and store the position for later playback. The thread continues to poll until it is instructed to stop, after which it frees all resources allocated earlier.

B.2 Control thread

The control thread manages playback of stored positions. The thread starts by looking up from which time it should start playback from and forwards to that time. Once that has been done the thread periodically polls the time to see if any sample point has been reached, in which case the appropriate engine is instructed to move towards the next sample at a speed that will make it reach the next breakpoint as close to the intended time as

Fig. 12

Audio thread flowchart

possible.The owchart for this thread is in gure 11 B.3 Audio thread

The audio thread has only one purpose, to playback an audio le. As such it opens the specied audio le, initializes a audio interface, seeks to the playback start position and keeps lling the out-buer until no data remains or it is instructed to stop. Figure 12 illustrates this with a owchart.

IV. Electronics A. Serial communication

[9] describes a Universal Asynchronous Receiver Transmitted (UART) as a device for converting between serial communication and parallel communication. The parallel side is often connected to a computer bus. When a computer sends data to the UART it will begin to transmit the data over the serial line.

The AX series uses a one wire4 _{half-duplex link and 5V logic. The RX series uses a two}

wire half-duplex link and RS-485 voltage levels. Due to the dierence in voltages and wires used they can not be connected to the same network. Some methods in overcoming this were considered.

The manual for the servo-actuators [10] states that the actuators use 8 data bits, one start bit and one stop bit. Thus to transmit one byte of useful data over the serial link 10 bits must be used.

To get a rough idea of the throughput over one network we make some approximations. These are:

• No jitter

• Perfect oscillators • 10 bytes sent • 10 bytes received

• Default time return delay (250 uS) • FTDI and USB introduces no latency

Thus the time to transmit (T, in seconds) is the time to send 20 bytes with the bitrate B

plus the return time delay of 250 uS.

T = 0.00025 +20 × 10

B (16)

Thus 1/T is the amount of instructions that can be fullled per second. Table III is a table of some bit rates and their corresponding throughput.

TABLE III

Instruction rate for different bitrates

Bit rate (bps) Instructions per second

9 000 47 19 200 94 57 600 269 115 200 504 200 000 800 250 000 952 400 000 1333 500 000 1539 1 000 000 2222 innite 4000

The later of the approximations turned out to be very wrong. Because our control algorithm waited for a reply after every command it had to wait for the FTDI receive buer latency timer to time out before receiving. [11]. As the default timeout is 16ms this would limit the throughput to 62.5 messages per second. For the readers convenience table IV lists maximum throughput at dierent FTDI timeouts.

As can be seen from table IV the maximum throughput we can hope to achieve if we choose to wait for a reply after every message is 1000.

Using a 57600 bps link we ran a test to measure how many instructions we could get through the bus. For the FTDI tests we took a time-stamp then proceeded to read the angle of an engine a xed amount of times. Then we took another time-stamp and calculated how long time it took to send the data. We then considered the time to send and receive a reply to be the total time spend divided by successful reads. While we did not encounter any

TABLE IV

Instruction rate for FTDI timeout

FTDI timeout (ms) Instructions per second 16 (default) 62.5 15 66.7 14 71.4 13 77.0 12 83.3 11 90.1 10 100.0 9 111.1 8 125.0 7 142.9 6 166.8 5 200.0 4 250.0 3 333.3 2 500.0 1 1000.0

unsuccessful reads in this test it would have been penalized by forcing extra read attempts until successful.

In another test we connected the TxD and RxD pin on a serial cable and transmitted 20 dummy bytes over it. The results are displayed in table V.

TABLE V Time for reads

FTDI timeout (ms) Iterations Time

16 100 1.6589

2 1000 6.0098

1 1000 5.0599

Serial 1000 4.375

We also looked at how to increase throughput by increasing link speed.

One approach investigated was to run the two networks, and interconnect them in a chain so they always contained the same data. This particular approach is not working due to two drivers driving the RxD pin 13. When no data is sent the drivers try to keep the line high. When data is high the line is high for logical 1 and low for logical 0. The result of

TABLE VI Time for reads

Bit Rate Iterations Time 115 200 1000 2.485 500 000 1000 1.313 1 000 000 1000 1.000

this is that when a logical 1 is transmitted the line is high. When a logical 0 is transmitted the line is halfway between high and low, which was insucient to bring the line low. Table VII shows the resulting input on TxD as a function of the states of the individual drivers along with the wanted input on TxD. From the table it can be seen that the input on TxD is driver 1 ∨ driver 2 and the wanted output is driver 1 ∧ driver 2. Therefore by connecting the drivers to an and gate such as gure 14 data should correctly be transmitted.

TABLE VII

Input to RxD as a function of individual drivers

Driver 1 Driver 2 Input on TxD Wanted input on TxD

Low Low Low Low

Low High High Low

High Low High Low

High High High High

Another solution could be to use two independent networks, each having their own dedicated UART arrangement.

B. Solution

Our solution involves having two FTDI transceivers directly connected to a computer. This solution was selected due to it's general low cost, both in hardware and development time. In order to increase throughput the FTDI read timeout was reduced from the default 16ms to 1ms.

Fig. 13

Two drivers are driving the RxD pin, causing a failure to receive data.

Fig. 14

In this thesis a dancing swan robot is made utilizing already existing components. We built the neck from stacking servo-actuators on top of each-other. The legs are a beam construction with 1 degree of freedom. The wings have 4 degrees of freedom, of which 3 in the shoulder joint. From the shoulder joint a beam protrudes outwards to an actuator in the elbow joint which in turn has a beam attached representing the outer wing. Communications are handled by USB transceivers making a regular personal computer capable of directly interfacing with the actuators. In order to choreograph the motion a recording solution was implemented utilizing the sensors in the servo-actuators to periodically sample the position of the joints. A control program is developed to handle both playback of recorded motion and a prerecorded audio track. While recording the control program periodically polls the engines about their current position and stores the time and position in a table. When playing the animation the control program periodically polls the table to see if any samples are due, in which case it instructs the actuator to move towards the sample point.

The nal construction can with reasonable accuracy duplicate the recorded movement. Suggestions for further work could be to for instance use a real time operating system, or make the construction fault-tolerant. Bypassing the USB bus would be a small improvement as well.

References

[1] Development of Stem Structure for Flower Robot Using SMA Actuators, Hao Lei Huang, Suk-Ho Park, Jong-Oh Park, Proceedings of the 2007 IEEE International Conference on Robotics and Biomimetics December 15 -18, 2007, Sanya, China

[2] Shape Memory Alloys - unconventional actuators, Vasina, M. Solc, F. Hoder, K.

[3] Construction and evaluation of a robot dance system, Shinozaki, K.; Iwatani, A.; Nakatsu, R. [4] http://www2.aldebaran-robotics.com/eng/Nao.php [5] http://www.facsystem.se/default_eng.asp [6] http://en.wikipedia.org/wiki/muteswan [7] http://www.blender.com [8] www.python.com

[9] Time-triggered communication with UARTs Elmenreich, W.; Delvai, M.; Factory Com-munication Systems, 2002. 4th IEEE International Workshop on

[10] http://www.robotis.com/zbxe/?module=le&act=procFileDownload&le_srl=5448&sid=892169ec0cc789252a4b905cd2f6c3c6 [11] http://www.ftdichip.com/Support/Knowledgebase/index.html?an232b_04adjlatency.htm

VI. Appendix Appendices

A. Controller source code

Some lines are too long to t on one line. The symbol -> is used to indicate that this line is a continuation of the previous line.

import Blender import s e r i a l import time import a r r a y import t h r e a d i n g t h r e a d l i g h t l y import pyaudio import wave import s y s

from Blender import ∗ from s t r u c t import ∗ EnginePin = [ 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ] c a l i b = ->[ −15.93 , −15.93 , −15.93 , −15.93 , −15.93 , −15.93 , −15.93 , −15.93 , −15.93 , −15.93 , −15.93 , −15.93 , −15.93 , −15.93 , −15.93 , −15.93 , −15.93 , −15.93 , −15.93 , −15.93 ,] -> iponame = ("R1" ,"R2" ,"R3" ,"R4" ," L1 " ," L2 " ," L3 " ," L4 " ,"N1" ,"N2" ,"N3" ,"N4" ,"N5" ,"N6" ,"N7" ,"N8" ,"H1" ,"H2" ,"H3") network = [ 0 , 0 , 1 , 1 , 0 , 0 , 1 , 1 , 0 , 0 , 1 , 1 , 1 , 0 , 0 , 0 , 0 , 1 , 1 , 1 , 0 , 0 , 0 , 0 , 0 ] s e n d t a b l e = [ 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ] r c v t a b l e = [ 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ] nEngines = 19 nEngineDelay = 0 . 0 1 BlendToAx = 0 . 0 3 f p s = 25 bRun = 0 sermutax = t h r e a d i n g . Lock ( ) sermutrx = t h r e a d i n g . Lock ( ) pathmutex = t h r e a d i n g . Lock ( ) RXBusTime = 0 AXBusTime = 0

d e f r e l i a b l e r e a d ( bus , mtex , msg , eng , t r i e s =10) : g l o b a l bRun

r e p l y = a r r a y . a r r a y ( 'B' , [ ] ) t r i g g e r s = 0 f o r i i n range ( t r i e s ) : bus . w r i t e ( msg ) s e n d t a b l e [ eng ] = s e n d t a b l e [ eng ] + 1 r e p l y = r e p l y + r e p l y + a r r a y . a r r a y ( 'B' , bus . read ( 8 ) ) w h i l e l e n ( r e p l y ) > 0 and t r i g g e r s < 2 : i f r e p l y . pop ( 0 ) == 2 5 5 : t r i g g e r s = t r i g g e r s + 1 e l s e : t r i g g e r s = 0 i f t r i g g e r s == 2 : i f l e n ( r e p l y ) < 6 : r e p l y = r e p l y + a r r a y . a r r a y ( 'B' , bus . read (6− l e n ( r e p l y ) ) ) i f l e n ( r e p l y ) > 5 : i f r e p l y [ 2 ] & 112 == 0 : mtex . r e l e a s e ( ) r c v t a b l e [ eng ] = r c v t a b l e [ eng ] + 1 r e t u r n r e p l y [ 3 ] + r e p l y [ 4 ] ∗ 2 5 6 mtex . r e l e a s e ( ) r e t u r n −1

d e f r e l i a b l e s e n d ( bus , mtex , msg , eng , t r i e s =3) : g l o b a l bRun mtex . a c q u i r e ( ) r e p l y = a r r a y . a r r a y ( 'B' , [ ] ) t r i g g e r s = 0 f o r i i n range ( t r i e s ) : bus . w r i t e ( msg ) s e n d t a b l e [ eng ] = s e n d t a b l e [ eng ] + 1 r e p l y = r e p l y + r e p l y + a r r a y . a r r a y ( 'B' , bus . read ( 6 ) ) w h i l e l e n ( r e p l y ) > 0 and t r i g g e r s < 2 : i f r e p l y . pop ( 0 ) == 2 5 5 : t r i g g e r s = t r i g g e r s + 1 e l s e : t r i g g e r s = 0 i f t r i g g e r s == 2 :

i f l e n ( r e p l y ) < 4 : r e p l y = r e p l y + a r r a y . a r r a y ( 'B' , bus . read (4− l e n ( r e p l y ) ) ) i f l e n ( r e p l y ) > 3 : i f r e p l y [ 2 ] != 0 : p r i n t ' e r r o r ' + s t r ( r e p l y [ 2 ] ) i f r e p l y [ 2 ] & 80 == 0 : r c v t a b l e [ eng ] = r c v t a b l e [ eng ] + 1 mtex . r e l e a s e ( ) r e t u r n mtex . r e l e a s e ( ) bRun = 0

p r i n t ' Shutdown due to e n g i n e ' + s t r ( eng ) t o r q u e ( 2 5 4 , 0 )

d e f moveenginespeed ( engine , pos , speed ) : g l o b a l serax , sermutax , sermutrx , s e r r x

l e n g t h = 7 i n s t r u c t i o n = 3 o f f s e t = 30

checksum = ~( e n g i n e+l e n g t h+i n s t r u c t i o n+o f f s e t+pos /256+ pos%256+speed /256+ speed %256)%256

cmd = pack ( 'BBBBBBBBBBB' , 2 5 5 , 2 5 5 , engine , l e n g t h , i n s t r u c t i o n , o f f s e t , pos %256 , pos /256 , speed %256 , speed ->/256 , checksum ) i f e n g i n e == 2 5 4 : sermutax . a c q u i r e ( ) s e r a x . w r i t e (cmd) sermutax . r e l e a s e ( ) sermutrx . a c q u i r e ( ) s e r r x . w r i t e (cmd) sermutrx . r e l e a s e ( ) r e t u r n s t i m e = time . time ( ) i f network [ e n g i n e ] == 1 : r e l i a b l e s e n d ( s e r r x , sermutrx , cmd , e n g i n e ) e l s e : r e l i a b l e s e n d ( serax , sermutax , cmd , e n g i n e )

p r i n t t i m e r

p r i n t time . time ( )−s t i m e

d e f l e d ( engine , pos ) :

g l o b a l serax , sermutax , sermutrx , s e r r x l e n g t h = 4

i n s t r u c t i o n = 3 o f f s e t = 25

checksum = ~( e n g i n e+l e n g t h+i n s t r u c t i o n+o f f s e t+pos ) %256

cmd = pack ( 'BBBBBBBB' , 2 5 5 , 2 5 5 , engine , l e n g t h , i n s t r u c t i o n , o f f s e t , pos , checksum )

i f e n g i n e == 2 5 4 : sermutax . a c q u i r e ( ) s e r a x . w r i t e (cmd) sermutax . r e l e a s e ( ) sermutrx . a c q u i r e ( ) s e r r x . w r i t e (cmd) sermutrx . r e l e a s e ( ) r e t u r n i f network [ e n g i n e ] == 1 : r e l i a b l e s e n d ( s e r r x , sermutrx , cmd , e n g i n e ) e l s e : r e l i a b l e s e n d ( serax , sermutax , cmd , e n g i n e ) d e f r e a d a n g l e ( e n g i n e ) :

g l o b a l serax , sermutax , s e r r x , sermutrx l e n g t h = 4

i n s t r u c t i o n = 2 o f f s e t = 36 r l e n = 2

checksum = ~( e n g i n e+l e n g t h+i n s t r u c t i o n+o f f s e t+r l e n ) %256

cmd = pack ( 'BBBBBBBB' , 2 5 5 , 2 5 5 , engine , l e n g t h , i n s t r u c t i o n , o f f s e t , r l e n , checksum )

i f network [ e n g i n e ] == 1 :

r e t u r n r e l i a b l e r e a d ( s e r r x , sermutrx , cmd , e n g i n e ) e l s e :

r e t u r n r e l i a b l e r e a d ( serax , sermutax , cmd , e n g i n e )

d e f t o r q u e ( engine , t o r q ) :

l e n g t h = 5 i n s t r u c t i o n = 3 o f f s e t = 34

checksum = ~( e n g i n e+l e n g t h+i n s t r u c t i o n+o f f s e t+t o r q /256+ t o r q %256)%256

cmd = pack ( 'BBBBBBBBB' , 2 5 5 , 2 5 5 , engine , l e n g t h , i n s t r u c t i o n , o f f s e t , t o r q %256 , t o r q /256 , checksum )

i f e n g i n e == 2 5 4 : sermutax . a c q u i r e ( ) s e r a x . w r i t e (cmd) sermutax . r e l e a s e ( ) sermutrx . a c q u i r e ( ) s e r r x . w r i t e (cmd) sermutrx . r e l e a s e ( ) r e t u r n i f network [ e n g i n e ] == 1 : r e l i a b l e s e n d ( s e r r x , sermutrx , cmd , e n g i n e ) e l s e : r e l i a b l e s e n d ( serax , sermutax , cmd , e n g i n e ) c l a s s CtrlThread ( t h r e a d i n g . Thread ) : d e f __init__ ( s e l f , c t l , smap ) : t h r e a d i n g . Thread . __init__ ( s e l f ) s e l f . c t r l l i s t = c t l s e l f . servomap = smap d e f run ( s e l f ) : g l o b a l bRun , stime , f p s , c a l i b bframe = Blender . Get ( ' curframe ' ) r p o s = [ ] p r i n t ' c t r l s t a r t ' f o r sub i n range ( l e n ( s e l f . c t r l l i s t ) ) : r p o s . append ( 0 ) f o r i i n range ( l e n ( s e l f . c t r l l i s t [ sub ] . b e z i e r P o i n t s ) ) : i f s e l f . c t r l l i s t [ sub ] . b e z i e r P o i n t s [ i ] . pt [ 0 ] > bframe : r p o s [ sub ] = i break ; f o r sub i n range ( l e n ( i p o l i s t ) ) :

newpos = i p o l i s t [ sub ] . b e z i e r P o i n t s [ r p o s [ sub ] ] . pt [ 1 ] # r a d i a n s engpos = i n t (( −( newpos )−c a l i b [ sub ] ) / 0 . 0 3 )

w h i l e bRun :

time . s l e e p ( 0 . 0 0 1 )

ctime = time . time ( ) g e t c u r r e n t timestamp f o r sub i n range ( l e n ( s e l f . c t r l l i s t ) ) :

i f l e n ( s e l f . c t r l l i s t [ sub ] . b e z i e r P o i n t s ) > r p o s [ sub ]+1 and s e l f . c t r l l i s t [ ->sub ] . b e z i e r P o i n t s [ r p o s [ sub ] ] . pt [ 0 ] < ( ctime − s t i m e ) ∗ f p s : time to change p o i n t s

l a s t t i m e = s e l f . c t r l l i s t [ sub ] . b e z i e r P o i n t s [ r p o s [ sub ] ] . pt [ 0 ] / f p s -> s e c o n d s l a s t t i m e = ctime − s t i m e l a s t p o s = s e l f . c t r l l i s t [ sub ] . b e z i e r P o i n t s [ r p o s [ sub ] ] . pt [ 1 ] -> r a d i a n s r p o s [ sub ] = r p o s [ sub ] + 1

newtime = s e l f . c t r l l i s t [ sub ] . b e z i e r P o i n t s [ r p o s [ sub ] ] . pt [ 0 ] / f p s -> s e c o n d s

newpos = s e l f . c t r l l i s t [ sub ] . b e z i e r P o i n t s [ r p o s [ sub ] ] . pt [ 1 ] r a d i a n s s l o p e = abs ( newpos − l a s t p o s ) /( newtime − l a s t t i m e ) rad / s

slopemod = i n t ( 0 . 2 ∗ s l o p e / 0 . 0 1 1 6 6 9 6 5 ) c o n v e r s i o n i f slopemod > 5 0 0 :

slopemod = 500 i f slopemod < 1 0 :

slopemod = 10

engpos = i n t (( −( newpos )−c a l i b [ sub ] ) / 0 . 0 3 ) p r i n t " t r i g g e r " + s t r ( engpos )

i f s e l f . c t r l l i s t [ sub ] . b e z i e r P o i n t s [ r p o s [ sub ] ] . pt [ 0 ] < ( ctime − -> s t i m e ) ∗ f p s :

p r i n t " frame dropped "

i f not s e l f . c t r l l i s t [ sub ] . b e z i e r P o i n t s [ r p o s [ sub ] ] . pt [ 0 ] < ( ctime − -> s t i m e ) ∗ f p s and l e n ( s e l f . c t r l l i s t [ sub ] . b e z i e r P o i n t s ) > r p o s [ sub ] + 1 :

moveenginespeed ( s e l f . servomap [ sub ] , engpos , slopemod ) ctime = time . time ( )

c l a s s CmdThread ( t h r e a d i n g . Thread ) : d e f __init__ ( s e l f , cmd) :

t h r e a d i n g . Thread . __init__ ( s e l f ) s e l f . cmd = cmd

d e f run ( s e l f ) :

g l o b a l bRun , EnginePin , nEngines , i p o l i s t , stime , fp s , serax , s e r r x , pathmutex , sermut , -> s e n d t a b l e , btime

s e r = s e r i a l . S e r i a l ( ' / dev / t t y . u s b s e r i a l −A7003N1t ' , 5 7 6 0 0 ) # open f i r s t s e r i a l p o r t s e r = s e r i a l . S e r i a l ( ' / dev /ttyUSB0 ' , 1 0 0 0 0 0 0 ) open f i r s t s e r i a l p o r t

s e r = s e r i a l . S e r i a l ( ' \ \ . \COM2' , 2 3 0 4 0 0 ) # open f i r s t s e r i a l p o r t s e r . timeout = 0 . 0 2 3

s e r 2 = s e r i a l . S e r i a l ( ' / dev /ttyUSB1 ' , 1 0 0 0 0 0 0 ) open f i r s t s e r i a l p o r t s e r 2 = s e r i a l . S e r i a l ( ' \ \ . \COM8' , 2 3 0 4 0 0 ) # open f i r s t s e r i a l p o r t s e r 2 . timeout = 0 . 0 2 3

p r i n t s e r . p o r t s t r check which p o r t was r e a l l y used

i p o l i s t = [ ] r e c o r d = 0 s e r a x = s e r i f r e a d a n g l e ( 1 ) != −1: s e r r x = s e r 2 e l s e : s e r r x = s e r s e r a x = s e r 2 s e n d t a b l e [ 1 ] = 0 t o r q u e ( 2 5 4 , 0 ) s e r a x . c l o s e ( ) s e r r x . c l o s e ( ) c l a s s S e r i a l T h r e a d ( t h r e a d i n g . Thread ) : d e f run ( s e l f ) :

g l o b a l bRun , EnginePin , nEngines , i p o l i s t , stime , fp s , serax , s e r r x , pathmutex , sermut , -> s e n d t a b l e , btime , AXBusTime , RXBusTime

s e r = s e r i a l . S e r i a l ( ' / dev / t t y . u s b s e r i a l −A7003N1t ' , 5 7 6 0 0 ) # open f i r s t s e r i a l p o r t s e r = s e r i a l . S e r i a l ( ' / dev /ttyUSB0 ' , 1 0 0 0 0 0 0 ) open f i r s t s e r i a l p o r t

s e r = s e r i a l . S e r i a l ( ' \ \ . \COM2' , 2 3 0 4 0 0 ) # open f i r s t s e r i a l p o r t s e r . timeout = 0 . 4

s e r 2 = s e r i a l . S e r i a l ( ' / dev /ttyUSB1 ' , 1 0 0 0 0 0 0 ) open f i r s t s e r i a l p o r t s e r 2 = s e r i a l . S e r i a l ( ' \ \ . \COM8' , 2 3 0 4 0 0 ) # open f i r s t s e r i a l p o r t s e r 2 . timeout = 0 . 4

p r i n t s e r . p o r t s t r check which p o r t was r e a l l y used

i p o l i s t = [ ] r e c o r d = 0 s e r a x = s e r i f r e a d a n g l e ( 1 ) != −1: s e r r x = s e r 2 e l s e :

s e r r x = s e r s e r a x = s e r 2 s e n d t a b l e [ 1 ] = 0

f o r sub i n iponame :

i p o = Ipo . Get ( sub ) . getCurve (" RotX ") i f not i p o :

i p o = Ipo . Get ( sub ) . addCurve (" RotX ") i p o l i s t . append ( i p o ) p r i n t EnginePin f o r i i n range ( nEngines ) : i f EnginePin [ i ] == 0 : i n i t a n g = r e a d a n g l e ( i +1) i f i n i t a n g >= 0 and i n i t a n g <= 1 0 2 4 : moveenginespeed ( i +1, i n i t a n g , 0 ) i f EnginePin [ i ] : r e c o r d = 1 f o r i i n range ( nEngines ) : t o r q u e ( i +1 ,1023 − EnginePin [ i ] ∗ 1 0 2 3 ) i f EnginePin [ 1 6 ] == 0 : t o r q u e ( 1 7 , 6 0 0 ) i f EnginePin [ 1 7 ] == 0 : t o r q u e ( 1 8 , 6 0 0 ) i f EnginePin [ 1 6 ] == 0 : t o r q u e ( 1 9 , 6 0 0 ) f o r j i n range ( nEngines ) : l e d ( j +1, 1) p r i n t ' l e d ' + s t r ( j +1)

bframe = Blender . Get ( ' curframe ' ) wpos = [ ] f o r sub i n range ( l e n ( i p o l i s t ) ) : wpos . append ( 0 ) f o r i i n range ( l e n ( i p o l i s t [ sub ] . b e z i e r P o i n t s ) ) : i f i p o l i s t [ sub ] . b e z i e r P o i n t s [ i ] . pt [ 0 ] > bframe : wpos [ sub ] = i break ; d i v i d e workload on t h r e a d s

axnetwork = [ ] rxnetwork = [ ] axmap = [ ] rxmap = [ ]

f o r sub i n range ( l e n ( i p o l i s t ) ) :

i f network [ sub +1] == 0 : i f on AX network axnetwork . append ( i p o l i s t [ sub ] ) axmap . append ( sub +1)

e l s e :

rxnetwork . append ( i p o l i s t [ sub ] ) rxmap . append ( sub +1)

btime = Blender . Get ( ' curframe ' ) / f p s f o r i i n range ( nEngines ) :

i f EnginePin [ i ] :

w h i l e l e n ( i p o l i s t [ i ] . b e z i e r P o i n t s ) > wpos [ i ] and i p o l i s t [ i ] . b e z i e r P o i n t s [ ->wpos [ i ] ] . pt [ 0 ] < ( btime ) ∗ f p s :

wpos [ i ] = wpos [ i ] + 1

c t r l a x = CtrlThread ( axnetwork , axmap ) c t r l r x = CtrlThread ( rxnetwork , rxmap ) aud = AudioThread ( ' . / juha . wav ' , 1 ) i f r e c o r d : time . s l e e p ( 4 . 0 ) s t a r t d e l a y = AudioThread ( ' . / s t a r t u p . wav ' , 0 ) s t a r t d e l a y . s t a r t ( ) s t a r t d e l a y . j o i n ( ) s t i m e = time . time ( ) s t i m e = s t i m e − btime aud . s t a r t ( ) c t r l a x . s t a r t ( ) c t r l r x . s t a r t ( ) w h i l e bRun : time . s l e e p ( 0 . 0 1 ) f o r i i n range ( nEngines ) : i f EnginePin [ i ] : p o s i t i o n = r e a d a n g l e ( i +1) ctime = time . time ( )

i f p o s i t i o n >= 0 : w h i l e l e n ( i p o l i s t [ i ] . b e z i e r P o i n t s ) > wpos [ i ] and i p o l i s t [ i -> ] . b e z i e r P o i n t s [ wpos [ i ] ] . pt [ 0 ] < ( ctime − s t i m e ) ∗ f p s : i p o l i s t [ i ] . d e l B e z i e r ( wpos [ i ] ) i p o l i s t [ i ] . append ( ( ( ctime−s t i m e ) ∗ fp s , −( p o s i t i o n ) ∗0.03 − -> c a l i b [ i ] ) ) wpos [ i ] = wpos [ i ] + 1 bRun = 0 c t r l a x . j o i n ( ) c t r l r x . j o i n ( ) aud . j o i n ( ) p r i n t time . c l o c k ( ) l e d ( 2 5 4 , 0 ) p r i n t ' c l o s i n g p o r t s ' s e r . c l o s e ( ) c l o s e p o r t s e r 2 . c l o s e ( ) p r i n t " c o n n e c t i o n " s t a t i s t i c s f o r i i n range ( 2 1 ) : i f s e n d t a b l e [ i ] > 0 : p r i n t ' ' p r i n t i p r i n t f l o a t ( r c v t a b l e [ i ] ) / f l o a t ( s e n d t a b l e [ i ] ) i f r e c o r d : f o r i p o i n i p o l i s t : i p o . c l e a n ( 0 . 0 5 ) c l a s s AudioThread ( t h r e a d i n g . Thread ) : d e f __init__ ( s e l f , afa , fwda ) :

t h r e a d i n g . Thread . __init__ ( s e l f ) s e l f . a f = a f a s e l f . fwd = fwda d e f run ( s e l f ) : g l o b a l bRun , btime chunk = 1024

wf = wave . open ( s e l f . af , ' rb ' ) p r i n t " p l a y i n g audio " + s e l f . a f

p = pyaudio . PyAudio ( )

open stream

stream = p . open ( format =

p . get_format_from_width ( wf . getsampwidth ( ) ) , c h a n n e l s = wf . g e t n c h a n n e l s ( ) , r a t e = wf . g e t f r a m e r a t e ( ) , output = True ) read data i f ( s e l f . fwd ) : wf . s e t p o s ( btime ∗ wf . g e t f r a m e r a t e ( ) ) data = wf . r e a d f r a m e s ( chunk ) pla y stream

w h i l e data != ' ' and bRun : stream . w r i t e ( data ) data = wf . r e a d f r a m e s ( chunk ) stream . c l o s e ( ) p . t e r m i n a t e ( ) i f s e l f . fwd : bRun = 0 t o r q u e ( 2 5 4 , 0 ) t o g g l e = 0

d e f event ( evt , v a l ) : the f u n c t i o n to handle i n p u t e v e n t s g l o b a l mystring , mymsg , EnginePin

i f e v t == Draw .ESCKEY:

Draw . Exit ( ) e x i t when u s e r p r e s s e s ESC r e t u r n

d e f button_event ( e v t ) : the f u n c t i o n to handle Draw Button e v e n t s g l o b a l t o g g l e , bRun , EnginePin

i f e v t == 1 :

i f t o g g l e == 0 : bRun = 1

S e r i a l T h r e a d ( ) . s t a r t ( ) e l s e : bRun = 0 t o g g l e = 1 − t o g g l e r e t u r n i f e v t == 2 : CmdThread ( 0 ) . s t a r t ( ) r e t u r n i f e v t > 1 5 :

EnginePin [ evt −16] = 1 − EnginePin [ evt −16]

d e f g u i ( ) : the f u n c t i o n to draw the s c r e e n g l o b a l t o g g l e , EnginePin

BGL. g l C l e a r C o l o r ( 0 , 0 , 1 , 1 )

BGL. g l C l e a r (BGL.GL_COLOR_BUFFER_BIT) BGL. g l C o l o r 3 f ( 1 , 1 , 1 )

Draw . Toggle (" Record " , 1 , 10 , 10 , 45 , 20 , t o g g l e , "A t o g g l e button ") Draw . Toggle (" Torque " , 2 , 60 , 10 , 45 , 20 , t o g g l e , "A t o g g l e button ") f o r i i n range ( 9 ) :

f o r j i n range ( 2 ) :

Draw . Toggle ( iponame [ i+j ∗ 9 ] , 16+ i+j ∗9 , 10+ j ∗50 , 30+ i ∗16 , 32 , 16 , EnginePin [ i+j ∗ 9 ] , " ->A t o g g l e button ")

Draw . Toggle ( iponame [ 1 8 ] , 16+18 , 10+1∗50 , 30+10∗16 , 32 , 16 , EnginePin [ 1 8 ] , "A t o g g l e button ")

Draw . R e g i s t e r ( gui , event , button_event ) r e g i s t e r i n g the 3 c a l l b a c k s

B. Spring position optimization

#i n c l u d e <s t d i o . h> #i n c l u d e <math . h> #d e f i n e PI 3 . 1 4 1 5 9 2 6 5 // s e t u p f l o a t f o r i g [ ] = { 0 . 0 5 f , 0 . 1 f } ; f l o a t armlen = 0 . 2 3 f ; f l o a t s t i f f = 3 2 0 0 . f ; f l o a t o l e n = 0 . 1 3 5 f ; f l o a t d e l t a ( f l o a t a , f l o a t b ) { i f ( a > b )

r e t u r n a − b ; r e t u r n b − a ; } f l o a t c a l c s c o r e ( ) { f l o a t s c o r e = 0 . f ; f o r ( i n t i = 0 ; i <21; i ++) { f l o a t armangle = (20− i ) ∗PI / 4 0 ; f l o a t armpos [ 2 ] ;

armpos [ 0 ] = −armlen ∗ s i n ( armangle ) ; armpos [ 1 ] = armlen ∗ c o s ( armangle ) ;

f l o a t s p r i n g l e n [ 3 ] ; s p r i n g l e n [ 0 ] = f o r i g [ 0 ] − armpos [ 0 ] ; s p r i n g l e n [ 1 ] = f o r i g [ 1 ] − armpos [ 1 ] ; s p r i n g l e n [ 2 ] = s q r t ( s p r i n g l e n [ 0 ] ∗ s p r i n g l e n [ 0 ] + s p r i n g l e n [ 1 ] ∗ s p r i n g l e n [ 1 ] ) ; f l o a t r a w f o r c e = s p r i n g l e n [ 2 ] ∗ s t i f f ; f l o a t s p r i n g a n g l e = atan ( s p r i n g l e n [ 0 ] / s p r i n g l e n [ 1 ] ) ; f l o a t attackmod = c o s ( s p r i n g a n g l e − armangle ) ; f l o a t bweight = s i n ( armangle ) ∗ 4 . 9 ; i f ( i == 0)

s t i f f = ( bweight ) / armlen / attackmod /( s p r i n g l e n [2] − o l e n ) ;

f l o a t t o r q u e = armlen ∗ attackmod ∗ ( s p r i n g l e n [2] − o l e n ) ∗ s t i f f ;

s c o r e += d e l t a ( torque , bweight ) ∗ d e l t a ( torque , bweight ) ;

p r i n t f ("% f \ t%f \n " , torque , bweight ) ; } p r i n t f ("\ n ") ; r e t u r n s c o r e ; } v o id main ( ) { f l o a t s c o r e = 9 9 9 9 . f ;

f l o a t p s c o r e = 9 9 9 9 . f ; i n t d i r = 1 ; f l o a t s t e p s i z e = 0 . 1 f ; c a l c s c o r e ( ) ; r e t u r n ; w h i l e ( s t e p s i z e > 0 . 0 0 0 1 ) // s o l u t i o n i t e r a t o r { f o r i g [ 0 ] += s t e p s i z e ∗ d i r ; do { p s c o r e = s c o r e ; s c o r e = c a l c s c o r e ( ) ; i f ( s c o r e < p s c o r e ) { f o r i g [ 0 ] += s t e p s i z e ∗ d i r ; // i f ( f o r i g [ 0 ] > 0 . 2 f ) // f o r i g [ 0 ] = 0 . 2 f ; } } w h i l e ( s c o r e < p s c o r e ) ; s c o r e = 9 9 9 9 9 9 9 9 9 . f ; f o r i g [ 1 ] += s t e p s i z e ∗ d i r ; do { p s c o r e = s c o r e ; s c o r e = c a l c s c o r e ( ) ; i f ( s c o r e < p s c o r e ) { f o r i g [ 1 ] += s t e p s i z e ∗ d i r ; // i f ( f o r i g [ 1 ] > 0 . 2 f ) // f o r i g [ 1 ] = 0 . 2 f ; // i f ( f o r i g [ 1 ] < 0 . f ) // f o r i g [ 1 ] = 0 . f ; } } w h i l e ( s c o r e < p s c o r e ) ; d i r = −d i r ; s t e p s i z e /=2. f ; } p r i n t f (" x : %f \ ty : %f \n " , f o r i g [ 0 ] , f o r i g [ 1 ] ) ;

p r i n t f (" c : %f " , s t i f f ) ;

}

C. AVR-bridge source code

#i n c l u d e <avr / i o . h> #i n c l u d e <avr / i n t e r r u p t . h> char Buff0 [ 3 2 ] ; char Buff1 [ 3 2 ] ; char Buff1R = 0 ; char Buff1W = 0 ; char Buff0R = 0 ; char Buff0W = 0 ; char nQueue = 0 ; vo i d i n i t _ u a r t ( ) { DDRD = DDRD | 8 ; DDRE = DDRE | 2 ; DDRA = 3 ;

UCSR0B |= (1 << RXEN0) + (1 << TXEN0) + (1 << RXCIE0) ;

UBRR0L = 0 ;

UCSR1B |= (1 << RXEN1) + (1 << TXEN1) + (1 << RXCIE1) + (1 << TXCIE1) ;

UBRR1L = 0 ; // 7 . 3 7 2 8 Osc . 56kBaud DDRB = 0xE5 ; } i n t main ( ) { i n i t _ u a r t ( ) ; DDRA = 1 ; PORTA = 0 ; s e i ( ) ; w h i l e ( 1 )

{

}

r e t u r n 0 ; }

ISR (USART1_TX_vect , ISR_NAKED) // TX 1 completed {

PORTA = 0 ; r e t i ( ) ; }

ISR (USART0_TX_vect , ISR_NAKED) // TX 00 completed {

r e t i ( ) ; }

ISR (USART0_RX_vect , ISR_NAKED) // we g e t s i g n a l {

PORTA = 1 ; nQueue++;

Buff1 [ Buff1W ] = UDR0; Buff1W = ( Buff1W + 1) & 3 1 ;

i f ( nQueue > 2)

UCSR1B |= (1 << UDRIE1) ;

r e t i ( ) ; }

ISR (USART1_RX_vect , ISR_NAKED) // we g e t s i g n a l {

Buff0 [ Buff0W ] = UDR1; Buff0W = ( Buff0W + 1) & 3 1 ;

UCSR0B |= (1 << UDRIE0) ;

r e t i ( ) ; }

{

UDR0 = Buff0 [ Buff0R ] ; Buff0R = ( Buff0R + 1) &31;

i f ( Buff0R == Buff0W )

UCSR0B ^= (1 << UDRIE0) ;

r e t i ( ) ; }

ISR (USART1_UDRE_vect, ISR_NAKED) // f e e d me ( 1 ) {

UDR1 = Buff1 [ Buff1R ] ; Buff1R = ( Buff1R + 1) &31;

i f ( Buff1R == Buff1W )

UCSR1B ^= (1 << UDRIE1) ;

nQueue−−;

r e t i ( ) ; }