Revision 8:d98e2dec0f40, committed 2015-12-31

Comitter:

henryrawas

Date:

Thu Dec 31 17:47:55 2015 +0000

Parent:

7:6723f6887d00

Child:

9:a0fb6c370dbb

Commit message:

add taps

Changed in this revision

--- a/AX-12A.lib	Tue Dec 29 23:31:28 2015 +0000
+++ b/AX-12A.lib	Thu Dec 31 17:47:55 2015 +0000
@@ -1,1 +1,1 @@
-http://developer.mbed.org/teams/robot-arm-demo-team/code/AX-12A/#a702043b1420
+http://developer.mbed.org/teams/robot-arm-demo-team/code/AX-12A/#155ecc801119

--- a/ActionBuf.cpp	Tue Dec 29 23:31:28 2015 +0000
+++ b/ActionBuf.cpp	Thu Dec 31 17:47:55 2015 +0000
@@ -28,21 +28,14 @@
 ActionSequence::ActionSequence(SequenceAction aType)
 {
     ActionType = aType;
+    Param = 0;
 };
 
-ActionSequence::ActionSequence(SequenceAction aType, vector<float>& vals, int ms)
+ActionSequence::ActionSequence(SequenceAction aType, vector<float>& vals, int param)
 {
     ActionType = aType;
-    
-    if (aType == SA_SetGoal)
-    {
-        GoalVals = vals;
-        Ms = ms;
-    }
-    else if (aType == SA_Delay)
-    {
-        Ms = ms;
-    }
+    GoalVals = vals;
+    Param = param;
 }
 
 void ActionSequence::SetGoal(vector<float>& vals)
@@ -50,9 +43,9 @@
     GoalVals = vals;
 }
 
-void ActionSequence::SetDelay(int delay)
+void ActionSequence::SetParam(int param)
 {
-    Ms = delay;
+    Param = param;
 }
     
 void ActionSequence::SetAction(SequenceAction aType)

--- a/ActionBuf.h	Tue Dec 29 23:31:28 2015 +0000
+++ b/ActionBuf.h	Thu Dec 31 17:47:55 2015 +0000
@@ -43,11 +43,11 @@
     
     ActionSequence(SequenceAction aType);
     
-    ActionSequence(SequenceAction aType, vector<float>& vals, int ms);
+    ActionSequence(SequenceAction aType, vector<float>& vals, int param);
     
     void SetGoal(vector<float>& vals);
     
-    void SetDelay(int ms);
+    void SetParam(int param);
     
     void SetAction(SequenceAction aType);
     
@@ -55,7 +55,7 @@
     
     vector<float> GoalVals;
     
-    int Ms;
+    int Param;
     
 };
 

--- a/Alert.cpp	Tue Dec 29 23:31:28 2015 +0000
+++ b/Alert.cpp	Thu Dec 31 17:47:55 2015 +0000
@@ -35,14 +35,30 @@
 
 void Alert::SetPositionAlert(int severity, time_t created, int partIx, float diff)
 {
-    char* msg = "Arm could not be moved to desired position. Part %d is off by %f";
+    char* msg = "Arm part failed to move to desired position. Part %d is off by %f";
     int slen = sprintf_s(Msg, AlertMsgMaxLen, msg, partIx, diff);
     Sev = severity;
     Created = created;
     
     strcpy(AlertType, "Position");
-    if (slen < 0)
-    {
-        strcpy(Msg, "Arm could not be moved to desired position.");
-    }
+}
+
+void Alert::SetHardwareAlert(int severity, time_t created, int partIx, int code)
+{
+    char* msg = "Arm part reported an error. Part %d error code %d";
+    int slen = sprintf_s(Msg, AlertMsgMaxLen, msg, partIx, code);
+    Sev = severity;
+    Created = created;
+    
+    strcpy(AlertType, "Hardware");
 }
+
+void Alert::SetTemperatureAlert(int severity, time_t created, int partIx, float temp)
+{
+    char* msg = "Arm part reported a high temperature. Part %d temperature %f";
+    int slen = sprintf_s(Msg, AlertMsgMaxLen, msg, partIx, temp);
+    Sev = severity;
+    Created = created;
+    
+    strcpy(AlertType, "Temperature");
+}

--- a/Alert.h	Tue Dec 29 23:31:28 2015 +0000
+++ b/Alert.h	Thu Dec 31 17:47:55 2015 +0000
@@ -27,6 +27,10 @@
     
     void SetPositionAlert(int severity, time_t created, int partIx, float diff);
     
+    void SetHardwareAlert(int severity, time_t created, int partIx, int code);
+    
+    void SetTemperatureAlert(int severity, time_t created, int partIx, float temp);
+    
     int Sev;
     
     char AlertType[AlertTypeMaxLen];

--- a/ArmController.cpp	Tue Dec 29 23:31:28 2015 +0000
+++ b/ArmController.cpp	Thu Dec 31 17:47:55 2015 +0000
@@ -17,6 +17,16 @@
 
 using namespace std;
 
+void DispMeasure(char* label, int partSize, vector<float>& vals)
+{
+    printf("%s: ", label);
+    for (int ix = 0; ix < partSize; ix++)
+    {
+        printf("%d:%f ", ix, vals[ix]); 
+    }
+    printf("\r\n");
+
+}
 
 void PushMeasurements(MeasureGroup measureGroup, int partSize, RobotArm robotArm)
 {
@@ -24,34 +34,26 @@
     time_t seconds = time(NULL);
     
     robotArm.GetArmMeasure(NM_Temperature, lastVals);
-    printf( "Temperatures: ");
-    for( int servoIndex = 0; servoIndex < partSize; servoIndex++ )
-    {
-        printf( "%d:%f ", servoIndex, lastVals[servoIndex]); 
-    }
-    printf( "\r\n");
+    DispMeasure("Temperatures", partSize, lastVals);
     measureGroup.SetMeasure(NM_Temperature, seconds, lastVals);
     MeasureBuf.push(measureGroup);
-    
+        
     robotArm.GetArmMeasure(NM_Voltage, lastVals);
-    printf( "Voltage: ");
-    for( int servoIndex = 0; servoIndex < partSize; servoIndex++ )
-    {
-        printf( "%d:%f ", servoIndex, lastVals[servoIndex]); 
-    }
-    printf( "\r\n");
+    DispMeasure("Voltage", partSize, lastVals);
     measureGroup.SetMeasure(NM_Voltage, seconds, lastVals);
     MeasureBuf.push(measureGroup);
     
     robotArm.GetArmMeasure(NM_Degrees, lastVals);
-    printf( "Rotation: ");
-    for( int servoIndex = 0; servoIndex < partSize; servoIndex++ )
-    {
-        printf( "%d:%f ", servoIndex, lastVals[servoIndex]); 
-    }
-    printf( "\r\n");
+    DispMeasure("Rotation", partSize, lastVals);
     measureGroup.SetMeasure(NM_Degrees, seconds, lastVals);
     MeasureBuf.push(measureGroup);
+    
+//    robotArm.GetArmMeasure(NM_Load, lastVals);
+//    DispMeasure("Load", partSize, lastVals);
+//    measureGroup.SetMeasure(NM_Load, seconds, lastVals);
+//    MeasureBuf.push(measureGroup);
+
+    SendIothubData();
 }
 
 void PushAlert(int severity, char* msg, char* atype)
@@ -61,6 +63,7 @@
     
     alert.SetAlert(severity, seconds, msg, atype);
     AlertBuf.push(alert);
+    SendIothubData();
 }
 
 void PushPositionAlert(int severity, int partIx, float pos)
@@ -70,9 +73,28 @@
     
     alert.SetPositionAlert(severity, seconds, partIx, pos);
     AlertBuf.push(alert);
+    SendIothubData();
 }
 
+void PushHardwareAlert(int severity, int partIx, int code)
+{
+    Alert alert;
+    time_t seconds = time(NULL);
+    
+    alert.SetHardwareAlert(severity, seconds, partIx, code);
+    AlertBuf.push(alert);
+    SendIothubData();
+}
 
+void PushTemperatureAlert(int severity, int partIx, float temp)
+{
+    Alert alert;
+    time_t seconds = time(NULL);
+    
+    alert.SetTemperatureAlert(severity, seconds, partIx, temp);
+    AlertBuf.push(alert);
+    SendIothubData();
+}
 
 vector<float> initPositions;
 
@@ -98,23 +120,43 @@
 
 osThreadId mainTid;
 
+// send alert if temperature is above 69C and return true
+#define MaxTemp     69
+bool TestTemperature(vector<float>& vals)
+{
+    bool err = false;
+
+    for (int ix = 0; ix < vals.size(); ix++)
+    {
+        if (vals[ix] > MaxTemp)
+        {
+            printf("Temperature threshold exceeded for part %d, temp %f \r\n", ix, vals[ix]); 
+            PushTemperatureAlert(AS_Error, ix, vals[ix]);
+            MainState = MS_Error;
+            err = true;
+        }
+    }
+    return err;
+}
+
+
 void ControlArm(const char* cmd)
 {
     if (strncmp(cmd, "pause", 5) == 0)
     {
-        printf( "Pause cmd\r\n"); 
+        printf("Pause cmd\r\n"); 
         MainState = MS_Paused;
         osSignalSet(mainTid, AS_Action);
     }
     else if (strncmp(cmd, "resume", 6) == 0)
     {
-        printf( "Resume cmd\r\n"); 
+        printf("Resume cmd\r\n"); 
         MainState = MS_Running;
         osSignalSet(mainTid, AS_Action);
     }
     else if (strncmp(cmd, "runupdown", 9) == 0)
     {
-        printf( "Runupdown cmd\r\n"); 
+        printf("Runupdown cmd\r\n"); 
         MainState = MS_CancelToM;
         CancelToMatch = &UpDownSeq;
         SequenceQ.put(&UpDownSeq);
@@ -122,7 +164,7 @@
     }
     else if (strncmp(cmd, "runuptwist", 10) == 0)
     {
-        printf( "Runuptwist cmd\r\n"); 
+        printf("Runuptwist cmd\r\n"); 
         MainState = MS_CancelToM;
         CancelToMatch = &UpTwistSeq;
         SequenceQ.put(&UpTwistSeq);
@@ -130,7 +172,7 @@
     }
     else if (strncmp(cmd, "runhome", 10) == 0)
     {
-        printf( "Runhome cmd\r\n"); 
+        printf("Runhome cmd\r\n"); 
         MainState = MS_CancelToM;
         CancelToMatch = &StartSeq;
         SequenceQ.put(&StartSeq);
@@ -138,15 +180,23 @@
     }
     else if (strncmp(cmd, "runwave", 10) == 0)
     {
-        printf( "Runwave cmd\r\n"); 
+        printf("Runwave cmd\r\n"); 
         MainState = MS_CancelToM;
         CancelToMatch = &WaveSeq;
         SequenceQ.put(&WaveSeq);
         osSignalSet(mainTid, AS_Action);
     }
+    else if (strncmp(cmd, "runtaps1", 10) == 0)
+    {
+        printf("Runtaps1cmd\r\n"); 
+        MainState = MS_CancelToM;
+        CancelToMatch = &TapsSeq;
+        SequenceQ.put(&TapsSeq);
+        osSignalSet(mainTid, AS_Action);
+    }
     else if (strncmp(cmd, "cancelone", 9) == 0)
     {
-        printf( "CancelOne cmd\r\n"); 
+        printf("CancelOne cmd\r\n"); 
         MainState = MS_CancelOne;
         osSignalSet(mainTid, AS_Action);
     }
@@ -181,7 +231,6 @@
     
     // do inital status report
     PushMeasurements(measureGroup, partSize, robotArm);
-    SendIothubData();
  
     // Prepare main thread
     mainTid = osThreadGetId();
@@ -214,19 +263,19 @@
                 break;
                 
             case MS_Paused:
-                printf( "Paused\r\n"); 
+                printf("Paused\r\n"); 
                 break;
                 
             case MS_Stopping:
-                printf( "Stopping\r\n"); 
+                printf("Stopping\r\n"); 
                 break;
                 
             case MS_Error:
-                printf( "Error\r\n"); 
+                printf("Error\r\n"); 
                 break;
                 
             case MS_CancelOne:
-                printf( "Cancel sequence\r\n"); 
+                printf("Cancel sequence\r\n"); 
                 RunInSequence = false;
                 RunInMoveStep = false;
                 if (RunInDelay)
@@ -288,7 +337,7 @@
                     osEvent evt = SequenceQ.get(0);
                     if (evt.status == osEventMessage)
                     {
-                        printf( "New Seq \r\n"); 
+                        printf("New Seq \r\n"); 
                         curseq = (vector<ActionSequence> *)evt.value.p;
                         curseqIx = 0;
                         RunInSequence = true;
@@ -305,15 +354,13 @@
                             // report position error
                             int ix = robotArm.GetLastErrorPart();
                             float diff = robotArm.GetLastPosDiff();
-                            printf( "Position error detected part %d, diff %f \r\n", ix, diff); 
+                            printf("Position error detected part %d, diff %f \r\n", ix, diff); 
                             PushPositionAlert(AS_Error, ix, diff);
                             MainState = MS_Error;
-                            SendIothubData();
                             break;
                         }
                         if (robotArm.MoveArmPositionsHasNext())
                         {
-                            //printf( "Next Step\r\n");
                             int delaystep = 0;
                             bool ok = robotArm.MoveArmPositionsNext();
                             if (ok)
@@ -332,21 +379,23 @@
                             else
                             {
                                 // report HW error
-                                // int partix = robotArm.GetLastErrorPart();
-                                // int errCode = robotArm.GetLastError();
-                                // send alert
+                                int partix = robotArm.GetLastErrorPart();
+                                int errCode = robotArm.GetLastError();
+                                printf("Hardware error detected part %d, code %d \r\n", partix, errCode); 
+                                PushHardwareAlert(AS_Error, partix, errCode);
                                 MainState = MS_Error;
+                                break;
                             }
                         }
                         else
                         {
-                            printf( "No more Step\r\n");
+                            printf("No more Step\r\n");
                             RunInMoveStep = false;
                         }
                     }
                     if (!RunInMoveStep)
                     {
-                        printf( "Next Seq %d\r\n", curseqIx); 
+                        printf("Next Seq %d\r\n", curseqIx); 
                         
                         if (curseq != NULL)
                         {
@@ -359,41 +408,33 @@
                             ActionSequence aseq = (*curseq)[curseqIx];
                             curseqIx++;
                             
-                            bool ok;
                             switch (aseq.ActionType)
                             {
                                 case SA_SetGoal:
-                                    printf( " - Move arm start\r\n"); 
-                                    ok = robotArm.MoveArmPositionsStart(aseq.GoalVals, aseq.Ms);
-                                    if (ok)
-                                    {
-                                        RunInMoveStep = true;
-                                        osSignalSet(mainTid, AS_Action);
-                                    }
-                                    else
-                                    {
-                                        // TODO: send alert
-                                    }
+                                    printf(" - Move arm start\r\n"); 
+                                    robotArm.MoveArmPositionsStart(aseq.GoalVals, aseq.Param);
+                                    RunInMoveStep = true;
+                                    osSignalSet(mainTid, AS_Action);
                                     break;
                                 case SA_Delay:
-                                    printf( " - Delay\r\n");
+                                    printf(" - Delay\r\n");
                                     RunInDelay = true;
-                                    delayTimer.start(aseq.Ms);
+                                    delayTimer.start(aseq.Param);
                                     break;
 
                                 case SA_Status:
-                                    printf( " - Status\r\n"); 
-                                    robotArm.GetArmPositions(lastVals);
-                                    
+                                    printf(" - Status\r\n"); 
+
                                     PushMeasurements(measureGroup, partSize, robotArm);
-                             
-                                    SendIothubData();
-            
+
+                                    robotArm.GetArmLastMeasure(NM_Temperature, lastVals);
+                                    TestTemperature(lastVals);
+                                    
                                     osSignalSet(mainTid, AS_Action);
                                     break;
                                 case SA_Repeat:
-                                    printf( " - Repeat\r\n"); 
-                                    curseqIx = 0;
+                                    printf(" - Repeat\r\n"); 
+                                    curseqIx = aseq.Param;
                                     osSignalSet(mainTid, AS_Action);
                                     break;
                                     

--- a/IothubRobotArm.cpp	Tue Dec 29 23:31:28 2015 +0000
+++ b/IothubRobotArm.cpp	Thu Dec 31 17:47:55 2015 +0000
@@ -56,7 +56,7 @@
     
     if (IoTHubMessage_GetByteArray(message, (const unsigned char**)&buffer, &size) == IOTHUB_MESSAGE_OK)
     {
-        (void)printf("Received Message [%d] with Data: <%.*s> & Size=%d\r\n", (int)size, *counter, buffer, (int)size);
+        (void)printf("Received Message [%d] with Data: <%.*s> & Size=%d\r\n", *counter, (int)size, buffer, (int)size);
         int slen = size;
         if (size >= 20)
             slen = 19;

--- a/IothubSerial.cpp	Tue Dec 29 23:31:28 2015 +0000
+++ b/IothubSerial.cpp	Thu Dec 31 17:47:55 2015 +0000
@@ -5,6 +5,7 @@
 char* nametemp = "temp";
 char* namevolt = "volt";
 char* namedeg = "rot";
+char* nameload = "load";
 
 IothubSerial::IothubSerial()
 {
@@ -35,6 +36,10 @@
             name = namedeg;
             break;
             
+        case NM_Load:
+            name = nameload;
+            break;
+            
         default:
             return -1;
     }

--- a/RobotArm.cpp	Tue Dec 29 23:31:28 2015 +0000
+++ b/RobotArm.cpp	Thu Dec 31 17:47:55 2015 +0000
@@ -21,32 +21,27 @@
 {
     // build arm
     int i = 0;
-    printf( "id[%d] = %d\r\n", i, PartIds[i]); 
-    AX12* servo1 = new AX12( &dynbus, PartIds[i] );
+    AX12* servo1 = new AX12(&dynbus, PartIds[i]);
     servo1->TorqueEnable(true);
     _armParts[i] = dynamic_cast<RobotNode*>(servo1);
     
     i++;
-    printf( "id[%d] = %d\r\n", i, PartIds[i]);  
-    AX12* servo2 = new AX12( &dynbus, PartIds[i] );
+    AX12* servo2 = new AX12(&dynbus, PartIds[i]);
     servo2->TorqueEnable(true);
     _armParts[i] = dynamic_cast<RobotNode*>(servo2);
     
     i++;
-    printf( "id[%d] = %d\r\n", i, PartIds[i]);  
-    AX12* servo3 = new AX12( &dynbus, PartIds[i] );
+    AX12* servo3 = new AX12(&dynbus, PartIds[i]);
     servo3->TorqueEnable(true);
     _armParts[i] = dynamic_cast<RobotNode*>(servo3);
     
     i++;
-    printf( "id[%d] = %d\r\n", i, PartIds[i]);  
-    AX12* servo4 = new AX12( &dynbus, PartIds[i] );
+    AX12* servo4 = new AX12(&dynbus, PartIds[i]);
     servo4->TorqueEnable(true);
     _armParts[i] = dynamic_cast<RobotNode*>(servo4);
     
     i++;
-    printf( "id[%d] = %d\r\n", i, PartIds[i]);  
-    AX12* servo6 = new AX12( &dynbus, PartIds[i] );
+    AX12* servo6 = new AX12(&dynbus, PartIds[i]);
     servo6->TorqueEnable(true);
     _armParts[i] = dynamic_cast<RobotNode*>(servo6);
     
@@ -68,21 +63,24 @@
     MoveArmPositionsEnd();
     GetArmPositions(lastpos);
     lastgoals.clear();
-
+    endgoals.clear();
+    
     numsteps = totms / _stepms;
     if (numsteps == 0) numsteps = 1;
     
     differentials.clear();
-    for( int ix = 0; ix < _numParts; ix++)
+    for (int ix = 0; ix < _numParts; ix++)
     {
         if (positions[ix] > 0.0f)
         {
+            endgoals.push_back(positions[ix]);
             float difference = (positions[ix] - lastpos[ix]) / (float)numsteps;
-            differentials.push_back( difference );
+            differentials.push_back(difference);
         }
         else
         {
             // negative goal. Treat as don't move
+            endgoals.push_back(lastpos[ix]);
             differentials.push_back(0.0f);
         }
     }
@@ -117,11 +115,13 @@
     bool ok = true;
 
     lastgoals.clear();
-    for( int ix = 0; ix < _numParts; ix++ )
+    for (int ix = 0; ix < _numParts; ix++)
     {
         if (_armParts[ix]->HasAction(NA_Rotate))
         {
-            float goal = lastpos[ix] + (differentials[ix] * (float)curstep);
+            float goal = (curstep == numsteps) ?
+                            endgoals[ix] :  // last step - use actual goal
+                            (lastpos[ix] + (differentials[ix] * (float)curstep));
             lastgoals.push_back(goal);
             if (differentials[ix] != 0.0f)
             {
@@ -176,7 +176,7 @@
     }
     else
     {
-        nextdelay = 0;
+        nextdelay = delayms;
     }
     
     return true;
@@ -186,7 +186,7 @@
 {
     vector<float> curpos;
     GetArmPositions(curpos);
-    for( int ix = 0; ix < _numParts; ix++ )
+    for (int ix = 0; ix < _numParts; ix++)
     {
         if (curpos[ix] > 0 && lastgoals.size() > ix)
         {
@@ -242,10 +242,10 @@
 bool RobotArm::GetArmPositions(vector<float>& outPos)
 {
     outPos.clear();
-    for( int ix = 0; ix < _numParts; ix++)
+    for (int ix = 0; ix < _numParts; ix++)
     {
         float pos = _armParts[ix]->GetMeasure(NM_Degrees);
-        outPos.push_back( pos );
+        outPos.push_back(pos);
     }
     return true;
 }
@@ -254,34 +254,34 @@
 bool RobotArm::GetArmLastPositions(vector<float>& outPos)
 {
     outPos.clear();
-    for( int ix = 0; ix < _numParts; ix++)
+    for (int ix = 0; ix < _numParts; ix++)
     {
         float pos = _armParts[ix]->GetLastMeasure(NM_Degrees);
-        outPos.push_back( pos );
+        outPos.push_back(pos);
     }
     return true;
 }
 
 // get all parts measurements
-bool RobotArm::GetArmMeasure(int measureId, vector<float>& outPos)
+bool RobotArm::GetArmMeasure(int measureId, vector<float>& outVals)
 {
-    outPos.clear();
-    for( int ix = 0; ix < _numParts; ix++)
+    outVals.clear();
+    for (int ix = 0; ix < _numParts; ix++)
     {
-        float pos = _armParts[ix]->GetMeasure(measureId);
-        outPos.push_back( pos );
+        float val = _armParts[ix]->GetMeasure(measureId);
+        outVals.push_back(val);
     }
     return true;
 }
 
 // get all parts last measurements
-bool RobotArm::GetArmLastMeasure(int measureId, vector<float>& outPos)
+bool RobotArm::GetArmLastMeasure(int measureId, vector<float>& outVals)
 {
-    outPos.clear();
-    for( int ix = 0; ix < _numParts; ix++)
+    outVals.clear();
+    for (int ix = 0; ix < _numParts; ix++)
     {
-        float pos = _armParts[ix]->GetLastMeasure(measureId);
-        outPos.push_back( pos );
+        float val = _armParts[ix]->GetLastMeasure(measureId);
+        outVals.push_back(val);
     }
     return true;
 }

--- a/RobotArm.h	Tue Dec 29 23:31:28 2015 +0000
+++ b/RobotArm.h	Thu Dec 31 17:47:55 2015 +0000
@@ -115,9 +115,10 @@
     // last position error
     float _lastPosDiff;
     
-    // for thread friendly moves
+    // step-wise position moves
+    vector<float> endgoals;
+    vector<float> differentials;
     vector<float> lastpos;
-    vector<float> differentials;
     vector<float> lastgoals;
     
     // allowance for difference between expected pos and actual pos

--- a/RobotNode/RobotNode.h	Tue Dec 29 23:31:28 2015 +0000
+++ b/RobotNode/RobotNode.h	Thu Dec 31 17:47:55 2015 +0000
@@ -7,9 +7,10 @@
 
 enum NodeMeasure
 {
-    NM_Temperature       = 0x1,
-    NM_Voltage           = 0x2,
-    NM_Degrees           = 0x3
+    NM_Temperature      = 0x1,
+    NM_Voltage          = 0x2,
+    NM_Degrees          = 0x3,
+    NM_Load             = 0x4
 };
 
 enum NodeAction

--- a/Sequences.cpp	Tue Dec 29 23:31:28 2015 +0000
+++ b/Sequences.cpp	Thu Dec 31 17:47:55 2015 +0000
@@ -11,6 +11,7 @@
 vector<ActionSequence> UpTwistSeq;
 vector<ActionSequence> StartSeq;
 vector<ActionSequence> WaveSeq;
+vector<ActionSequence> TapsSeq;
 
 
 void MakeSequences(int partSize, vector<float>& startPositions)
@@ -20,8 +21,19 @@
     vector<float> homePositions;
     vector<float> waveUpPositions;
     vector<float> waveDownPositions;
+    vector<float> waveMiddlePositions;
     vector<float> rightPositions;
     
+    vector<float> tapStartPositions;
+    vector<float> tap1Positions;
+    vector<float> tap2Positions;
+    vector<float> tap3Positions;
+    vector<float> tap4Positions;
+    vector<float> tap5Positions;
+    vector<float> tap6Positions;
+    vector<float> tap7Positions;
+    vector<float> tap8Positions;
+    
     for (int partIx = 0; partIx < partSize; partIx++)
     {
         upPositions.push_back(UpPos);
@@ -63,22 +75,78 @@
     {
         waveUpPositions.push_back(NoMove);
         waveDownPositions.push_back(NoMove);
+        waveMiddlePositions.push_back(NoMove);
     }
-    waveUpPositions.push_back(120.0f);
-    waveDownPositions.push_back(210.0f);
+    waveUpPositions.push_back(135.0f);
+    waveDownPositions.push_back(225.0f);
+    waveMiddlePositions.push_back(180.0f);
 
+    // define tapping start
+    tapStartPositions.push_back(RightPos);
+    if (partSize > 3)
+    {
+        tapStartPositions.push_back(135.0f);
+        tapStartPositions.push_back(225.0f);
+        tapStartPositions.push_back(180.0f);
+        for (int partIx = 4; partIx < partSize; partIx++)
+        {
+            tapStartPositions.push_back(180.0f);
+        }
+    }
+    // define other taps to just swivel - no other moves
+    tap1Positions.push_back(60.2f);
+    tap2Positions.push_back(68.7f);
+    tap3Positions.push_back(77.2f);
+    tap4Positions.push_back(85.7f);
+    tap5Positions.push_back(94.2f);
+    tap6Positions.push_back(102.7f);
+    tap7Positions.push_back(111.2f);
+    tap8Positions.push_back(119.7f);
+    for (int partIx = 1; partIx < partSize; partIx++)
+    {
+        tap1Positions.push_back(NoMove);
+        tap2Positions.push_back(NoMove);
+        tap3Positions.push_back(NoMove);
+        tap4Positions.push_back(NoMove);
+        tap5Positions.push_back(NoMove);
+        tap6Positions.push_back(NoMove);
+        tap7Positions.push_back(NoMove);
+        tap8Positions.push_back(NoMove);
+    }
+    
     // define actions
     ActionSequence moveStart(SA_SetGoal, homePositions, 1500);
-    ActionSequence moveUp(SA_SetGoal, upPositions, 2500);
-    ActionSequence moveDown(SA_SetGoal, downPositions, 2500);
-    ActionSequence waveUp(SA_SetGoal, waveUpPositions, 1500);
-    ActionSequence waveDown(SA_SetGoal, waveDownPositions, 1500);
+    ActionSequence moveUp(SA_SetGoal, upPositions, 1000);
+    ActionSequence moveDown(SA_SetGoal, downPositions, 1000);
+    ActionSequence waveUp(SA_SetGoal, waveUpPositions, 1000);
+    ActionSequence waveDown(SA_SetGoal, waveDownPositions, 1000);
+    ActionSequence tapsStart(SA_SetGoal, tapStartPositions, 1000);
+    ActionSequence taps1(SA_SetGoal, tap1Positions, 200);
+    ActionSequence taps2(SA_SetGoal, tap2Positions, 200);
+    ActionSequence taps3(SA_SetGoal, tap3Positions, 200);
+    ActionSequence taps4(SA_SetGoal, tap4Positions, 200);
+    ActionSequence taps5(SA_SetGoal, tap5Positions, 200);
+    ActionSequence taps6(SA_SetGoal, tap6Positions, 200);
+    ActionSequence taps7(SA_SetGoal, tap7Positions, 200);
+    ActionSequence taps8(SA_SetGoal, tap8Positions, 200);
+    ActionSequence tapDown(SA_SetGoal, waveDownPositions, 80);
+    ActionSequence tapUp(SA_SetGoal, waveMiddlePositions, 80);
     
     ActionSequence report(SA_Status);
     ActionSequence pause2(SA_Delay);
-    pause2.SetDelay(2000);
+    pause2.SetParam(2000);
     ActionSequence rep(SA_Repeat);  
-
+    ActionSequence rep1(SA_Repeat);  
+    rep1.SetParam(1);
+    ActionSequence pause100ms(SA_Delay);
+    pause100ms.SetParam(100);
+    ActionSequence pause200ms(SA_Delay);
+    pause200ms.SetParam(200);
+    ActionSequence pause500ms(SA_Delay);
+    pause500ms.SetParam(500);
+    ActionSequence pause1(SA_Delay);
+    pause1.SetParam(1000);
+    
     // add actions into StartSeq
     StartSeq.clear();
     StartSeq.push_back(moveStart);
@@ -86,11 +154,12 @@
     
     // add actions into WaveSeq
     WaveSeq.clear();
+    WaveSeq.push_back(moveStart);
     WaveSeq.push_back(waveUp);
     WaveSeq.push_back(report);
     WaveSeq.push_back(waveDown);
     WaveSeq.push_back(report);
-    WaveSeq.push_back(rep);
+    WaveSeq.push_back(rep1);
     
     // add actions into UpDownSeq
     UpDownSeq.clear();
@@ -125,6 +194,39 @@
         UpTwistSeq.push_back(moveUp);
     }
     UpTwistSeq.push_back(pause2);
-    UpTwistSeq.push_back(moveDown);
+    UpTwistSeq.push_back(moveStart);
     
+    // add actions to tap sequence
+    TapsSeq.clear();
+    TapsSeq.push_back(tapsStart);
+    TapsSeq.push_back(pause1);
+    TapsSeq.push_back(taps3);
+    TapsSeq.push_back(tapDown);
+    TapsSeq.push_back(tapUp);
+    TapsSeq.push_back(pause100ms);
+    TapsSeq.push_back(taps3);
+    TapsSeq.push_back(tapDown);
+    TapsSeq.push_back(tapUp);
+    TapsSeq.push_back(pause200ms);
+    TapsSeq.push_back(taps1);
+    TapsSeq.push_back(tapDown);
+    TapsSeq.push_back(tapUp);
+    TapsSeq.push_back(pause100ms);
+    TapsSeq.push_back(taps5);
+    TapsSeq.push_back(tapDown);
+    TapsSeq.push_back(tapUp);
+    TapsSeq.push_back(pause100ms);
+    TapsSeq.push_back(taps7);
+    TapsSeq.push_back(tapDown);
+    TapsSeq.push_back(tapUp);
+    TapsSeq.push_back(pause200ms);
+    TapsSeq.push_back(taps6);
+    TapsSeq.push_back(tapDown);
+    TapsSeq.push_back(tapUp);
+    TapsSeq.push_back(pause100ms);
+    TapsSeq.push_back(taps7);
+    TapsSeq.push_back(tapDown);
+    TapsSeq.push_back(tapUp);
+    TapsSeq.push_back(report);
+
 }

--- a/Sequences.h	Tue Dec 29 23:31:28 2015 +0000
+++ b/Sequences.h	Thu Dec 31 17:47:55 2015 +0000
@@ -9,6 +9,7 @@
 extern vector<ActionSequence> UpTwistSeq;
 extern vector<ActionSequence> StartSeq;
 extern vector<ActionSequence> WaveSeq;
+extern vector<ActionSequence> TapsSeq;
 
 
 void MakeSequences(int partSize, vector<float>& homePositions);

--- a/main.cpp	Tue Dec 29 23:31:28 2015 +0000
+++ b/main.cpp	Thu Dec 31 17:47:55 2015 +0000
@@ -88,7 +88,7 @@
     pc.foreground(Yellow);
     pc.background(Black);
 
-    pc.locate( 0, 0 );
+    pc.locate(0, 0);
     pc.printf("**********************\r\n");
     pc.printf("RobotArmDemo start\r\n");
     pc.printf("**********************\r\n");
@@ -106,7 +106,7 @@
     // TODO: test for connection established
     Thread::wait(15000);
     
-    pc.printf( "Initialization done. Ready to run. \r\n");
+    pc.printf("Initialization done. Ready to run. \r\n");
 
 
     RunController();