robot arm demo team / Mbed 2 deprecated RobotArmDemo Featured

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   AX-12A Dynamixel mbed iothub_client EthernetInterface NTPClient ConfigFile SDFileSystem iothub_amqp_transport mbed-rtos proton-c-mbed wolfSSL

Files at this revision

API Documentation at this revision

Revision 9:a0fb6c370dbb, committed 2015-12-31

Comitter:
henryrawas
Date:
Thu Dec 31 20:02:58 2015 +0000
Parent:
8:d98e2dec0f40
Child:
10:9b21566a5ddb
Commit message:
report errors

Changed in this revision

AX-12A.lib Show annotated file Show diff for this revision Revisions of this file
ArmController.cpp Show annotated file Show diff for this revision Revisions of this file
RobotArm.cpp Show annotated file Show diff for this revision Revisions of this file
RobotNode/RobotNode.h Show annotated file Show diff for this revision Revisions of this file 
--- a/AX-12A.lib	Thu Dec 31 17:47:55 2015 +0000
+++ b/AX-12A.lib	Thu Dec 31 20:02:58 2015 +0000
@@ -1,1 +1,1 @@
-http://developer.mbed.org/teams/robot-arm-demo-team/code/AX-12A/#155ecc801119
+http://developer.mbed.org/teams/robot-arm-demo-team/code/AX-12A/#b6979be5a0a7

--- a/ArmController.cpp	Thu Dec 31 17:47:55 2015 +0000
+++ b/ArmController.cpp	Thu Dec 31 20:02:58 2015 +0000
@@ -17,6 +17,31 @@
 
 using namespace std;
 
+vector<float> initPositions;
+
+
+enum MainStates
+{
+    MS_Idle           = 0x0,
+    MS_Running        = 0x1,
+    MS_Paused         = 0x2,
+    MS_Stopping       = 0x3,
+    MS_Error          = 0x4,
+    MS_CancelOne      = 0x5,
+    MS_CancelAll      = 0x6,
+    MS_CancelToM      = 0x7
+};
+
+Queue<vector<ActionSequence>, 16> SequenceQ;
+volatile MainStates MainState;
+bool RunInSequence;
+bool RunInMoveStep;
+bool RunInDelay;
+void* CancelToMatch;
+
+osThreadId mainTid;
+
+
 void DispMeasure(char* label, int partSize, vector<float>& vals)
 {
     printf("%s: ", label);
@@ -28,33 +53,6 @@
 
 }
 
-void PushMeasurements(MeasureGroup measureGroup, int partSize, RobotArm robotArm)
-{
-    vector<float> lastVals;
-    time_t seconds = time(NULL);
-    
-    robotArm.GetArmMeasure(NM_Temperature, lastVals);
-    DispMeasure("Temperatures", partSize, lastVals);
-    measureGroup.SetMeasure(NM_Temperature, seconds, lastVals);
-    MeasureBuf.push(measureGroup);
-        
-    robotArm.GetArmMeasure(NM_Voltage, lastVals);
-    DispMeasure("Voltage", partSize, lastVals);
-    measureGroup.SetMeasure(NM_Voltage, seconds, lastVals);
-    MeasureBuf.push(measureGroup);
-    
-    robotArm.GetArmMeasure(NM_Degrees, lastVals);
-    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)
 {
@@ -96,30 +94,6 @@
     SendIothubData();
 }
 
-vector<float> initPositions;
-
-
-enum MainStates
-{
-    MS_Idle           = 0x0,
-    MS_Running        = 0x1,
-    MS_Paused         = 0x2,
-    MS_Stopping       = 0x3,
-    MS_Error          = 0x4,
-    MS_CancelOne      = 0x5,
-    MS_CancelAll      = 0x6,
-    MS_CancelToM      = 0x7
-};
-
-Queue<vector<ActionSequence>, 16> SequenceQ;
-volatile MainStates MainState;
-bool RunInSequence;
-bool RunInMoveStep;
-bool RunInDelay;
-void* CancelToMatch;
-
-osThreadId mainTid;
-
 // send alert if temperature is above 69C and return true
 #define MaxTemp     69
 bool TestTemperature(vector<float>& vals)
@@ -139,6 +113,50 @@
     return err;
 }
 
+void PushMeasurements(MeasureGroup measureGroup, int partSize, RobotArm robotArm)
+{
+    vector<float> lastVals;
+    time_t seconds = time(NULL);
+    bool ok = true;
+    
+    ok = robotArm.GetArmMeasure(NM_Temperature, lastVals);
+    DispMeasure("Temperatures", partSize, lastVals);
+    measureGroup.SetMeasure(NM_Temperature, seconds, lastVals);
+    MeasureBuf.push(measureGroup);
+    
+    if (ok)
+    {
+        ok = robotArm.GetArmMeasure(NM_Voltage, lastVals);
+        DispMeasure("Voltage", partSize, lastVals);
+        measureGroup.SetMeasure(NM_Voltage, seconds, lastVals);
+        MeasureBuf.push(measureGroup);
+    }
+    
+    if (ok)
+    {
+        ok = robotArm.GetArmMeasure(NM_Degrees, lastVals);
+        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();
+    
+    if (!ok)
+    {
+        // report HW error
+        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;
+    }
+}
 
 void ControlArm(const char* cmd)
 {

--- a/RobotArm.cpp	Thu Dec 31 17:47:55 2015 +0000
+++ b/RobotArm.cpp	Thu Dec 31 20:02:58 2015 +0000
@@ -241,49 +241,77 @@
 // get all parts positions
 bool RobotArm::GetArmPositions(vector<float>& outPos)
 {
+    bool ok = true;
     outPos.clear();
     for (int ix = 0; ix < _numParts; ix++)
     {
         float pos = _armParts[ix]->GetMeasure(NM_Degrees);
         outPos.push_back(pos);
+        if (_armParts[ix]->HasError())
+        {
+            _lastErrorPart = ix;
+            _lastError = _armParts[ix]->GetLastError();
+            ok = false;
+        }
     }
-    return true;
+    return ok;
 }
 
 // get all parts last measured positions
 bool RobotArm::GetArmLastPositions(vector<float>& outPos)
 {
+    bool ok = true;
     outPos.clear();
     for (int ix = 0; ix < _numParts; ix++)
     {
         float pos = _armParts[ix]->GetLastMeasure(NM_Degrees);
         outPos.push_back(pos);
+        if (_armParts[ix]->HasError())
+        {
+            _lastErrorPart = ix;
+            _lastError = _armParts[ix]->GetLastError();
+            ok = false;
+        }
     }
-    return true;
+    return ok;
 }
 
 // get all parts measurements
 bool RobotArm::GetArmMeasure(int measureId, vector<float>& outVals)
 {
+    bool ok = true;
     outVals.clear();
     for (int ix = 0; ix < _numParts; ix++)
     {
         float val = _armParts[ix]->GetMeasure(measureId);
         outVals.push_back(val);
+        if (_armParts[ix]->HasError())
+        {
+            _lastErrorPart = ix;
+            _lastError = _armParts[ix]->GetLastError();
+            ok = false;
+        }
     }
-    return true;
+    return ok;
 }
 
 // get all parts last measurements
 bool RobotArm::GetArmLastMeasure(int measureId, vector<float>& outVals)
 {
+    bool ok = true;
     outVals.clear();
     for (int ix = 0; ix < _numParts; ix++)
     {
         float val = _armParts[ix]->GetLastMeasure(measureId);
         outVals.push_back(val);
+        if (_armParts[ix]->HasError())
+        {
+            _lastErrorPart = ix;
+            _lastError = _armParts[ix]->GetLastError();
+            ok = false;
+        }
     }
-    return true;
+    return ok;
 }
 
 int RobotArm::GetNumParts()

--- a/RobotNode/RobotNode.h	Thu Dec 31 17:47:55 2015 +0000
+++ b/RobotNode/RobotNode.h	Thu Dec 31 20:02:58 2015 +0000
@@ -39,6 +39,8 @@
     
     virtual int GetLastError() = 0;
     
+    virtual bool HasError() = 0;
+    
     virtual NodePartType GetNodeType() = 0;
 };