Welcome to Hackster!
Hackster is a community dedicated to learning hardware, from beginner to pro. Join us, it's free!
tolgadurudogan
Published © GPL3+

A Chronicle of Fails

A humanoid arm + hand prototype made of balsa wood

ExpertProtip2,828
A Chronicle of Fails

Things used in this project

Hardware components

Arduino Due
Arduino Due
×1
Arduino Leonardo
Arduino Leonardo
×1
Jumper wires (generic)
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 Micro-servo motor
SG90 Micro-servo motor
×14
SM-4303 Standard Servo Motor
×2
Flat Washer 1/4" Screw Size
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

Software apps and online services

Arduino IDE
Arduino IDE

Story

Read more

Schematics

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

Code

Application Program - Arduino Due

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

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

Credits

tolgadurudogan
4 projects • 23 followers
Contact

Comments

Please log in or sign up to comment.