A Chronicle of Fails

A Chronicle of Fails © GPL3+

A humanoid arm + hand prototype made of balsa wood

  • 997 views
  • 1 comment
  • 3 respects

Components and supplies

Ard due
Arduino Due
×1
A000057 featured
Arduino Leonardo
×1
11026 02
Jumper wires (generic)
×100
Balsa Wood parts
~75 main part on mechanical structure, have been made of balsa wood
×75
CAD Application
Any CAD Application (preferably with 3D modeling capabilities) can be used.
×1
Crafting Tools (Drills and Accessories)
Micro drill, drill bits, grinding and cutting accessories...
×1
Hand Tools (Screwdrivers, Cutters, etc.)
pliers, utility/hobby knifes, screwdrivers...
×1
Sg90 servo motor 180 degrees sg90 micro
SG90 Micro-servo motor
×14
SM-4303 Standard Servo Motor
×2
Flatwasher s w 1  59650 1510774396 ntw3qhvae4
OpenBuilds Flat Washer 1/4" Screw Size
×14
PCB 8x2 cm
×2
Header Pins
×192
Packaging Rubber Bands
lots of them are used since they loose elasticity in time.
×1
Fishing Ropes
meters of fishing ropes are used in parts
×1
PVC Tube, 6mm diameter
~1m by cutting into parts
×1
PVC Pipe 40mm Diameter
~10cm part on Upper Arm-Shoulder joint
×1
PVC Pipe 25mm Diameter
~10cm part on Upper Arm-Shoulder joint
×1
Glass Nails
mainly used on fingers for guiding fishing ropes.
×1
Plastic Cable Ties
×1
Staple Pins
used to fix some parts in more flexible way
×1
Paper Clips
mainly pulley bases and hangers are made of paper clips
×1

Apps and online services

About this project

Intro :: Autumn 2017

Even everybody prefers to share successful works and amazing prototypes, here I’m going to take the other way: Almost all hobby-grade robotic arm makers focus industrial type robotic arms, I intended to make a humanoid hand and arm assembly with the mechanicals and functionalities almost same as on the human body, but on the cheapest way as possible. But due to abilities of materials and components, it became a story of fails, however a story-of-fails is also a story-of-lessons learnt.

At first, I started to watch out and study on movements of my hand, as well as its dimensions, and even while walking around, so I took a significant risk that almost all the people on my neighborhood may think that I have a strong mental disease :)

The first design is completed in 2-3 months, then prototype is made by using cardboard and toothpicks. It was good to see whether mechanical approach is satisfactory and able to achieve the basic requirements.

Then I took a long break for thinking on better opinions.

First design had lots of parts on mechanical side, and it should be optimized. So, I decided to restart from scratch, and chose material at the beginning: Balsa Wood, but that choice would force me to end Project earlier than I think. Anyway, I can say that it worked well.

Autumn 2018 :: Restarted from Scratch

After deciding material, I restarted mechanical design by using mono-block parts as much as possible. All the Metacarpal / Proximal/ Medial / Distal “bones”, Trapezium and Wrist section are designed, virtually assembled and interacted with each other on computer. During that period, I also noticed that my current design is not able to hold up something. Why?

Because at the initial stage palm of hand was exactly flat, as same as most of the other robotic hands available. So it leads slipping out the object, even the fingers are closed.

However, palm of human hand is not so. If you hold your fingers straight and look from side, you may think that it’s flat. But when you look by facing to fingertips directly (you can also touch on top of your hand), it’s clear that your palm has a concave structure created by Metacarpal bones led by a concave Trapezium. By the way, when you close your fingers, they cover a spherical volume from all sides and together with your palm.

So I reflected that to my design: Trapezium is reshaped to have a concave face, consequently Palm became concave due to Metacarpals aligned by Trapezium. Just needed several trials by adjusting angles, and yes, it worked theoretically :)

At that period, I also would like to go beyond mechanical-only and started to think about actuation. But how? I had to use servo motors but even I use the smallest types, I had no place to locate them on Hand section including Wrist. I noticed that, I need 12 servos just for Hand and Wrist section to imitate all of their movements. In the meantime, I decided also how the “muscles” and “tendons” shall be imitated: After trials with few different materials, I found fishing ropes are the best “muscles” due to strength and flexibility, and packaging rubbers as “tendons” (we will also talk about them). During those trials, I also implemented “muscle guides” by using 6mm PVC tubes.

It was clear that arm sections should also be designed.But since Front Arm has twisting and levering movements, just one possible place remained for servos: Upper Arm.

And it would be good to actuate also Upper Arm, but establishing a universal joint on Upper Arm-Shoulder connection, would be extremely complicated and hard. Anyway Upper Arm might be levered only and whole structure might be turned by Shoulder, so I decided to limit mechanical structure at Shoulder.

First Problem :: Twisting the Wrist

After assembling the Hand and Wrist sections, I started to study on Front Arm, and immediately question came up: How can I twist my hand?

Perhaps till now you also never thought about that. Mechanically, twisting actions require a pivot connection. But how is it assembled on human body? The answer I seek was on a pages by Stan Prokopenko: https://www.proko.com/types-of-joints/

(Many thanks to him for that great guideline. In the meantime, I also learnt that twisting movements are called as “pronation”and “supination” on medics.)

So it’s clear that there’s a pivot connection between Ulna- and Radius-bones. But why that pivot is located on the back-end, on the elbow? Isn’t it something hard to do? And how could I imitate that mechanism?

As can be seen on Prokopenko’s animations, Ulna-and Radius-bones wrapped together around pivot. When the dedicated muscle stretches, Radius revolves onto Ulna so the Wrist will be twisted.

So I did exactly the same at first: I prepared a prototype by placing pivot onto the elbow-end, fixed both of Ulna and Radius to the Wrist, and finally wrapped them at elbow-end by using packaging rubbers.However, that configuration could not move smoothly even by manually. After a bit more careful look to the mechanism I took completely different way by reshaping Ulna and Radius completely: Ulna is designed to lever only and it’s fully separated from Wrist. Ulna-Radius pivot is relocated onto the middle section of Front Arm. Finally, Wrist is fixed only onto Radius. So, also the shapes of Ulna and Radius finally became similar to the real bones.

(On your Front Arm, that pivot is located on elbow end, because that’s the best place to protect pivot against any damage. If Front Arm bone(s) are fractured due to any impact, you can continue using it normally when healed completely. However, if pivot was placed at the middle, it would get damage in case of mechanical impacts, so possibly you would never be able to twist your hand anymore even it healed.)

After solving this issue, now I had to decide routing of “muscles”. The routing should not be effected by movement of Ulna and Radius, so it was clear that all muscle ropes should reach to actuators at Upper Arm by passing through Ulna-Radius pivot. So I sacrificed a pen to use its body as a hollow joint.

At the end, entire Humanoid Arm + Hand Mechanicals were able to perform almost all movements of the real one: Opening and closing Fingers as well as ability of holding something up, turning right-left and up-down at the Wrist, twisting the Front Arm nearly 270 degrees as well as levering it, levering the Upper Arm, and turning whole arm structure nearly 180-degrees at Shoulder.

Design v.2.1-Pronation of Radius

in actual, actuated by the real hand :)

Note that if I counted correctly, real human arm has 27 axes of movement from Shoulder to Hand:

  • Turning, levering and circling of Upper Arm = 3 axes
  • Levering and twisting Front Arm = 2 axes
  • Turning Wrist horizontally (right-left) and vertically(up-down) = 2 axes
  • Circling and levering Thumb Metacarpal = 2 axes
  • Levering Thumb Proximal and Distal = 2 axes
  • Levering Proximals, Medials and Distals of Index, Middle, Ring and Pinky Fingers = 3 x 4 = 12 axes
  • Closing and opening Index, Middle, Ring and Pinky Fingers on a horizontal plane while keeping all straight = 1 x 4 = 4 axes - try to show Vulkan Salute :)

So, the completed mechanical prototype was able to do all those on 22 axes, except circling of Upper Arm and moving fingers for Vulkan Salute.

By completing Front Arm mechanics, necessity of 13th Servo came up for “pronation” and “supination”, and 14th one for levering the whole Front Arm.

Upper Arm and Shoulder designs were easier comparing to Front Arm mechanical. But due to the weakness of balsa, shoulder-end of Upper Arm should be supported well from inside and 40 mm diameter PVC pipe perfectly did it. And a part of 25 mm PVC pipe is driven through, so the levering axis of Upper Arm is completed.

(Here I’m writing as quickly done, but it wasn’t so. Before realizing something, all the parts are modeled, matched and interacted with others, and checked for suitability on computer. Implementation comes after Engineering.)

However, since the beginning of actuation idea, there was also another question in my mind…

“OK, up to now we did quite well, but… how shall we support that thing, baby?”

Supporting question was not only for mechanical structure. It was also needed to fix controllers somewhere, but close to mechanical structure due to limited length of jumper wires. During days, I imagined a support structure specially made by wood, pipe, other materials… But I loved none of those ideas.

And at one night, I found a perfect solution already hanged beside my table since long ago: My camera tripod. Just by replacing its screw with a bolt to hold the turning axis of Shoulder, it may work perfectly, and worked.

So I designed just an axis and bottom plate for Shoulder finally, and just by using my excess furniture parts, plastic waste water pipe caps and balsa wood. Then everything is placed onto tripod.

Now I could start to install muscles, actuators and further, but before that another thing came up…

“There must be one more controller…”

Just after decided to actuate the whole arm, I counted the signals I have to use and faced with the bad news unavoidably: I had only one Arduino Due, and even it has lots of input/outputs comparing to the other models, still it was not enough. At first I have to define the further necessities, and then I found Arduino Leonardo as optimum solution for the secondary controller. Upon preparing an Input/Output List and a bit study on it, Hand- and Wrist-section are left under Due’s scope; while controlling of Front Arm, Upper Arm and Shoulder are to be managed by Leonardo.

Then, another question raised…

“Which controller should be the master, and how should they ‘talk’ to each other?”

Because of higher capacity and faster speed, it would be better to assign Due as master. However, there was a problem on “talking”: Due runs at 3.3V while Leonardo is at 5V level. Since their I2C communication use also same voltage levels as controllers, direct connection would burn Due.

I had to make that interface as simple as possible and the best way was to use analog (voltage) signals. Simply Leonardo should “understand”;

  • which part shall be actuated, and,
  • to which position it should be moved.

So I decided to use DAC0 and DAC1 analog (voltage) outputs on Due, and correspondingly A0 and A1 on Leonardo to define those.

However, stabilization of voltage outputs take a bit time. So Leonardo could misunderstand the requests by Due, so it should know also whether the commands are ready to be read. The third signal of communication appeared: “Execute”. Good news is, it could be directly wired from Due’s Ch.50 (digital output) to Leonardo’s Ch.8 (digital input).

Finally, how would Due understand that its command is executed? A fourth signal should be sent from Leonardo to Due: “Command Executed” feedback, but there’s a problem because of above mentioned voltage levels. If that confirmation signal would be wired directly from Leonardo’s Ch.12 (digital output) to Due’s Ch.51 (digital input), then I should organize a funeral ceremony for poor Due :)

Anyway, this problem was the easiest of entire project: Leonardo’s 5V output might be divided by just two kiloohm-level resistors, and then it might be sent to Due as ~3.3V. At the implementation-stage, just a 270k and a 470k worked very well.

Muscles and Tendons

You actuate muscles to take some action, but then body section must be returned back to original position when related muscle is released. So there are tendons on your body, located contrary to the muscles. But tendons are not strong as corresponding muscle due their intention of use.

I already wrote above the materials I decided to use as Muscles and Tendons. However, during trials I noticed that fishing ropes might be easily jammed between mechanical joints. So it would be better to use some kind of “guides”, and 6mm diameter PVC tube parts are exactly fit to that purpose.

No any tendons are used for Ulna and Upper Arm, since gravity would act well.

Limit Switches and Position Sensors

When I decided to actuate the arm, necessity of Limit Switches directly came up. Without them it would be very easy to damage the mechanical parts by servos. But unfortunately, servos were not powerful as I thought.

At the beginning I thought Limit switches would be one of the easiest parts, but it wasn’t so, especially on fingers. Actually that problem came up due to 2 main reasons: First is the balsa wood again, due to its weakness. And the second one is scale of my design. As you can see on some photos above, my intention was to make a humanoid hand and arm at 1:1 scale.

Anyway, I made contact points by just by wires and aluminum tape. Even they were essential to protect mechanicals, they lost their functionalities in time especially on Fingers due to wears on balsa parts.

Position Sensors are implemented by using potentiometers at few points only and they worked almost well.

Servo Actuators and Their Limitations

Due to limited space, I had just one choice to actuate (perhaps I should say “stretch” or "flexion/extension" as on medical terminology) “muscles” dedicated for Front Arm, Wrist and Hand sections, including Fingers as well: Multi-turn Micro Servo Motors. All of those should be located onto Upper Arm, so the Upper Arm design is done by considering rope routing as well.

At first, I made ~10 mm diameter pulleys made of balsa. However, output torques of Micro Servos were quite low. Then I modified their wheel horns: Diameters are reduced by grinding and a washer is used between horn and servo, and drilled close to the centers to connect ropes.By such a way, Servos could be able pull a bit higher loads.

At later stages, another pulley sets are required also to lever Front Arm and Upper Arm, so they’re done. Even Micro Servo could handle levering of Front Arm, standard servo is needed for Upper Arm, since it should handle the weight of Upper Arm, Front Arm, Hand and all wiring on them as well.

Another standard servo is used also for turning Shoulder.

“All those lead wires must be organized… Even a bit…”

And the best way to do that, was to make some kind of “terminal assemblies”. After studying few days, I found that 3 Terminal Assemblies could be made just by 2 pieces of 8x2 cm pre-fabricated PCBs.

  • TB01 is designed to handle all signals dedicated for Hand, Wrist and Radius (to twist Front Arm), as well as servos dedicated for Wrist and Radius (to twist Front Arm)
  • TB02 is designed to handle all signals and motors dedicated for Shoulder, Upper Arm and Ulna (to lever Front Arm)
  • TB03 is designed to handle just servos for Fingers

Of course all Terminal Assemblies should be tested after first “manufacturing” and during soldering of wires.

Early-Stage Tests of Servos

After -almost- everything installed, I took just Arduino Leonardo and started to test each servo by a simple routine individually to check it out whether it’s able to function as required. That routine was very basic and written to operate just one servo at lowest speed and direction selected by digital inputs. At that time it was possible to operate everything one by one. But when all the servos are energized together, an unexpected surprise came up. Later we’re going to talk about that trouble.

First Problem Continued :: Twisting the Wrist

Even I modified the servo arms to use as extremely low-diameter pulleys, yet it was not enough to “pronate” and “supinate” the Radius. So I made a micro pulley set.

The first one worked well, but it was too large. So it’s minimized on next step.

Unfortunately, still there was too much torque on respective Servo. I tried to adjust, but while Front Arm is at horizontal position and the Servo brought Radius fully onto Ulna and then tried to release, respective “tendon” rubber failed to return it because of frictions and counterforces on micro pulley. I tightened tendon, but then the servo was not able to pull anymore… After few trials, servo became unstable and soon later its motor controller burned. That was the first victim of Project. Please salute and pray for nameless hero -no, sorry its tag is “11FR”- :)

(Of course every single point/component had its own unique tag. Otherwise it would be impossible to make all that construction, wiring and programming. Patience, we’re going to talk also about importance of Engineering.)

So I decided to stop trials for twisting the Front Arm. It would need significant torque, but in the meantime further trials could lead damaging of entire Front Arm.

Other Trials :: Fingers

Nothing particular to say. At the beginning, everything was looking OK… Until servos were dead and balsa parts worn out.

First Codes :: Due<->Leonardo Intercommunication

I already mentioned above the principles, and on the programming stage it was the first section. On analog channels, Due/DAC0and Leonardo/A0 are assigned for Set Value Register to transmit and receive set values as percentage. Due/DAC1 and Leonardo/A1 are assigned for Section Selector.

But it was just needed to make some ranging: Due can use the full range but since DACx outputs would generate corresponding voltages in 0.75-2.55V range, matching scales should be adjusted on Leonardo.So, Leonardo is configured to read Set Value between (integer) 168-846corresponding to 0-100%. For the section selector the full range is divided into 5 sections on Due-side, and corresponding values are calculated for Leonardo. But in that implementation it was also necessary to use deadband. So, even the exact values used on Due, they're interpreted on Leonardo as (Value +/- Deadband) to ensure selected section.

At the end, intercommunication routines completed as follows on both controllers and worked well.

on Due:

   const int   _FA_Radius  =  840;    // Front Arm Radius Section
   const int   _FA_Ulna    = 1530;    // Front Arm Ulna Section
   const int   _UpperArm   = 2220;    // Upper Arm Section
   const int   _Shoulder   = 2910;    // Front Arm Radius Section
void CommandToLeonardo(String Section, int SetValue) {
   /* SetValue is given as Percentage */
   int Section_Num=0;
   if (Section=="_11FR") Section_Num = _FA_Radius;
      else if (Section=="_11FU") Section_Num = _FA_Ulna;
           else if (Section=="_21UA") Section_Num = _UpperArm;
               else if (Section=="_31SH") Section_Num = _Shoulder;
   if (Section_Num==0)
      Serial.println("Section cannot be recognized.");
   if ((SetValue<0) || (SetValue>100))
      Serial.println("Set Value should be between 0-100).");
   if ((Section_Num!=0) && (SetValue>=0) && (SetValue<=100) )
      {
       Serial.print(String(SetValue) + "% -> ");
       switch(Section_Num){
             case _FA_Radius: Serial.println("Front Arm-Radius"); 
                              break;
             case _FA_Ulna  : Serial.println("Front Arm-Ulna"); 
                              break;
             case _UpperArm : Serial.println("Upper Arm"); break;
             case _Shoulder : Serial.println("Shoulder"); break;
          }
       analogWrite (_00XXCM, Section_Num);
       analogWrite (_00XXSP, map(SetValue, 0, 100, 0, 4095));
       delay(50);        // Delay for stabilization of outputs
       digitalWrite(_00XXXC, HIGH);  // Execute Command
       do { // Wait until getting OK from Leonardo
          } while(!digitalRead(_00XXOK)); 
              
       Serial.println("ARD LNRD = OK.");
       analogWrite (_00XXCM, 0); 
       analogWrite (_00XXSP, 0);  
                    // Resetting Section and Set Value outputs
       delay(50);   // Delay for stabilization of outputs
       digitalWrite(_00XXXC, LOW);   // Resetting Command
      } /* End of "if (Section_Num!=0)" */
} /* End of "void CommandToLeonardo()" */
void loop() {
    if (Serial.available())
       {
        String Command=ReadUserInputOverSerial();
        Serial.println(Command); 
        if (Command.substring(0,1)=="?") /* Help */
           {
            Serial.println("Commands:");
            Serial.println("  CTL [Section] [SetValue (0-100)]");
            Serial.println("      CommandToLeonardo - Section: First 5 characters of PWM Tag (e.g. _10WH for _10WHCM).");
            Serial.end(); Serial.begin(_BAUDRATE);
            while(!Serial) { };
           }
        if (Command.substring(0,3)=="CTL")
           CommandToLeonardo(Command.substring(4,9),
                             Command.substring(10).toInt());
        CommandPromptOverSerial("[ARD DUE/] > ");
       } /* End of "if (Serial.available())..." */
} /* End of "void loop()": ARDUINO DUE */

on Leonardo:

   const int   _FA_Radius  = 310;    // Front Arm Radius Section
   const int   _FA_Ulna    = 425;    // Front Arm Ulna Section
   const int   _UpperArm   = 540;    // Upper Arm Section
   const int   _Shoulder   = 655;    // Front Arm Radius Section
   const int   _ICDB       =  40;    // Deadband
   /* Since the signals cannot generate exact integer values, 
    * Section shall be identified by [ (SectionValue) +/- _ICDB ]
    * reading.
   */
void setup() {
  /* Initial Feedback to Due */
  digitalWrite(_00XXOK, LOW); 
}
void CommandFromDue() {  
   CommandFromDue_Section = analogRead(_00XXCM);
   if ( ((_FA_Radius-_ICDB)<=CommandFromDue_Section) && 
        (CommandFromDue_Section<=(_FA_Radius+_ICDB)) )
      CommandFromDue_Section=_FA_Radius;
      else if ( ((_FA_Ulna-_ICDB)<=CommandFromDue_Section) && 
                (CommandFromDue_Section<=(_FA_Ulna+_ICDB)) )
              CommandFromDue_Section=_FA_Ulna;
      else if ( ((_UpperArm-_ICDB)<=CommandFromDue_Section) &&
              (CommandFromDue_Section<=(_UpperArm+_ICDB)) )
              CommandFromDue_Section=_UpperArm;
      else if ( ((_Shoulder-_ICDB)<=CommandFromDue_Section) &&
                (CommandFromDue_Section<=(_Shoulder+_ICDB)) )
               CommandFromDue_Section=_Shoulder;
      else
               CommandFromDue_Section=0;
   CommandFromDue_SetValue = 
               map(analogRead(_00XXSP),168,846,0,100);
   /* Due generates DAC outputs between 0.55-2.75 V and it 
    * corresponds (int) 168-846 on Arduino Leonardo.
    * Here analog reading is converted to Percentage.
    */
   boolean CommandFromDue_Execute = digitalRead(_00XXXC);
   String  Section = "";
   int     Speed   = 0;
   if ((CommandFromDue_Section!=0) && CommandFromDue_Execute)
      {
       Serial.println();
       Serial.print("Command by ARD DUE: "+
                     String(CommandFromDue_SetValue) + "% -> ");
       switch(CommandFromDue_Section){
             case _FA_Radius: Serial.println("Front Arm-Radius");
                              Section = "_11FR";
                              Speed = 20;
                              break;
             case _FA_Ulna  : Serial.println("Front Arm-Ulna");
                              Section = "_11FU";
                              Speed = 20;
                              break;
             case _UpperArm : Serial.println("Upper Arm");
                              Section = "_21UA";
                              Speed = 90;
                              break;
             case _Shoulder : Serial.println("Shoulder");
                              Section = "_31SH";
                              Speed = 30;
                              break;
           }
       Serial.println(" -action-");
       MoveToPosition(Section, Speed, CommandFromDue_SetValue);
       Serial.println("OK -> ARD DUE.");
       CommandPromptOverSerial("[ARD LNRD/] > ");
       digitalWrite(_00XXOK, HIGH); delay(100);
       digitalWrite(_00XXOK, LOW); // OK Feedback to Due
      } /* End of "if (Section_Num!=0)" */
} /* End of "void CommandFromDue()" */

Coding :: Further

Other than intercommunication, here I'm not going to explain deep details of further codes just to avoid boring reader, since at the end you can find complete programs as 800+ lines for Due and 500+ lines for Leonardo, as well as explanatory notes. As you can see on programs, there are already functions with same names. Principally such functions are exactly same in principle/algorithm but just some changes made due to scopes of controls.

void CommandPromptOverSerial(Prompt)

It's created just to make that fancy command line interface. Besides that, it also reinitialized serial communication so that ensures flush.

String ReadUserInputOverSerial()

reads the user input at Serial Monitor, and returns it as string.

void Read(Tag)

void LoopTest()

Both of those functions are created for testing of individual loops (input/output channels).

LoopTest() has already subsections dedicated for Analog Input, Analog Output, Digital Input, Digital Output and PWM-Output types; but Read(Tag) is intended only to read an input directly from top-level without going to LoopTest().

void CommandToServo(Tag, Value)

As you recognized till now, I referred everything as tags. That's also the intention of this function: It sends set value (0-180)to the Servo motor assigned to "Tag".

void FullTravelDurationMeasurement(PWM_Tag, LimitSw_CCW, LimitSw_CW, int Speed)

This function is written to check full travel durations for the Fingers since they could not be equipped with actual position sensors. According to results, they could be pulled or released by timing. Even it worked theoretically by manual forcing on input/outputs, it could not be used on actual system because switches could not work properly due to the wears on mechanicals. Consequently, full travel durations could be determined by manual trials only, instead of using this function.

void MoveDuringMillis(Tag, Direction, Speed, Milliseconds)

This function is also written to actuate a Finger defined by Tag, at the specified Direction and during specified Milliseconds.

void MoveToPosition (Section, Speed, SetValue)

This function is written to move a Section to the position given by SetValue and at the specified Speed. Note that Section should also be equipped with position sensor.

void Fingers_Hold()

void Fingers_Release()

These functions are written to close (to hold something) and release the fingers -almost- simultaneously. Unfortunately, only once and partially the Fingers could work…

void Final_Demo()

As its name says: At the end only the levering mechanicals of Front Arm and Upper Arm, and rotating of Shoulder was able to be used. So this function is written to demonstrate them.

Upon successfully testing all individual signals and functions, I finished mechanical connections between servos and mechanical sections. Now, arm was ready for testing real functionality but…

Energized :: Crazy Servos

After testing and adjusting all servos one by one and individually, it was the time to power all servos in once. But the result was not as I expected.

My original design had 14x Micro Servos at Upper Arm to handle lightweight loads, while 2x standard servos were placed at Shoulder for levering of Upper Arm and turning Shoulder. All of them were able to accept one common voltage level, 6V, so I did that.

But once I operated a standard servo, some micro servos are just started without any command. Fortunately, I put a switch onto power supply line, so I could cut all in once but anyway some Fingers are over forced because of instant and extreme acceleration. Again I unscrewed the pulleys of all micro servos, then re-energized all. The result was the same:Unstable micro servos could never be adjusted, or could not keep stability even adjusted, and even unloaded.

My first doubt was suitability of power supply: I used a 6V, 3Ah adapter.

According to available data, each micro servo consumes 6 mA at idle, 120 mA at no-load operation and 800 mA at stall conditions. For standard servos only one parameter is given: 100 mA as operating current, I considered it for no-load operation, and assumed the rest of the parameters similar to micro servos.

So, my power supply would be enough since only one servo would be operated at a moment. So, what could be the reason of fail?

Still I was thinking on power supply, and started to monitor it by oscilloscope. The result was interesting while just one of standard servo was running:

When I saw those changes, I had a theory: Possibly micro servos were more sensitive against voltage fluctuations, and those were misinterpreted by them as “command”. Then I decided to power micro- and standard servos separately. Standard servos are kept connected at current power supply together with just one micro servo (the one dedicated for levering Front Arm), while all other micro servos are connected to other power supply separately. And that modification worked.

Fingers :: One Last Time

As I already wrote above, at the end I was not able to operate all sections because of wears. Anyway, it could open and close fingers just once, and possibly I would kill the remaining 6 micro servos if I continue trying. So it was the best to stop here. Theoretically, you would see sequential but smoother working if the balsa parts were not worn.

Final Demo

Finally after ~4.5 months of design and assembling, it was possible to operate only levering functions for Front- and Upper-Arms, and turning function of Shoulder.

Engineering :: Industrial Practices Revisited

Perhaps you can follow everything by your eyes and hands if your project is within very small area and contain just few or several devices.

But if you have to work on ~1-meter length equipment contain 500+ parts in 140+ different types, ~60 electrical and electronic devices (switches, potentiometers, servo motors), 60+ signals on two different controllers as well as interconnecting both, and hundreds of wires for connections; then working on computer and papers become a must and would be the best for troubleshooting.

So everything started by designing mechanicals on computer. At later stages; servo motors, switches, position sensors and terminal assemblies were added onto model.

As I mentioned on previous sections, dedicated unique tags are also assigned to all individual parts. You’re going to see those tags also on the programs, those made life a bit easier. At the end, there are “3D Model”, “Pulley Rope Routing Sketches”, “Instrument List”, “I/O List”, “Instrument Layout”, “Terminal Board Layout” and “Wiring Diagram” as the documentation package of Project.

For Future :: Bottlenecks

I agree that my design is extremely primitive. My initial intention was just to imitate the human hand and arm mechanically, but later I decided to try actuating whole structure.

On the mechanical design-side everything was almost as expected: All mechanical parts perfectly matched and interacted with each other manually.

Anyway, the first bottleneck was the chosen material, the balsa wood: I chose just because it’s available in pre-formed/pre-cut parts widely for hobby works, very cheap to use on prototyping, easy to form by simple hand tools, easy to repair just by using strong glues. And by using balsa wood, I could avoid buying a quite expensive CNC or 3D printing machine as well as their accessories and consumables.

However, balsa wood is also a very weak material, and led wears especially on fingers earlier than I imagined.

Besides those, 1:1 scale of my design “amplified” the weakness of balsa. I had to make repairs on many places during building up and trying to operate. As final example, holder plates of servo motors on Upper Arm were also made of balsa, and after few removals they started to crack.

And also it was not possible to use any bearings, especially on hand section due to the scale.

Even though, I recommend balsa wood for making mechanical prototypes on cheapest way.

The other bottleneck was size and power of the servos. Because of the scale, I had to use micro- and standard servos only, but they have limited abilities. That's also the reason of pulley sets considered for pronation of Radius and flexion of Ulna and Upper Arm, so entire arm had a crane-type structure instead of a real robotic one.

For Future :: Lessons

Imitating of natural mechanisms is a very hard and complicated project, since some of their functionalities provided by flexible (nearly universal) connections between mechanical sections (=bones) held together by actuators (=muscles) and reversing springs (=tendons). Also tendons could be stretched - even a bit and not much as muscles.

But that’s not all. When you actuate a “section”, you also know it’s actual position in real-time, as well as interlocking actuation if it’s reached to the movement limits. It means, a position sensor and at least 2 limit switches should be used for each section.

As you noticed, I had to use servo motors, but the alternatives are stepper motors if you would like to avoid position sensors. However, 4 digital outputs would be needed for each stepper in that case, consequently 2 Arduino-class controllers would not be enough.

Do you think that we finished? No, not yet.

Let’s make a simple experiment: Hold your right hand with facing your palm straight upwards. Then bend index finger of your right hand, and “hang” it at side of left hand palm. Now, try to holding your right hand just by tip of your left-index finger, while pushing right hand to straight upwards. Do you see that? Up to a certain limit, you can withstand against muscles on your arm, just by muscles controlling the index finger bones.

If you try to do that by imitated mechanisms, it’s almost sure that they’re going to be broken. During even simple trials, I burned 7 micro servos.

And more:

On the natural mechanisms, the "actuators" are placed very close to the mechanical sections. Today the best way to do that on mechanical imitations, might be to use hydraulic cylinders. But in that case, a powerful hydraulic pump system, oil reservoir, check valves, solenoid valves…should also be used and that would bring necessity of far larger space requirements and controller + input/output capacities.

Those latest 2 paragraphs mean that, far smaller and far more powerful actuators are needed.

Finished? No, there’s one more issue:

Let you try to move a finger…

Now try moving 2 or 3 fingers simultaneously… Good.

Try moving all fingers while moving your hand up-down at the wrist, then left-right… Perfect.

Move all fingers while twisting your wrist while levering your front arm…

[ What? Are you OK? Look at me, shall we take you to a clinic? :)) ]

If you have no any serious health problem, you could do all, right? Now how can we do it with a mechanical imitation?

As I mentioned already, human arm and hand have ~30 different movements (axes) in total, so one actuator must be used for each, for a realistic imitation able to act smoothly.

If we would like to do it by using some simple controllers like Arduino-class, simply we need one sub-controller for each axis, and a master-controller to command them all. Sub-controllers should have their specific routines define only specific movements of respective section, for predefined overall movements of entire system. Master should just send set points and requested overall movement information to the sub-controllers, and then send an “execute” command to all in once, and wait for confirmations.

For example, each sub-controller dedicated individually for Proximal, Medial and Distal sections of Index-, Middle-, Ring- and Pinky-fingers (so we're talking about 12 sub-controllers together with actuators, position sensors, limit switches and so on as needed) should have -for example- a "Hold_a_Circular_Plate()" subroutine defines specific movement only for the respective section. When the master controller sends "Command_to_Hold_Circular_Plate()", it should trigger just one output goes to all sub-controllers simultaneously. But another important issue is to synchronize sub-controllers in microseconds, otherwise all would fail on action.

Second option is to use a far more powerful controller and actuators with far more faster responses, so that switching between different actuators shall not be recognizable.

Third option is the best one is, if possible: To use the most powerful controller ever made, that is able to perform absolute multi-processing at extreme speeds: An organic brain together with neural system, preferably a human brain for a humanoid arm (fictionally it's been done on Robocop-series).

:: Ending ::

Even today some companies are manufacturing prosthetic humanoid hands for disabled people, and even they’re fantastic designs and making people’s life easier, we cannot say that all those are perfect imitations.

I think almost all of us see possibly the most amazing robot “Atlas” designed and constructed by Boston Dynamics. Such research and developments consume extreme amount of money, time and requires long hours of studies, trials, countless fails and so on. For example, Boston Dynamics has been founded in 1992 and since that time they’re continuously working on robots.That means, as of today, there’s nearly 30 years of study, trials, fails, retries… and so on behind Atlas.

On the budget side, “fathers” and “mothers” of Atlas are funded by US Defense Advanced Research Projects Agency. Yes, unfortunately military agencies are the master supporters of such amazing projects and mankind is still very good on finding the best ways to kill each other :(

And such amazing things cannot be made by consumer products on markets. All the mechanical, electronics, actuators, sensors…should be specifically and individually designed by customized parameters as tailor-made for the Project.

Perhaps recently-invented artificial muscles may help to develop better prostheses.

In all cases, I hope you find all that long story, and your body as well, interesting from mechanical and controls point of view…

Code

Application Program - Arduino DueArduino
This program shall be loaded onto Due
#include <Servo.h>

/* Serial Comm.Baudrate Setting: ARDUINO DUE */
    const int _BAUDRATE = 4800;
/* End of Serial Comm.Baudrate Setting: ARDUINO DUE */

/* Tag-Channel(Pin) Assignments and IO_Channel() Function: ARDUINO DUE */
    const int _01MCZL = 22;   // 01: Thumb Finger Metacarpal ZL
    const int _01MCZH = 23;   // 01: Thumb Finger Metacarpal ZH
    const int _01PDZL = 24;   // 01: Thumb Finger Proximal+Distal ZL
    const int _01PDZH = 25;   // 01: Thumb Finger Proximal+Distal ZH
    const int _01MCCM = 2;    // 01: Thumb Finger Metacarpal CMD
    const int _01PDCM = 3;    // 01: Thumb Finger Distal CMD

    const int _02PXZL = 26;   // 02: Index Finger Proximal ZL
    const int _02PXZH = 27;   // 02: Index Finger Proximal ZH
    const int _02MDZL = 28;   // 02: Index Finger Medial+Distal ZL
    const int _02MDZH = 29;   // 02: Index Finger Medial+Distal ZH
    const int _02PXCM = 4;    // 02: Index Finger Proximal CMD
    const int _02MDCM = 5;    // 02: Index Finger Distal CMD

    const int _03PXZL = 30;   // 03: Middle Finger Proximal ZL
    const int _03PXZH = 31;   // 03: Middle Finger Proximal ZH
    const int _03MDZL = 32;   // 03: Middle Finger Medial+Distal ZL
    const int _03MDZH = 33;   // 03: Middle Finger Medial+Distal ZH
    const int _03PXCM = 6;    // 03: Middle Finger Proximal CMD
    const int _03MDCM = 7;    // 03: Middle Finger Distal CMD

    const int _04PXZL = 34;   // 04: Ring Finger Proximal ZL
    const int _04PXZH = 35;   // 04: Ring Finger Proximal ZH
    const int _04MDZL = 36;   // 04: Ring Finger Medial+Distal ZL
    const int _04MDZH = 37;   // 04: Ring Finger Medial+Distal ZH
    const int _04PXCM = 8;    // 04: Ring Finger Proximal CMD
    const int _04MDCM = 9;    // 04: Ring Finger Distal CMD

    const int _05PXZL = 38;   // 05: Pinky Finger Proximal ZL
    const int _05PXZH = 39;   // 05: Pinky Finger Proximal ZH
    const int _05MDZL = 40;   // 05: Pinky Finger Medial+Distal ZL
    const int _05MDZH = 41;   // 05: Pinky Finger Medial+Distal ZH
    const int _05PXCM = 10;   // 05: Pinky Finger Proximal CMD
    const int _05MDCM = 11;   // 05: Pinky Finger Distal CMD

    const int _10WHZL = 42;   // 10: Wrist Horizontal ZL
    const int _10WHZH = 43;   // 10: Wrist Horizontal ZH
    const int _10WHTX = A0;   // 10: Wrist Horizontal Actual Pos
    const int _10WHCM = 12;   // 10: Wrist Horizontal CMD

    const int _10WVZL = 44;   // 10: Wrist Vertical ZL
    const int _10WVZH = 45;   // 10: Wrist Vertical ZH
    const int _10WVTX = A1;   // 10: Wrist Vertical Actual Pos
    const int _10WVCM = 13;   // 10: Wrist Vertical CMD

    const int _11FRTX = A2;   // 11: Front Arm Radius Actual Pos
    const int _11FUTX = A3;   // 11: Front Arm Ulna Actual Pos
    const int _21UATX = A4;   // 21: Upper Arm Actual Pos
    const int _31SHTX = A5;   // 31: Shoulder Actual Pos

    const int _00XXXC = 50;   // Execute Command (0: Stop/Idle, 1: Execute)
    const int _00XXSP = DAC0; // Setpoint Register
    const int _00XXCM = DAC1; // Command Selector (*1)
    const int _00XXOK = 51;   // Command Executed by Arduino Leonardo (*2-IMPORTANT)

    int IO_Channel(String Tag) { /* VERIFIED FUNCTION: ARDUINO DUE */
       /* This function is used to validate IO Channel for DI/DO/AO/PWM-O. 
        * If given tag is valid, respective channel number will be returned.
        * Otherwise -1 will be returned as "error".
        */
       if (Tag=="_01MCZL") return(_01MCZL);
       if (Tag=="_01MCZH") return(_01MCZH);
       if (Tag=="_01PDZL") return(_01PDZL);
       if (Tag=="_01PDZH") return(_01PDZH);
       if (Tag=="_01MCCM") return(_01MCCM);
       if (Tag=="_01PDCM") return(_01PDCM);
       if (Tag=="_02PXZL") return(_02PXZL);
       if (Tag=="_02PXZH") return(_02PXZH);
       if (Tag=="_02MDZL") return(_02MDZL);
       if (Tag=="_02MDZH") return(_02MDZH);
       if (Tag=="_02PXCM") return(_02PXCM);
       if (Tag=="_02MDCM") return(_02MDCM);
       if (Tag=="_03PXZL") return(_03PXZL);
       if (Tag=="_03PXZH") return(_03PXZH);
       if (Tag=="_03MDZL") return(_03MDZL);
       if (Tag=="_03MDZH") return(_03MDZH);
       if (Tag=="_03PXCM") return(_03PXCM);
       if (Tag=="_03MDCM") return(_03MDCM);
       if (Tag=="_04PXZL") return(_04PXZL);
       if (Tag=="_04PXZH") return(_04PXZH);
       if (Tag=="_04MDZL") return(_04MDZL);
       if (Tag=="_04MDZH") return(_04MDZH);
       if (Tag=="_04PXCM") return(_04PXCM);
       if (Tag=="_04MDCM") return(_04MDCM);
       if (Tag=="_05PXZL") return(_05PXZL);
       if (Tag=="_05PXZH") return(_05PXZH);
       if (Tag=="_05MDZL") return(_05MDZL);
       if (Tag=="_05MDZH") return(_05MDZH);
       if (Tag=="_05PXCM") return(_05PXCM);
       if (Tag=="_05MDCM") return(_05MDCM);
       if (Tag=="_10WHZL") return(_10WHZL);
       if (Tag=="_10WHZH") return(_10WHZH);
       if (Tag=="_10WHTX") return(_10WHTX);
       if (Tag=="_10WHCM") return(_10WHCM);
       if (Tag=="_10WVZL") return(_10WVZL);
       if (Tag=="_10WVZH") return(_10WVZH);
       if (Tag=="_10WVTX") return(_10WVTX);
       if (Tag=="_10WVCM") return(_10WVCM);
       if (Tag=="_11FRTX") return(_11FRTX);
       if (Tag=="_11FUTX") return(_11FUTX);
       if (Tag=="_21UATX") return(_21UATX);
       if (Tag=="_31SHTX") return(_31SHTX);
       if (Tag=="_00XXXC") return(_00XXXC);
       if (Tag=="_00XXSP") return(_00XXSP);
       if (Tag=="_00XXCM") return(_00XXCM);
       if (Tag=="_00XXOK") return(_00XXOK);
       return(-1);
} /* End of "IO_Channel(String Tag)": ARDUINO DUE */
/* End of Tag-Channel(Pin) Assignments and IO_Channel() Function: ARDUINO DUE */


/* Servo _Definitions + CommandToServo() function: ARDUINO DUE */
    Servo _01MC_Servo;
    Servo _01PD_Servo;
    Servo _02PX_Servo;
    Servo _02MD_Servo;
    Servo _03PX_Servo;
    Servo _03MD_Servo;
    Servo _04PX_Servo;
    Servo _04MD_Servo;
    Servo _05PX_Servo;
    Servo _05MD_Servo;
    Servo _10WH_Servo;
    Servo _10WV_Servo;

    void CommandToServo(String Tag, int Value) { /* VERIFIED FUNCTION: ARDUINO DUE */
       /* This function is used to actuate PWM outputs to Servo Motors. */
       if (Tag=="_01MCCM") _01MC_Servo.write(Value);
       if (Tag=="_01PDCM") _01PD_Servo.write(Value);
       if (Tag=="_02PXCM") _02PX_Servo.write(Value);
       if (Tag=="_02MDCM") _02MD_Servo.write(Value);
       if (Tag=="_03PXCM") _03PX_Servo.write(Value);
       if (Tag=="_03MDCM") _03MD_Servo.write(Value);
       if (Tag=="_04PXCM") _04PX_Servo.write(Value);
       if (Tag=="_04MDCM") _04MD_Servo.write(Value);
       if (Tag=="_05PXCM") _05PX_Servo.write(Value);
       if (Tag=="_05MDCM") _05MD_Servo.write(Value);
       if (Tag=="_10WHCM") _10WH_Servo.write(Value);
       if (Tag=="_10WVCM") _10WV_Servo.write(Value);
    } /* End of "CommandToServo()": ARDUINO DUE */
/* End of Servo _Definitions + CommandToServo() function: ARDUINO DUE */


/* Constant Parameters, floatMap() Function: ARDUINO DUE */
    const float _DBPercent  =    1.0;  // % Deadband
    const int   _CCW        =    1;    // Servo Direction for Counter Clockwise
    const int   _CW         =   -1;    // Servo Direction for Counter Clockwise
    
    float floatMap /* VERIFIED FUNCTION: ARDUINO DUE */
          (int InputValue, int fromLow, int fromHigh, int toLow, int toHigh) {
      /* This function has exacly same functionality with Arduino's 
       * map(value, fromLow, fromHigh, toLow, toHigh)
       * function. However, original map function truncates fractions and return
       * just integers. So this one is programmed to return exactly scaled values.
      */
      return( (float)toLow + (float)((float)((InputValue-fromLow)*(toHigh-toLow))/(float)(fromHigh-fromLow)) );
    } /* End of "floatMap()" */
    
/* End of Constant Parameters, floatMap() Function: ARDUINO DUE */


/* Constant Parameters for Intercommunication: ARDUINO DUE */
    const int   _FA_Radius  =  840;    // Front Arm Radius Section
    const int   _FA_Ulna    = 1530;    // Front Arm Ulna Section
    const int   _UpperArm   = 2220;    // Upper Arm Section
    const int   _Shoulder   = 2910;    // Front Arm Radius Section
/* End of Constant Parameters for Intercommunication: ARDUINO DUE */


/* "void setup()": ARDUINO DUE */
void setup() {
  // TEST CODES:
  
  // VERIFIED CODES:
    pinMode(_01MCZL, INPUT_PULLUP);
    pinMode(_01MCZH, INPUT_PULLUP);
    pinMode(_01PDZL, INPUT_PULLUP);
    pinMode(_01PDZH, INPUT_PULLUP);
    pinMode(_02PXZL, INPUT_PULLUP);
    pinMode(_02PXZH, INPUT_PULLUP);
    pinMode(_02MDZL, INPUT_PULLUP);
    pinMode(_02MDZH, INPUT_PULLUP);
    pinMode(_03PXZL, INPUT_PULLUP);
    pinMode(_03PXZH, INPUT_PULLUP);
    pinMode(_03MDZL, INPUT_PULLUP);
    pinMode(_03MDZH, INPUT_PULLUP);
    pinMode(_04PXZL, INPUT_PULLUP);
    pinMode(_04PXZH, INPUT_PULLUP);
    pinMode(_04MDZL, INPUT_PULLUP);
    pinMode(_04MDZH, INPUT_PULLUP);
    pinMode(_05PXZL, INPUT_PULLUP);
    pinMode(_05PXZH, INPUT_PULLUP);
    pinMode(_05MDZL, INPUT_PULLUP);
    pinMode(_05MDZH, INPUT_PULLUP);
    pinMode(_10WHZL, INPUT_PULLUP);
    pinMode(_10WHZH, INPUT_PULLUP);
    pinMode(_10WVZL, INPUT_PULLUP);
    pinMode(_10WVZH, INPUT_PULLUP);
    pinMode(_00XXOK, INPUT);

   _01MC_Servo.attach(_01MCCM);
   _01PD_Servo.attach(_01PDCM);
   _02PX_Servo.attach(_02PXCM);
   _02MD_Servo.attach(_02MDCM);
   _03PX_Servo.attach(_03PXCM);
   _03MD_Servo.attach(_03MDCM);
   _04PX_Servo.attach(_04PXCM);
   _04MD_Servo.attach(_04MDCM);
   _05PX_Servo.attach(_05PXCM);
   _05MD_Servo.attach(_05MDCM);
   _10WH_Servo.attach(_10WHCM);
   _10WV_Servo.attach(_10WVCM);

   /* This part is used to send initial "neutral position/stop" 
    * to all PWM outputs assigned to Servo Motors.
    */
   _01MC_Servo.write(90);
   _01PD_Servo.write(90);
   _02PX_Servo.write(90);
   _02MD_Servo.write(90);
   _03PX_Servo.write(90);
   _03MD_Servo.write(90);
   _04PX_Servo.write(90);
   _04MD_Servo.write(90);
   _05PX_Servo.write(90);
   _05MD_Servo.write(90);
   _10WH_Servo.write(90);
   _10WV_Servo.write(90);
  
   analogReadResolution(12);  /* Analog Reading Scale: 0-4095 for ARDUINO DUE*/

   analogWriteResolution(12); /* Analog Writing Scale: 0-4095 for ARDUINO DUE*/
  /* DAC Initialize
   *  DAC initialization is needed at first start, otherwise DAC of Due 
   *  generates incorrect voltage levels.
   *  Note that 0 and 4095 have no functions on Leonardo.
   */
   analogWrite(_00XXSP,0); delay(50); analogWrite(_00XXSP,4095); delay(50); analogWrite(_00XXSP,0);
   analogWrite(_00XXCM,0); delay(50); analogWrite(_00XXCM,4095); delay(50); analogWrite(_00XXCM,0);

   digitalWrite(_00XXXC,LOW);
   
   Serial.begin(_BAUDRATE); while(!Serial) { };
   Serial.println("? -> Help.");
   Serial.print("[ARD DUE/] > ");
} /* End of "void setup()": ARDUINO DUE */


/* Global Variables for Test Purposes: ARDUINO DUE */

/* End of Global Variables for Test Purposes: ARDUINO DUE */


/* Verified Global Variables: ARDUINO DUE */

/* End of Verified Global Variables: ARDUINO DUE */


void CommandPromptOverSerial(String _Prompt) { /* VERIFIED FUNCTION: ARDUINO DUE */
  /* Reprompting and flushing serial for next input.
   *  This function will be used after each use of ReadUserInputOverSerial()
  */
  Serial.print(_Prompt); Serial.end(); Serial.begin(_BAUDRATE); while(!Serial) { };
} /* End of "CommandPromtOverSerial()": ARDUINO DUE */


String ReadUserInputOverSerial(){ /* VERIFIED FUNCTION: ARDUINO DUE */
  /* This function will be used only "if (Serial.available())" */
  char   UserInput[32];
  Serial.readBytes(UserInput,32); 
  /* Buffering and capitalizing User Input */
  String Buffer=UserInput; Buffer.toUpperCase();
  return(Buffer);
} /* End of "ReadUserInputOverSerial()": ARDUINO DUE */


void Read(String Tag) { /* VERIFIED FUNCTION: ARDUINO DUE */
  /* Indicates value at input channel defined by "Tag" */
  if ( (Tag.substring(5,7)=="ZL") || (Tag.substring(5,7)=="ZH") || (Tag.substring(5,7)=="OK"))
     Serial.println(Tag + " = " + String(digitalRead(IO_Channel(Tag))));
     else if (Tag.substring(5,7)=="TX")
        Serial.println(Tag + " = " + String(analogRead(IO_Channel(Tag))));
        else Serial.println(Tag + "is not an input.");
} /* End of "void Read()": ARDUINO DUE */


void LoopTest() { /* VERIFIED FUNCTION: ARDUINO DUE */
 String Command, User_Tag;
 Serial.println("? -> Help, X -> Terminate");
 CommandPromptOverSerial("[ARD DUE/LoopTest/] > "); 
 do
  {
     if (Serial.available())
        {
         byte   Command_to_Case=0; /* Command is invalid if remains as 0. */
         int    User_Value=0, User_Value_Last=0, Reading=0, Reading_Last=-1;
         String Buffer = ReadUserInputOverSerial();
         Serial.println(Buffer); Serial.end(); Serial.begin(_BAUDRATE); while(!Serial) { };
         
         /* Seperating Command and Tag*/
         Command = Buffer.substring(0,2); User_Tag = Buffer.substring(3,10);
         
         /* Converting (string)Command to (byte)Command_to_Case */
         if (Command=="AI") Command_to_Case=11;
         if (Command=="AO") Command_to_Case=12;
         if (Command=="DI") Command_to_Case=21;
         if (Command=="DO") Command_to_Case=22;
         if (Command=="PO") Command_to_Case=32;
         if (Command[0]=='?') Command_to_Case=98;   // Display Help
         if (Command[0]=='X') Command_to_Case=99;   // Terminate Test
         
         /* Checking whether tag is valid. */
         if ((Command_to_Case!=0)&&(Command_to_Case<90)&&(IO_Channel(User_Tag)<0))
            Command_to_Case=1; /* Redirecting to unrecognized tag case. */

         /* Taking respective actions and giving feedbacks for each command. */
         switch (Command_to_Case) {
               case 0 : Serial.println("Invalid command.");
                        CommandPromptOverSerial("[ARD DUE/LoopTest/] > ");
                        break;
               case 1 : Serial.println("Tag not exists: " + User_Tag);
                        CommandPromptOverSerial("[ARD DUE/LoopTest/] > ");
                        break;
               case 11: Serial.println("AI-Test('X'->Terminate): " + User_Tag);
                        Reading_Last = -1;
                        do
                          {
                            Buffer = ReadUserInputOverSerial();
                            Reading = analogRead(IO_Channel(User_Tag));
                            if (Reading != Reading_Last)
                               {
                                Serial.println(User_Tag + " = " + String(Reading));
                                Reading_Last = Reading; delay(1000);
                                /* 1s delay is used to avoid fast updates on Serial Monitor */
                               }
                          }
                        while (Buffer[0]!='X');
                        Serial.println();
                        CommandPromptOverSerial("[ARD DUE/LoopTest/] > ");
                        break;
               case 12: Serial.println("AO-Test('X'->Terminate): " + User_Tag);
                        Serial.println("Integer Value(0-4095): ");
                        Buffer="";
                        do
                          { 
                            if (Serial.available())
                               {
                                Buffer = ReadUserInputOverSerial();
                                if (Buffer[0]=='X') User_Value=0; else User_Value=Buffer.substring(0).toInt();
                                Serial.println(String(User_Value) + " -> " + User_Tag);
                                if (User_Value!=User_Value_Last) 
                                   {
                                    analogWrite(IO_Channel(User_Tag),User_Value);
                                    User_Value_Last = User_Value;
                                   }
                                   else analogWrite(IO_Channel(User_Tag),User_Value_Last);
                                Serial.println("Integer Value(0-4095): ");
                                Serial.end(); Serial.begin(_BAUDRATE); while(!Serial) { };
                               } /* End of "if (Serial.available())..." */
                          }
                        while (Buffer[0]!='X');
                        Serial.println();
                        CommandPromptOverSerial("[ARD DUE/LoopTest/] > ");
                        break;
               case 21: Serial.println("DI-Test('X'->Terminate): " + User_Tag); 
                        Reading_Last = -1;
                        do
                          {
                            Buffer = ReadUserInputOverSerial();
                            Reading = digitalRead(IO_Channel(User_Tag));
                            if (Reading != Reading_Last)
                               {
                                Serial.println(User_Tag + " = " + String(Reading));
                                Reading_Last = Reading;
                               }
                          }
                        while (Buffer[0]!='X');
                        Serial.println();
                        CommandPromptOverSerial("[ARD DUE/LoopTest/] > ");
                        break;
               case 22: Serial.println("DO-Test('X'->Terminate): " + User_Tag);
                        Serial.print("Enter 0 or 1: ");
                        Buffer="";
                        do
                          { 
                            if (Serial.available())
                               {
                                Buffer = ReadUserInputOverSerial();
                                if (Buffer[0]=='X') User_Value=0; else User_Value=Buffer.substring(0).toInt();
                                Serial.println(String(User_Value) + " -> " + User_Tag);
                                if (((User_Value==0)||(User_Value==1))&&(User_Value!=User_Value_Last))
                                   {
                                    
                                    digitalWrite(IO_Channel(User_Tag),User_Value);
                                    User_Value_Last = User_Value;
                                   }
                                   else digitalWrite(IO_Channel(User_Tag),User_Value_Last);
                                Serial.print("Enter 0 or 1: ");
                                Serial.end(); Serial.begin(_BAUDRATE); while(!Serial) { };
                               } /* End of "if (Serial.available())..." */
                          }
                        while (Buffer[0]!='X');
                        CommandPromptOverSerial("[ARD DUE/LoopTest/] > ");
                        break;
               case 32: Serial.println("PWM-Test('X'->Terminate): " + User_Tag);
                        CommandToServo(User_Tag,90); /* Sending "stop" to selected tag */
                        Serial.print("Integer Value(0-180): ");
                        Buffer="";
                        do
                          { 
                            if (Serial.available())
                               {
                                Buffer = ReadUserInputOverSerial();
                                if (Buffer[0]=='X') User_Value=90; else User_Value=Buffer.substring(0).toInt();
                                Serial.println(String(User_Value) + " -> " + User_Tag);
                                if (((User_Value>=0)||(User_Value<=180))&&(User_Value!=User_Value_Last))
                                   { CommandToServo(User_Tag,User_Value); User_Value_Last = User_Value; }
                                   else CommandToServo(User_Tag,User_Value);
                                Serial.print("Integer Value(0-180): ");
                                Serial.end(); Serial.begin(_BAUDRATE); while(!Serial) { };
                               } /* End of "if (Serial.available())..." */
                          }
                        while (Buffer[0]!='X');
                        Serial.println();
                        CommandPromptOverSerial("[ARD DUE/LoopTest/] > ");
                        break;
               case 98: Serial.println("Usage: '[CC] [Tag]'");
                        Serial.println("    [CC]: Channel Type");
                        Serial.println("          AI, AO: Analog Input/Output");
                        Serial.println("          DI, DO: Digital Input/Output");
                        Serial.println("          PO: PWM Output for Servo Motor");
                        Serial.println("Other Commands:");
                        Serial.println("      ? : Help (this display)");
                        Serial.println("      X : Exit from Loop Testing");
                        CommandPromptOverSerial("[ARD DUE/LoopTest/] > ");
                        break;                        
               case 99: Serial.println("Loop Test is terminated.");
                        break;                        
             } /* End of "switch (Command_to_Case)..." */
        } /* End of "if (Serial.available())..." */
  }
 while (Command[0]!='X');
} /* End of "void LoopTest()": ARDUINO DUE */


void FullTravelDurationMeasurement( /* VERIFIED FUNCTION: ARDUINO DUE */
     String PWM_Tag, String LimitSw_CCW, String LimitSw_CW, int Speed) {
   /* This function is used to measure full travel time of a "section"
    * equipped with Servo:User_Tag and Limit Switches.
    */
  int           LimitSw_CCW_Ch = IO_Channel(LimitSw_CCW),
                LimitSw_CW_Ch  = IO_Channel(LimitSw_CW),
                _LimitSwCCW_Ch = 0, /* Corrected Limit Sw.Ch.for CCW-direction */
                _LimitSwCW_Ch  = 0, /* Corrected Limit Sw.Ch.for CW-direction */
                Direction      = _CCW,
                Counter        = 0;
  unsigned long StartTime=0, Duration=0;
  Serial.println("Full Travel Duration Test");
  if ((IO_Channel(PWM_Tag)*LimitSw_CCW_Ch*LimitSw_CW_Ch)!=-1)
     {
        Serial.println("     PWM           : " + PWM_Tag + " [Ch. " + String(IO_Channel(PWM_Tag)) + "]");
        Serial.println("     Limit Switch.1: " + LimitSw_CCW + " [Ch. " + String(LimitSw_CCW_Ch) + "]");
        Serial.println("     Limit Switch.2: " + LimitSw_CW + " [Ch. " + String(LimitSw_CW_Ch) + "]");
        
        /* Matching Limit Switches with Directions 
         * Servo is set to turn CCW direction at first. Then reached limit switch is
         * marked as the one corresponding to CCW-end, while the other is for CW-end.
         * Notes:
         *   - Limit switches gives 0 when reached.
         *   - (90+Speed) -> Counter Clockwise turn (CCW)
         *   - (90-Speed) -> Clockwise turn (CW)
        */
        CommandToServo(PWM_Tag, 90 + Direction*Speed);
        while ( digitalRead(LimitSw_CCW_Ch) && digitalRead(LimitSw_CW_Ch) ) { }
        CommandToServo(PWM_Tag, 90);
        if ( ! digitalRead(LimitSw_CCW_Ch) ) 
           { _LimitSwCCW_Ch = LimitSw_CCW_Ch; _LimitSwCW_Ch = LimitSw_CW_Ch; }
           else 
           { _LimitSwCCW_Ch = LimitSw_CW_Ch; _LimitSwCW_Ch = LimitSw_CCW_Ch; }
        Serial.println("     DI-Channel matches CCW-end: Ch. " + String(_LimitSwCCW_Ch));
        Serial.println("     DI-Channel matches CW-end : Ch. " + String(_LimitSwCW_Ch));

        /* Main Test, 5 steps */
        for (Counter=0; Counter<=4; Counter++)
            { 
             /* Direction must be reverted when limit is reached */
             if ( ! digitalRead(_LimitSwCCW_Ch) ) Direction = _CW;
                else if ( ! digitalRead(_LimitSwCW_Ch) ) Direction = _CCW;
                     else Direction = 0;
             
             /* Starting time is stored and Servo is actuated */
             StartTime = millis(); CommandToServo(PWM_Tag, 90 + Direction*Speed); 

             /* Wait until reaching to limit at direction */
             if (Direction==-1) while ( digitalRead(_LimitSwCW_Ch) ) { }
                else if (Direction==1) while ( digitalRead(_LimitSwCCW_Ch) ) { }
            
             CommandToServo(PWM_Tag, 90); Duration = millis() - StartTime; Direction *= -1;
             Serial.println("     Duration.#" + String(Counter) + ": " + String(Duration) + " ms");
            }
     }
     else
        Serial.println("Error: Unrecognized tag(s).");
} /* End of "void FullTravelDurationMeasurement()": ARDUINO DUE */


void MoveDuringMillis /* VERIFIED FUNCTION: ARDUINO DUE */
    (String Tag, int Direction, int Speed, unsigned long Milliseconds) {
  /* This function is used to operate a "section" given by "Tag" to "Direction" 
   * at specified "Speed" and during determined "Milliseconds", unless "LimitSw"
   * is activated (Limit Switches give 0 when activated).
   */
  unsigned long StartTime=0, EndTime=0;
  Serial.print("Actuate " + Tag + ", "); 
  switch (Direction) {
         case _CCW: Serial.print("Counter Clockwise, "); break;
         case _CW : Serial.print("Clockwise, "); break;
         case 0   : Serial.print("STOP, "); break;
  }
  Serial.print("Speed="+String(Speed)+", ");
  Serial.println("Time="+String(Milliseconds)+"ms");
  if (IO_Channel(Tag)==-1) Serial.println("Error: Recheck given tag.");
     else 
        { 
          StartTime=millis();
          CommandToServo(Tag, 90 + Direction*Speed);
          while ( (millis()-StartTime)<Milliseconds ) 
                { /* Waits until time is over */ };
          CommandToServo(Tag, 90); EndTime=millis(); /* Stop command to Servo */
          Serial.println(Tag + " STOP: Time reached - " + String(EndTime-StartTime) + "ms.");
        }
} /* End of "void MoveDuringMillis()": ARDUINO DUE */


void MoveToPosition /* VERIFIED FUNCTION: ARDUINO DUE */
     (String Section, int Speed, float SetValue) {
  /* This function is used to operate a "Section" at a determined 
   * "Speed" to required position given by "SetValue".
   * ("Section" is the first 5 characters of any I/O tag.)
   */
  String PWM_Tag=Section+"CM", TX_Tag=Section+"TX"; 
  
  boolean Actuated=0,
          Flag_SPMatched=0,
          Flag_LL=0, Flag_L=0, Flag_H=0, Flag_HH=0,
          Alarmed_1=0, Alarmed_2=0;

  byte    Section_to_Case=0;
  
  int     IncreaseDirection=0, Direction=0,
          TX_RawMin, TX_RawMax,
          Alarm_LL, Alarm_L, Alarm_H, Alarm_HH,
          CurrentTxReading;

  if (Section=="_10WH") { Section_to_Case=1; IncreaseDirection=_CCW; }
     else if (Section=="_10WV") { Section_to_Case=2; IncreaseDirection=_CW; }

  /* Scale and Alarm values corresponding to Sections should be adjusted
   *  depending on tests.
   * Direction equations should be adjusted by multiplying -1 depending on 
   * mechanical configuration.
   */
  switch (Section_to_Case) {
         case 0 : Serial.println("Invalid section."); 
                  break;
         case 1 : /* Customizations for _10WH Section */
                  TX_RawMin=0; TX_RawMax=4095;
                  Alarm_LL=10; Alarm_L=20; Alarm_H=80; Alarm_HH=90;
                  break;
         case 2 : /* Customizations for _10WL Section */
                  TX_RawMin=0; TX_RawMax=4095;
                  Alarm_LL=10; Alarm_L=20; Alarm_H=80; Alarm_HH=90;
                  break;
  }

  CurrentTxReading=map( analogRead(IO_Channel(TX_Tag)), TX_RawMin, TX_RawMax, 0, 100 );

  if (Section_to_Case!=0 && Speed!=0)
     {
      Serial.print("Actuate " + PWM_Tag + ", Pos.Tx:" + TX_Tag + "[=" + String(CurrentTxReading) + "%], ");
      if (IncreaseDirection==_CCW) Direction = (int)( (SetValue-CurrentTxReading)/abs(SetValue-CurrentTxReading) );
         else                      Direction = (int)( (-1)*(SetValue-CurrentTxReading)/abs(SetValue-CurrentTxReading) );
      switch (Direction) {
             case _CCW: Serial.print("CCW, ");
                        break;
             case _CW : Serial.print("CW, ");
                        break;
             case 0   : Serial.print("STOP, ");
                        break;
             }
      Serial.print("Speed="+String(Speed)+", ");
      Serial.println("Set Value="+String(SetValue)+"%");
      while ( !( Flag_SPMatched || Flag_LL || Flag_HH ) )
            { 
             CurrentTxReading=map( analogRead(IO_Channel(TX_Tag)), TX_RawMin, TX_RawMax, 0, 100 );
             if (IncreaseDirection==_CCW)
                {
                 if ( (Direction==_CW)  && (CurrentTxReading<=Alarm_L) )  Flag_L  = 1;
                 if ( (Direction==_CW)  && (CurrentTxReading<=Alarm_LL) ) Flag_LL = 1;
                 if ( (Direction==_CCW) && (CurrentTxReading>=Alarm_H) )  Flag_H  = 1;
                 if ( (Direction==_CCW) && (CurrentTxReading>=Alarm_HH) ) Flag_HH = 1;
                 if ( ( (Direction==_CCW) && (CurrentTxReading>=SetValue) ) || 
                      ( (Direction==_CW)  && (CurrentTxReading<=SetValue) ) ) Flag_SPMatched  = 1;
                }
             if (IncreaseDirection==_CW)
                {
                 if ( (Direction==_CCW) && (CurrentTxReading<=Alarm_L) )  Flag_L  = 1;
                 if ( (Direction==_CCW) && (CurrentTxReading<=Alarm_LL) ) Flag_LL = 1;
                 if ( (Direction==_CW)  && (CurrentTxReading>=Alarm_H) )  Flag_H  = 1;
                 if ( (Direction==_CW)  && (CurrentTxReading>=Alarm_HH) ) Flag_HH = 1;
                 if ( ( (Direction==_CW)  && (CurrentTxReading>=SetValue) ) || 
                      ( (Direction==_CCW) && (CurrentTxReading<=SetValue) ) ) Flag_SPMatched = 1;
                }

             if (Flag_L || Flag_H)
                if (!Alarmed_1)
                   {
                    if ( Flag_L ) Serial.println("Alarm      : "+ String(Alarm_L) + "% is reached.");
                    if ( Flag_H ) Serial.println("Alarm      : "+ String(Alarm_H) + "% is reached.");
                    Alarmed_1 = 1;
                   }

             if (Flag_LL || Flag_HH)
               {  
                CommandToServo(PWM_Tag, 90); /* Interlocked */
                if (!Alarmed_2)
                   {
                    if ( Flag_LL ) Serial.println("Interlocked: "+ String(Alarm_LL) + "% is reached.");
                    if ( Flag_HH ) Serial.println("Interlocked: "+ String(Alarm_HH) + "% is reached.");
                    Alarmed_2 = 1;
                   }
               } 
                else if (! Actuated )
                        { CommandToServo(PWM_Tag, 90+Direction*Speed); Actuated = 1; }
             
            } /* Waits until reaching to setpoint */
      CommandToServo(PWM_Tag, 90); /* Stop command to Servo */
      Serial.println(PWM_Tag + " is stopped at " + String(CurrentTxReading) + "%.");
     } /* End of "if (Section_to_Case!=0)" */
} /* End of "void MoveToPosition()" */


void CommandToLeonardo(String Section, int SetValue) {  /* VERIFIED FUNCTION: ARDUINO DUE */
    /* SetValue is given as Percentage */
    int Section_Num=0;
    if (Section=="_11FR") Section_Num = _FA_Radius;
       else if (Section=="_11FU") Section_Num = _FA_Ulna;
               else if (Section=="_21UA") Section_Num = _UpperArm;
                       else if (Section=="_31SH") Section_Num = _Shoulder;
    if (Section_Num==0) Serial.println("Section cannot be recognized.");
    if ((SetValue<0) || (SetValue>100)) Serial.println("Set Value should be between 0-100).");
    if ((Section_Num!=0) && (SetValue>=0) && (SetValue<=100) )
       {
        Serial.print(String(SetValue) + "% -> ");
        switch(Section_Num){
              case _FA_Radius: Serial.println("Front Arm-Radius"); break;
              case _FA_Ulna  : Serial.println("Front Arm-Ulna"); break;
              case _UpperArm : Serial.println("Upper Arm"); break;
              case _Shoulder : Serial.println("Shoulder"); break;
        }

        analogWrite (_00XXCM, Section_Num);
        analogWrite (_00XXSP, map(SetValue, 0, 100, 0, 4095));
        delay(50);                           // Delay for stabilization of outputs
        digitalWrite(_00XXXC, HIGH);         // Execute Command
        do { } while(!digitalRead(_00XXOK)); // Wait until getting OK from Leonardo
        Serial.println("ARD LNRD = OK.");
        analogWrite (_00XXCM, 0); 
        analogWrite (_00XXSP, 0);            // Resetting Section and Set Value outputs
        delay(50);                           // Delay for stabilization of outputs
        digitalWrite(_00XXXC, LOW);          // Resetting Command
       } /* End of "if (Section_Num!=0)" */
} /* End of "void CommandToLeonardo()" */


void Fingers_Hold() {  
    /* CurrentPos and SetValue are given as Percentage and multipliers of 10.*/
    /* Finger#0 is 01-Metacarpal and #1 is 01-Proximal+Distal.
     * The rest are only for Medial+Distal Sections of respective fingers.
     * All timing values are determined by Speed = 20 ( PWM = 90+/-20 )
     * Some sections should be released in 2 steps, indicated as (x+y).
     * Finger#.............: _01MCCM   _01PDCM _02MDCM    _03MDCM _04MDCM _05MDCM
     * Tigth Direction.....: CCW       CCW     CCW        CCW     CW      CW
     * Full Tight Time...ms: 2000      2500    4000       2500    2500    2500
     * Full Release Time.ms: 1500+500  1500    2000+1000  2000    2000    2000
    */ 

    /* Tighting Order of Fingers: 03-01(MD+PD)-05-02-04
     * 10% tight on each step.
    */
    for (int i=1; i<=5; i++) {
         _03MD_Servo.write(120); delay(500); _03MD_Servo.write(90);
         _01MC_Servo.write(120); delay(200); _01MC_Servo.write(90);
         _01PD_Servo.write(120); delay(250); _01PD_Servo.write(90);
         _05MD_Servo.write(70);  delay(250); _05MD_Servo.write(90);
         _02MD_Servo.write(120); delay(400); _02MD_Servo.write(90);
         _04MD_Servo.write(70);  delay(250); _04MD_Servo.write(90);
        }
} /* End of "int Fingers_Hold()" */


void Fingers_Release() {  
    /* CurrentPos and SetValue are given as Percentage and multipliers of 10.*/
    /* Finger#0 is 01-Metacarpal and #1 is 01-Proximal+Distal.
     * The rest are only for Medial+Distal Sections of respective fingers.
     * All timing values are determined by Speed = 20 ( PWM = 90+/-20 )
     * Some sections should be released in 2 steps, indicated as (x+y).
     * Finger#.............: _01MCCM   _01PDCM _02MDCM    _03MDCM _04MDCM _05MDCM
     * Tigth Direction.....: CCW       CCW     CCW        CCW     CW      CW
     * Full Tight Time...ms: 2000      2500    4000       2500    2500    2500
     * Full Release Time.ms: 1500+500  1500    2000+1000  2000    2000    2000
    */ 

    /* Relesing Order of Fingers: 03-01(MD+PD)-05-02-04
     * 10% tight on each step.
    */
    for (int i=1; i<=5; i++) {
         _03MD_Servo.write(60);  delay(200); _03MD_Servo.write(90);
         _01MC_Servo.write(70);  delay(200); _01MC_Servo.write(90);
         _01PD_Servo.write(70);  delay(150); _01PD_Servo.write(90);
         _05MD_Servo.write(110); delay(200); _05MD_Servo.write(90);
         _02MD_Servo.write(60);  delay(300); _02MD_Servo.write(90);
         _04MD_Servo.write(110); delay(200); _04MD_Servo.write(90);
        }
} /* End of "int Fingers_Release()" */


void Final_Demo(){
   CommandToLeonardo("_31SH", 0);
   CommandToLeonardo("_11FU", 0);
   CommandToLeonardo("_21UA", 0);
   
   for (int i=0;i<=100;i+=10) {
       CommandToLeonardo("_31SH", i);
       if (i<=50) CommandToLeonardo("_11FU", i);
          else  CommandToLeonardo("_11FU", (i-50));
       CommandToLeonardo("_21UA", i);
   }
   /* CommandToLeonardo(String Section, int SetValue) */
   CommandToLeonardo("_31SH", 0);
   CommandToLeonardo("_11FU", 0);
   CommandToLeonardo("_21UA", 0);
}


/* "void loop(): ARDUINO DUE */
void loop() {
  int           User_Int1=0;
  /* TEST CODES: */
  
  /* VERIFIED CODES: */
     /* Commands and Actions */
     /* All recognized commands give a feedback in all cases.
      * If you don't see any feedback on Serial Monitor, check your input
      * for any typewriting errors.
      * Example: "XYZ" command does not exist, so will result as follows:
      *     [ARD DUE/] > XYZ
      *     [ARD DUE/] > 
      */
     if (Serial.available())
        {
         String Command=ReadUserInputOverSerial(); Serial.println(Command); 
         if (Command.substring(0,1)=="?") /* Help */
            {
             Serial.println("Commands:");
             Serial.println("  CTL [Section] [SetValue (0-100)]");
             Serial.println("      CommandToLeonardo - Section: First 5 characters of PWM Tag (e.g. _10WH for _10WHCM).");
             Serial.println("  FD");
             Serial.println("      Final Demo.");
             Serial.println("  FTD [ServoTag] [LimitSwitch1Tag] [LimitSwitch2Tag] [Speed (0-90)]");
             Serial.println("      Full Travel Duration(Test).");
             Serial.println("  HLD");
             Serial.println("      Hold (by Fingers).");
             Serial.println("  LT");
             Serial.println("      Starts Loop Test subroutine.");
             Serial.println("  MDM [ServoTag] [Direction] [Speed (0-90)] [Milliseconds]");
             Serial.println("      Move During Millis-Actuate a Servo during a determined time.");
             Serial.println("      Direction   : Either CC(=CounterClockwise) or CW(=Clockwise)");
             Serial.println("      WARNING:");
             Serial.println("      This function does not check whether limit switch is matching with direction.");
             Serial.println("      Incorrect usage can result damages. Ensure applicable durations by using");
             Serial.println("      FullTravelDurationMeasurement() in advance.");
             Serial.println("  MTP [Section] [Speed (0-90)] [SetValue]");
             Serial.println("      Move To Position - Section: First 5 characters of PWM Tag (e.g. _10WH for _10WHCM).");
             Serial.println("  RD [IO_Tag]");
             Serial.println("      Reads value on a single input channel.");
             Serial.println("  REL");
             Serial.println("      Release (by Fingers).");
             Serial.end(); Serial.begin(_BAUDRATE); while(!Serial) { };
            }
            
         if (Command.substring(0,3)=="CTL")
            CommandToLeonardo(Command.substring(4,9), Command.substring(10).toInt());
            /* CommandToLeonardo(String Section, int SetValue) */

         if (Command.substring(0,2)=="FD")
            Final_Demo();

         if (Command.substring(0,3)=="FTD")
            FullTravelDurationMeasurement(Command.substring(4,11), Command.substring(12,19), Command.substring(20,27), Command.substring(28,30).toInt());
            /* FullTravelDurationMeasurement(PWM-Output, Limit Switch.1, Limit Switch.2 Tag, Speed) */

         if (Command.substring(0,3)=="HLD")
            Fingers_Hold();
         if (Command.substring(0,3)=="REL")
            Fingers_Release();

         if (Command.substring(0,2)=="LT")
            LoopTest();

         if (Command.substring(0,3)=="MDM")
            {
             if (Command.substring(12,14)=="CC") User_Int1=_CCW; 
                else if (Command.substring(12,14)=="CW") User_Int1=_CW;
                     else User_Int1=0;                                   /* Direction */
             MoveDuringMillis(Command.substring(4,11), User_Int1, 
                              Command.substring(15,17).toInt(), (unsigned long)Command.substring(18,22).toInt());
             /* MoveDuringMillis(PWM-Output, Direction, Speed, Milliseconds) */
            }

         if (Command.substring(0,3)=="MTP")
            MoveToPosition(Command.substring(4,9), Command.substring(10,12).toInt(), Command.substring(13).toFloat());
            /* MoveToPosition(Section, Speed, Set Value) */

         if (Command.substring(0,2)=="RD") 
            Read(Command.substring(3,10));
            /* Read(Tag) */ 

         CommandPromptOverSerial("[ARD DUE/] > ");
        } /* End of "if (Serial.available())..." */
} /* End of "void loop()": ARDUINO DUE */ 
Application Program - Arduino LeonardoArduino
This one shall be loaded to Leonardo
#include <Servo.h>

/* Serial Comm.Baudrate Setting: ARDUINO LEONARDO */
    const int _BAUDRATE = 4800;
/* End of Serial Comm.Baudrate Setting: ARDUINO LEONARDO */

/* Tag-Channel(Pin) Assignments and IO_Channel() Function: ARDUINO LEONARDO */
    const int _11FRZL = 0;    // 11: Front Arm Radius ZL
    const int _11FRZH = 1;    // 11: Front Arm Radius ZH
    const int _11FRTX = A2;   // 11: Front Arm Radius Actual Pos
    const int _11FRCM = 9;    // 11: Front Arm Radius CMD

    const int _11FUZL = 2;    // 11: Front Arm Ulna ZL
    const int _11FUZH = 3;    // 11: Front Arm Ulna ZH
    const int _11FUTX = A3;   // 11: Front Arm Ulna Actual Pos
    const int _11FUCM = 10;   // 11: Front Arm Ulna CMD

    const int _21UAZL = 4;    // 21: Upper Arm ZL
    const int _21UAZH = 5;    // 21: Upper Arm ZH
    const int _21UATX = A4;   // 21: Upper Arm Actual Pos
    const int _21UACM = 11;   // 21: Upper Arm CMD

    const int _31SHZL = 6;    // 31: Shoulder ZL
    const int _31SHZH = 7;    // 31: Shoulder ZH
    const int _31SHTX = A5;   // 31: Shoulder Actual Pos
    const int _31SHCM = 13;   // 21: Shoulder CMD

    const int _00XXXC = 8;    // Execute Command (0: Stop/Idle, 1: Execute)
    const int _00XXSP = A0;   // Setpoint Register
    const int _00XXCM = A1;   // Command Selector (*1)
    const int _00XXOK = 12;   // Command Executed by Arduino Leonardo (*2-IMPORTANT)

    int IO_Channel(String Tag) { /* VERIFIED FUNCTION: ARDUINO LEONARDO */
       /* This function is used to validate IO Channel for DI/DO/AO/PWM-O. 
        * If given tag is valid, respective channel number will be returned.
        * Otherwise -1 will be returned as "error".
        */
       if (Tag=="_11FRZL") return(_11FRZL);
       if (Tag=="_11FRZH") return(_11FRZH);
       if (Tag=="_11FRTX") return(_11FRTX);
       if (Tag=="_11FRCM") return(_11FRCM);
       if (Tag=="_11FUZL") return(_11FUZL);
       if (Tag=="_11FUZH") return(_11FUZH);
       if (Tag=="_11FUTX") return(_11FUTX);
       if (Tag=="_11FUCM") return(_11FUCM);
       if (Tag=="_21UAZL") return(_21UAZL);
       if (Tag=="_21UAZH") return(_21UAZH);
       if (Tag=="_21UATX") return(_21UATX);
       if (Tag=="_21UACM") return(_21UACM);
       if (Tag=="_31SHZL") return(_31SHZL);
       if (Tag=="_31SHZH") return(_31SHZH);
       if (Tag=="_31SHTX") return(_31SHTX);
       if (Tag=="_31SHCM") return(_31SHCM);
       if (Tag=="_00XXXC") return(_00XXXC);
       if (Tag=="_00XXSP") return(_00XXSP);
       if (Tag=="_00XXCM") return(_00XXCM);
       if (Tag=="_00XXOK") return(_00XXOK);
       return(-1);
} /* End of "IO_Channel(String Tag)": ARDUINO LEONARDO */
/* End of Tag-Channel(Pin) Assignments and IO_Channel() Function: ARDUINO LEONARDO */


/* Servo _Definitions + CommandToServo() function: ARDUINO LEONARDO */
    Servo _11FR_Servo;
    Servo _11FU_Servo;
    Servo _21UA_Servo;
    Servo _31SH_Servo;

    void CommandToServo(String Tag, int Value) { /* VERIFIED FUNCTION: ARDUINO LEONARDO */
       /* This function is used to actuate PWM outputs to Servo Motors. */
       if (Tag=="_11FRCM") _11FR_Servo.write(Value);
       if (Tag=="_11FUCM") _11FU_Servo.write(Value);
       if (Tag=="_21UACM") _21UA_Servo.write(Value);
       if (Tag=="_31SHCM") _31SH_Servo.write(Value);
    } /* End of "CommandToServo()": ARDUINO LEONARDO */
/* End of Servo _Definitions + CommandToServo() function: ARDUINO LEONARDO */


/* Constant Parameters: ARDUINO LEONARDO */
    const int _DBPercent =  1;  // % Deadband
    const int _CCW       =  1;  // Servo Direction for Counter Clockwise
    const int _CW        = -1;  // Servo Direction for Counter Clockwise
/* End of Constant Parameters: ARDUINO LEONARDO */


/* Constant Parameters for Intercommunication: ARDUINO LEONARDO */
    const int   _FA_Radius  = 310;    // Front Arm Radius Section
    const int   _FA_Ulna    = 425;    // Front Arm Ulna Section
    const int   _UpperArm   = 540;    // Upper Arm Section
    const int   _Shoulder   = 655;    // Front Arm Radius Section
    const int   _ICDB       =  40;    // Front Arm Radius Section
    /* Since the signals cannot generate exact integer values, Section 
     * shall be identified by [ (SectionValue) +/- _ICDB ] reading.
    */
/* End of Constant Parameters for Intercommunication: ARDUINO LEONARDO */


/* "void setup()": ARDUINO LEONARDO */
void setup() {
  // TEST CODES:
  
  // VERIFIED CODES:
    pinMode(_11FRZL, INPUT_PULLUP);
    pinMode(_11FRZH, INPUT_PULLUP);
    pinMode(_11FUZL, INPUT_PULLUP);
    pinMode(_11FUZH, INPUT_PULLUP);
    pinMode(_21UAZL, INPUT_PULLUP);
    pinMode(_21UAZH, INPUT_PULLUP);
    pinMode(_31SHZL, INPUT_PULLUP);
    pinMode(_31SHZH, INPUT_PULLUP);
    pinMode(_00XXXC, INPUT_PULLUP);
    pinMode(_00XXOK, OUTPUT);
    
   _11FR_Servo.attach(_11FRCM);
   _11FU_Servo.attach(_11FUCM);
   _21UA_Servo.attach(_21UACM);
   _31SH_Servo.attach(_31SHCM);

   /* This part is used to send initial "neutral position/stop" 
    * to all PWM outputs assigned to Servo Motors.
    */
   _11FR_Servo.write(90);
   _11FU_Servo.write(90);
   _21UA_Servo.write(90);
   _31SH_Servo.write(90);

   /* Initial Feedback to Due */
   digitalWrite(_00XXOK, LOW); 
   
   /* analogReadResolution(10);  Analog Reading Scale: 0-1023 for ARDUINO LEONARDO*/

   /* analogWriteResolution(10); Analog Writing Scale: 0-1023 for ARDUINO LEONARDO*/

   Serial.begin(_BAUDRATE); while(!Serial) { };  
   Serial.println("? -> Help.");
   Serial.print("[ARD LNRD/] > ");
} /* End of "void setup()": ARDUINO LEONARDO */


/* Global Variables for Test Purposes: ARDUINO LEONARDO */

/* End of Global Variables for Test Purposes: ARDUINO LEONARDO */


/* Verified Global Variables: ARDUINO LEONARDO */
   int CommandFromDue_Section, CommandFromDue_SetValue;
/* End of Verified Global Variables: ARDUINO LEONARDO */


void CommandPromptOverSerial(String _Prompt) { /* VERIFIED FUNCTION: ARDUINO LEONARDO */
  /* Reprompting and flushing serial for next input.
   *  This function will be used after each use of ReadUserInputOverSerial()
  */
  Serial.print(_Prompt); Serial.end(); Serial.begin(_BAUDRATE); while(!Serial) { };
} /* End of "CommandPromtOverSerial()": ARDUINO LEONARDO */


String ReadUserInputOverSerial(){ /* VERIFIED FUNCTION: ARDUINO LEONARDO */
  /* This function will be used only "if (Serial.available())" */
  char   UserInput[32];
  Serial.readBytes(UserInput,32); 
  /* Buffering and capitalizing User Input */
  String Buffer=UserInput; Buffer.toUpperCase();
  return(Buffer);
} /* End of "ReadUserInputOverSerial()": ARDUINO LEONARDO */


void Read(String Tag) { /* VERIFIED FUNCTION: ARDUINO LEONARDO */
  /* Indicates value at input channel defined by "Tag" */
  if ( (Tag.substring(5,7)=="ZL") || (Tag.substring(5,7)=="ZH") || (Tag.substring(5,7)=="OK"))
     Serial.println(Tag + " = " + String(digitalRead(IO_Channel(Tag))));
     else if (Tag.substring(5,7)=="TX")
        Serial.println(Tag + " = " + String(analogRead(IO_Channel(Tag))));
        else Serial.println(Tag + "is not an input.");
} /* End of "void Read()": ARDUINO LEONARDO */


void LoopTest() { /* VERIFIED FUNCTION: ARDUINO LEONARDO */
 String Command, User_Tag;
 Serial.println("? -> Help, X -> Terminate");
 CommandPromptOverSerial("[ARD LNRD/LoopTest/] > "); 
 do
  {
     if (Serial.available())
        {
         byte   Command_to_Case=0; /* Command is invalid if remains as 0. */
         int    User_Value=0, User_Value_Last=0, Reading=0, Reading_Last=-1;
         String Buffer = ReadUserInputOverSerial();
         Serial.println(Buffer); Serial.end(); Serial.begin(_BAUDRATE); while(!Serial) { };
         
         /* Seperating Command and Tag*/
         Command = Buffer.substring(0,2); User_Tag = Buffer.substring(3,10);
         
         /* Converting (string)Command to (byte)Command_to_Case */
         if (Command=="AI") Command_to_Case=11;
         if (Command=="AO") Command_to_Case=12;
         if (Command=="DI") Command_to_Case=21;
         if (Command=="DO") Command_to_Case=22;
         if (Command=="PO") Command_to_Case=32;
         if (Command[0]=='?') Command_to_Case=98;   // Display Help
         if (Command[0]=='X') Command_to_Case=99;   // Terminate Test
         
         /* Checking whether tag is valid. */
         if ((Command_to_Case!=0)&&(Command_to_Case<90)&&(IO_Channel(User_Tag)<0))
            Command_to_Case=1; /* Redirecting to unrecognized tag case. */

         /* Taking respective actions and giving feedbacks for each command. */
         switch (Command_to_Case) {
               case 0 : Serial.println("Invalid command.");
                        CommandPromptOverSerial("[ARD LNRD/LoopTest/] > ");
                        break;
               case 1 : Serial.println("Tag not exists: " + User_Tag);
                        CommandPromptOverSerial("[ARD LNRD/LoopTest/] > ");
                        break;
               case 11: Serial.println("AI-Test('X'->Terminate): " + User_Tag);
                        Reading_Last = -1;
                        do
                          {
                            Buffer = ReadUserInputOverSerial();
                            Reading = analogRead(IO_Channel(User_Tag));
                            if (Reading != Reading_Last)
                               {
                                Serial.println(User_Tag + " = " + String(Reading));
                                Reading_Last = Reading; delay(1000);
                                /* 1s delay is used to avoid fast updates on Serial Monitor */
                               }
                          }
                        while (Buffer[0]!='X');
                        Serial.println();
                        CommandPromptOverSerial("[ARD LNRD/LoopTest/] > ");
                        break;
               case 12: Serial.println("AO-Test('X'->Terminate): " + User_Tag);
                        Serial.println("Integer Value(0-1023): ");
                        Buffer="";
                        do
                          { 
                            if (Serial.available())
                               {
                                Buffer = ReadUserInputOverSerial();
                                if (Buffer[0]=='X') User_Value=0; else User_Value=Buffer.substring(0).toInt();
                                Serial.println(String(User_Value) + " -> " + User_Tag);
                                if (User_Value!=User_Value_Last) 
                                   {
                                    analogWrite(IO_Channel(User_Tag),User_Value);
                                    User_Value_Last = User_Value;
                                   }
                                   else analogWrite(IO_Channel(User_Tag),User_Value_Last);
                                Serial.print("Integer Value(0-1023): ");
                                Serial.end(); Serial.begin(_BAUDRATE); while(!Serial) { };
                               } /* End of "if (Serial.available())..." */
                          }
                        while (Buffer[0]!='X');
                        Serial.println();
                        CommandPromptOverSerial("[ARD LNRD/LoopTest/] > ");
                        break;
               case 21: Serial.println("DI-Test('X'->Terminate): " + User_Tag); 
                        Reading_Last = -1;
                        do
                          {
                            Buffer = ReadUserInputOverSerial();
                            Reading = digitalRead(IO_Channel(User_Tag));
                            if (Reading != Reading_Last)
                               {
                                Serial.println(User_Tag + " = " + String(Reading));
                                Reading_Last = Reading;
                               }
                          }
                        while (Buffer[0]!='X');
                        Serial.println();
                        CommandPromptOverSerial("[ARD LNRD/LoopTest/] > ");
                        break;
               case 22: Serial.println("DO-Test('X'->Terminate): " + User_Tag);
                        Serial.print("Enter 0 or 1: ");
                        Buffer="";
                        do
                          { 
                            if (Serial.available())
                               {
                                Buffer = ReadUserInputOverSerial();
                                if (Buffer[0]=='X') User_Value=0; else User_Value=Buffer.substring(0).toInt();
                                Serial.println(String(User_Value) + " -> " + User_Tag);
                                if (((User_Value==0)||(User_Value==1))&&(User_Value!=User_Value_Last))
                                   {
                                    
                                    digitalWrite(IO_Channel(User_Tag),User_Value);
                                    User_Value_Last = User_Value;
                                   }
                                   else digitalWrite(IO_Channel(User_Tag),User_Value_Last);
                                Serial.print("Enter 0 or 1: ");
                                Serial.end(); Serial.begin(_BAUDRATE); while(!Serial) { };
                               } /* End of "if (Serial.available())..." */
                          }
                        while (Buffer[0]!='X');
                        Serial.println();
                        CommandPromptOverSerial("[ARD LNRD/LoopTest/] > ");
                        break;
               case 32: Serial.println("PWM-Test('X'->Terminate): " + User_Tag);
                        CommandToServo(User_Tag,90); /* Sending "stop" to selected tag */
                        Serial.print("Integer Value(0-180): ");
                        Buffer="";
                        do
                          { 
                            if (Serial.available())
                               {
                                Buffer = ReadUserInputOverSerial();
                                if (Buffer[0]=='X') User_Value=90; else User_Value=Buffer.substring(0).toInt();
                                Serial.println(String(User_Value) + " -> " + User_Tag);
                                if (((User_Value>=0)||(User_Value<=180))&&(User_Value!=User_Value_Last))
                                   { CommandToServo(User_Tag,User_Value); User_Value_Last = User_Value; }
                                   else CommandToServo(User_Tag,User_Value);
                                Serial.print("Integer Value(0-180): ");
                                Serial.end(); Serial.begin(_BAUDRATE); while(!Serial) { };
                               } /* End of "if (Serial.available())..." */
                          }
                        while (Buffer[0]!='X');
                        Serial.println();
                        CommandPromptOverSerial("[ARD LNRD/LoopTest/] > ");
                        break;
               case 98: Serial.println("Usage: '[CC] [Tag]'");
                        Serial.println("    [CC]: Channel Type");
                        Serial.println("          AI, AO: Analog Input/Output");
                        Serial.println("          DI, DO: Digital Input/Output");
                        Serial.println("          PO: PWM Output for Servo Motor");
                        Serial.println("Other Commands:");
                        Serial.println("      ? : Help (this display)");
                        Serial.println("      X : Exit from Loop Testing");
                        CommandPromptOverSerial("[ARD LNRD/LoopTest/] > ");
                        break;                        
               case 99: Serial.println("Loop Test is terminated.");
                        break;                        
             } /* End of "switch (Command_to_Case)..." */
        } /* End of "if (Serial.available())..." */
  }
 while (Command[0]!='X');

} /* End of "void LoopTest()": ARDUINO LEONARDO */


void MoveToPosition /* VERIFIED FUNCTION: ARDUINO LEONARDO */
     (String Section, int Speed, int SetValue) {
  /* This function is used to operate a "Section" at a determined 
   * "Speed" to required position given by "SetValue".
   * ("Section" is the first 5 characters of any I/O tag.)
   */
  String PWM_Tag=Section+"CM", TX_Tag=Section+"TX"; 
  
  boolean Actuated=0,
          Flag_SPMatched=0,
          Flag_LL=0, Flag_L=0, Flag_H=0, Flag_HH=0,
          Alarmed_1=0, Alarmed_2=0;

  byte    Section_to_Case=0;
  
  int     IncreaseDirection=0, Direction=0,
          TX_RawMin, TX_RawMax,
          Alarm_LL, Alarm_L, Alarm_H, Alarm_HH,
          CurrentTxReading;

  if (Section=="_11FR") { Section_to_Case=1; IncreaseDirection=_CCW; }
     else if (Section=="_11FU") { Section_to_Case=2; IncreaseDirection=_CCW; }
             else if (Section=="_21UA") { Section_to_Case=3; IncreaseDirection=_CW; }
                     else if (Section=="_31SH") { Section_to_Case=4; IncreaseDirection=_CW; }
      
  /* Scale and Alarm values corresponding to Sections should be adjusted
   *  depending on tests.
   * Direction equations should be adjusted by multiplying -1 depending on 
   * mechanical configuration.
   */
  switch (Section_to_Case) {
         case 0 : Serial.println("Invalid section."); 
                  break;
         case 1 : /* Customizations for _11FR Section */
                  TX_RawMin=0; TX_RawMax=1023;
                  Alarm_LL=10; Alarm_L=20; Alarm_H=80; Alarm_HH=90;
                  break;
         case 2 : /* Customizations for _11FU Section */
                  TX_RawMin=690; TX_RawMax=230; 
                  Alarm_LL=5; Alarm_L=10; Alarm_H=80; Alarm_HH=90;
                  break;
         case 3 : /* Customizations for _21UA Section - Completed */
                  TX_RawMin=775; TX_RawMax=520;
                  Alarm_LL=10; Alarm_L=20; Alarm_H=80; Alarm_HH=90;
                  break;
         case 4 : /* Customizations for _31SH Section - Completed */
                  TX_RawMin=250; TX_RawMax=760;
                  Alarm_LL=10; Alarm_L=20; Alarm_H=80; Alarm_HH=90;
                  break;
     }
  
  CurrentTxReading=map( analogRead(IO_Channel(TX_Tag)), TX_RawMin, TX_RawMax, 0, 100 );
   
  if (Section_to_Case!=0 && Speed!=0)
     {
      Serial.print("Actuate " + PWM_Tag + ", Pos.Tx:" + TX_Tag + "[=" + String(CurrentTxReading) + "%], ");
      if (IncreaseDirection==_CCW) Direction = (int)( (SetValue-CurrentTxReading)/abs(SetValue-CurrentTxReading) );
         else                      Direction = (int)( (-1)*(SetValue-CurrentTxReading)/abs(SetValue-CurrentTxReading) );
      switch (Direction) {
             case _CCW: Serial.print("CCW, ");
                        break;
             case _CW : Serial.print("CW, ");
                        break;
             case 0   : Serial.print("STOP, ");
                        break;
             }
      Serial.print("Speed="+String(Speed)+", ");
      Serial.println("Set Value="+String(SetValue)+"%");
      while ( !( Flag_SPMatched || Flag_LL || Flag_HH ) )
            { 
             CurrentTxReading=map( analogRead(IO_Channel(TX_Tag)), TX_RawMin, TX_RawMax, 0, 100 );
             if (IncreaseDirection==_CCW)
                {
                 if ( (Direction==_CW)  && (CurrentTxReading<=Alarm_L) )  Flag_L  = 1;
                 if ( (Direction==_CW)  && (CurrentTxReading<=Alarm_LL) ) Flag_LL = 1;
                 if ( (Direction==_CCW) && (CurrentTxReading>=Alarm_H) )  Flag_H  = 1;
                 if ( (Direction==_CCW) && (CurrentTxReading>=Alarm_HH) ) Flag_HH = 1;
                 if ( ( (Direction==_CCW) && (CurrentTxReading>=SetValue) ) || 
                      ( (Direction==_CW)  && (CurrentTxReading<=SetValue) ) ) Flag_SPMatched  = 1;
                }
             if (IncreaseDirection==_CW)
                {
                 if ( (Direction==_CCW) && (CurrentTxReading<=Alarm_L) )  Flag_L  = 1;
                 if ( (Direction==_CCW) && (CurrentTxReading<=Alarm_LL) ) Flag_LL = 1;
                 if ( (Direction==_CW)  && (CurrentTxReading>=Alarm_H) )  Flag_H  = 1;
                 if ( (Direction==_CW)  && (CurrentTxReading>=Alarm_HH) ) Flag_HH = 1;
                 if ( ( (Direction==_CW)  && (CurrentTxReading>=SetValue) ) || 
                      ( (Direction==_CCW) && (CurrentTxReading<=SetValue) ) ) Flag_SPMatched = 1;
                }

             if (Flag_L || Flag_H)
                if (!Alarmed_1)
                   {
                    if ( Flag_L ) Serial.println("Alarm      : "+ String(Alarm_L) + "% is reached.");
                    if ( Flag_H ) Serial.println("Alarm      : "+ String(Alarm_H) + "% is reached.");
                    Alarmed_1 = 1;
                   }

             if (Flag_LL || Flag_HH)
               {  
                CommandToServo(PWM_Tag, 90); /* Interlocked */
                if (!Alarmed_2)
                   {
                    if ( Flag_LL ) Serial.println("Interlocked: "+ String(Alarm_LL) + "% is reached.");
                    if ( Flag_HH ) Serial.println("Interlocked: "+ String(Alarm_HH) + "% is reached.");
                    Alarmed_2 = 1;
                   }
               } 
                else if (! Actuated )
                        { CommandToServo(PWM_Tag, 90+Direction*Speed); Actuated = 1; }
             
            } /* Waits until reaching to setpoint */
      CommandToServo(PWM_Tag, 90); /* Stop command to Servo */
      Serial.println(PWM_Tag + " is stopped at " + String(CurrentTxReading) + "%.");
     } /* End of "if (Section_to_Case!=0)" */
} /* End of "void MoveToPosition()" */


void CommandFromDue() {  /* VERIFIED FUNCTION: ARDUINO LEONARDO */
    CommandFromDue_Section = analogRead(_00XXCM);
       if ( ((_FA_Radius-_ICDB)<=CommandFromDue_Section) && (CommandFromDue_Section<=(_FA_Radius+_ICDB)) )
          CommandFromDue_Section=_FA_Radius;
          else if ( ((_FA_Ulna-_ICDB)<=CommandFromDue_Section) && (CommandFromDue_Section<=(_FA_Ulna+_ICDB)) )
                  CommandFromDue_Section=_FA_Ulna;
                  else if ( ((_UpperArm-_ICDB)<=CommandFromDue_Section) && (CommandFromDue_Section<=(_UpperArm+_ICDB)) )
                          CommandFromDue_Section=_UpperArm;
                          else if ( ((_Shoulder-_ICDB)<=CommandFromDue_Section) && (CommandFromDue_Section<=(_Shoulder+_ICDB)) )
                                  CommandFromDue_Section=_Shoulder;
                                  else
                                  CommandFromDue_Section=0;

    CommandFromDue_SetValue = map(analogRead(_00XXSP),168,846,0,100);
    /* Due generates DAC outputs between 0.55-2.75 V and it corresponds
     * (int) 168-846 on Arduino Leonardo.
     * Here analog reading is converted to Percentage.
     */
    
    boolean CommandFromDue_Execute = digitalRead(_00XXXC);
    String  Section = "";
    int     Speed   = 0;
    if ((CommandFromDue_Section!=0) && CommandFromDue_Execute)
       {
        Serial.println();
        Serial.print("Command by ARD DUE: "+ String(CommandFromDue_SetValue) + "% -> ");
        switch(CommandFromDue_Section){
              case _FA_Radius: Serial.println("Front Arm-Radius");
                               Section = "_11FR";
                               Speed = 20;  /* to be adjusted depending on tests */
                               break;
              case _FA_Ulna  : Serial.println("Front Arm-Ulna");
                               Section = "_11FU";
                               Speed = 20;  /* to be adjusted depending on tests */
                               break;
              case _UpperArm : Serial.println("Upper Arm");
                               Section = "_21UA";
                               Speed = 90;  /* to be adjusted depending on tests */
                               break;
              case _Shoulder : Serial.println("Shoulder");
                               Section = "_31SH";
                               Speed = 30;  /* to be adjusted depending on tests */
                               break;
            }
        Serial.println(" -action-");
        MoveToPosition(Section, Speed, CommandFromDue_SetValue);
        Serial.println("OK -> ARD DUE.");
        CommandPromptOverSerial("[ARD LNRD/] > ");
        digitalWrite(_00XXOK, HIGH); delay(100); digitalWrite(_00XXOK, LOW); // OK Feedback to Due
       } /* End of "if (Section_Num!=0)" */
} /* End of "void CommandToLeonardo()" */


/* "void loop(): ARDUINO LEONARDO */
void loop() {
  /* TEST CODES: */
  CommandFromDue();
  /* VERIFIED CODES: */
     /* Commands and Actions */
     /* All recognized commands give a feedback in all cases.
      * If you don't see any feedback on Serial Monitor, check your input
      * for any typewriting errors.
      * Example: "XYZ" command does not exist, so will result as follows:
      *     [ARD LNRD/] > XYZ
      *     [ARD LNRD/] > 
      */
     if (Serial.available())
        {
         String Command=ReadUserInputOverSerial(); Serial.println(Command); 
         if (Command.substring(0,1)=="?") /* Help */
            {
             Serial.println("Commands:");
             Serial.println("  CFD");
             Serial.println("      Reads Command from ARD DUE.");
             Serial.println("  LT");
             Serial.println("      Starts Loop Test subroutine.");
             Serial.println("  MTP [Section] [Speed (0-90)] [SetValue]");
             Serial.println("      Move To Position - Section: First 5 characters of PWM Tag (e.g. _10WH for _10WHCM).");
             Serial.println("  RD [IO_Tag]");
             Serial.println("      Reads value on a single input channel.");
               /* This function is used to operate a "section" given by "Tag" to "Direction" 
              * at specified "Speed" and during determined "Milliseconds", unless "LimitSw"
               * is activated (Limit Switches give 0 when activated).
               */
             Serial.end(); Serial.begin(_BAUDRATE); while(!Serial) { };
            }
            
         if (Command.substring(0,3)=="CFD")
            CommandFromDue();

         if (Command.substring(0,2)=="LT")
            LoopTest();

         if (Command.substring(0,3)=="MTP")
            MoveToPosition(Command.substring(4,9), Command.substring(10,12).toInt(), Command.substring(13).toInt());
            /* MoveToPosition(Section, Speed, Set Value) */

         if (Command.substring(0,2)=="RD") 
            Read(Command.substring(3,10));
            /* Read(Tag) */ 


         CommandPromptOverSerial("[ARD LNRD/] > ");
        } /* End of "if (Serial.available())..." */
} /* End of "void loop()": ARDUINO LEONARDO */ 

Schematics

Instr.List, I/O List, Terminal Board Layout, Wiring Diagram
electronic_design_s0QkNCSRA4.xls

Comments

Similar projects you might like

Bluetooth Controlled Servo

Project tutorial by JANAK13

  • 7,459 views
  • 16 comments
  • 32 respects

Tobor, the Arm

Project showcase by NerdFatherRJ

  • 3,753 views
  • 1 comment
  • 7 respects

Tricks for Controlling DC Motors

Project tutorial by tolgadurudogan

  • 22,127 views
  • 6 comments
  • 79 respects

Gesture Mouse

by Team ONYX

  • 8,854 views
  • 0 comments
  • 17 respects

Windows IoT//Build 2015 B15 HoloBot

Project showcase by Team Windows IoT

  • 3,088 views
  • 0 comments
  • 7 respects

Embedis: Project Keystore Library

by Team thingSoC

  • 1,730 views
  • 1 comment
  • 8 respects
Add projectSign up / Login