Diff: AX12.cpp

Revision:

11:8de493bd8922

Parent:

10:e4c9b94b5879

Child:

12:f66c779ca018

--- a/AX12.cpp	Thu Jan 07 17:31:09 2016 +0000
+++ b/AX12.cpp	Tue Jan 19 19:48:59 2016 +0000
@@ -10,6 +10,14 @@
     _pbus = pbus;
     _ID = ID;
     _LastError = statusValid;
+    _HasReadCache = false;
+}
+
+/*****/
+
+void AX12::ClearCache()
+{
+    _HasReadCache = false;
 }
 
 /*****/
@@ -52,127 +60,106 @@
 
 /*****/
 
-bool AX12::IsMoving(void)
+StatusCode AX12::SetReplyDelay(int us)
 {
     _LastError = statusValid;
+    int val = 250;
+    if (us < 500)
+        val = ((us + 1) / 2);
+
     CommBuffer data;
-    StatusCode s = _pbus->Read(_ID, ctMoving, 1, data); 
-    if( s == statusValid )
+    data.push_back( val & 0xff );
+
+    // write the packet, return the error code
+    int offset = ctReturnDelayTime;
+    return _pbus->Write(_ID, offset, data); 
+}
+
+/*****/
+
+bool AX12::IsMoving(void)
+{
+    if (!_HasReadCache)
     {
-        return data[0] == 1;
+        _LastError = LoadReadCache();
+        if (_LastError != statusValid)
+        {
+            return false;
+        }
     }
-    else
-    {
-        _LastError = s;
-        return false;
-    }
+    
+    return _ReadCache[cctMoving] == 1;
 }
 
 /*****/
 
 float AX12::GetPosition()
 {
-    _LastError = statusValid;
-    CommBuffer data;
-    StatusCode s = _pbus->Read(_ID, ctPresentPositionL, 2, data); 
-    if( s == statusValid )
-    {
-        int16_t value = data[0] | (data[1] << 8);
-        float degrees = 30.0f + ((float)value * 300.0f / 1024.0f);
-        return degrees;
-    }
-    else
+    if (!_HasReadCache)
     {
-        // try again one time
-        s = _pbus->Read(_ID, ctPresentPositionL, 2, data); 
-        if( s == statusValid )
+        _LastError = LoadReadCache();
+        if (_LastError != statusValid)
         {
-            int16_t value = data[0] | (data[1] << 8);
-            float degrees = 30.0f + ((float)value * 300.0f / 1024.0f);
-            return degrees;
+            return 0.0f;
         }
-        _LastError = s;
-        return 0.0f;
     }
+    
+    int16_t value = _ReadCache[cctPresentPositionL] | (_ReadCache[cctPresentPositionH] << 8);
+    float degrees = 30.0f + ((float)value * 300.0f / 1024.0f);
+    return degrees;
 }
 
 /*****/
 
 int AX12::GetTemperature(void)
 {
-    _LastError = statusValid;
-    CommBuffer data;
-    StatusCode s = _pbus->Read(_ID, ctPresentTemperature, 1, data); 
-    if( s == statusValid )
-    {
-        return (int)data[0];
-    }
-    else
+    if (!_HasReadCache)
     {
-        // try again one time
-        s = _pbus->Read(_ID, ctPresentTemperature, 1, data); 
-        if( s == statusValid )
+        _LastError = LoadReadCache();
+        if (_LastError != statusValid)
         {
-            return (int)data[0];
+            return 0;
         }
-
-        _LastError = s;
-        return 0;
     }
+    
+    return (int)_ReadCache[cctPresentTemperature];
 }
 
 /*****/
 
 float AX12::GetSupplyVoltage(void)
 {
-    _LastError = statusValid;
-    CommBuffer data;
-    StatusCode s = _pbus->Read(_ID, ctPresentVoltage, 1, data); 
-    if( s == statusValid )
-    {
-        float volts = (float)data[0] / 10.0f;
-        return volts;
-    }
-    else
+    if (!_HasReadCache)
     {
-        // try again one time
-        s = _pbus->Read(_ID, ctPresentVoltage, 1, data); 
-        if( s == statusValid )
+        _LastError = LoadReadCache();
+        if (_LastError != statusValid)
         {
-            float volts = (float)data[0] / 10.0f;
-            return volts;
+            return 0.0f;
         }
-        _LastError = s;
-        return 0.0f;
     }
+
+    return (float)_ReadCache[cctPresentVoltage] / 10.0f;
 }
 
 /*****/
 
 float AX12::GetLoad(void)
 {
-    _LastError = statusValid;
-    CommBuffer data;
-    StatusCode s = _pbus->Read(_ID, ctPresentLoadL, 2, data); 
-    if( s == statusValid )
-    {
-        int16_t value = data[0] | (data[1] << 8);
-        return (float)(value & 0x03ff);
-    }
-    else
+    if (!_HasReadCache)
     {
-        // try again one time
-        s = _pbus->Read(_ID, ctPresentLoadL, 2, data); 
-        if( s == statusValid )
+        _LastError = LoadReadCache();
+        if (_LastError != statusValid)
         {
-            int16_t value = data[0] | (data[1] << 8);
-            return (float)(value & 0x03ff);
+            return 0.0f;
         }
-        _LastError = s;
-        return 0.0f;
     }
+
+    int16_t value = _ReadCache[cctPresentLoadL] | (_ReadCache[cctPresentLoadH] << 8);
+    return (float)(value & 0x03ff);
 }
 
+/*****/
+
 StatusCode AX12::TorqueEnable(bool enable)
 {
     _LastError = statusValid;
@@ -183,13 +170,40 @@
     return _pbus->Write(_ID, offset, data); 
 }
 
+/*****/
 
 int AX12::GetLastError()
 {
     return _LastError;
 }
 
+/*****/
+
 bool AX12::HasError()
 {
     return _LastError != statusValid;
 }
+
+/*****/
+
+StatusCode AX12::LoadReadCache(void)
+{
+    _LastError = statusValid;
+    CommBuffer data;
+    _HasReadCache = false;
+    
+    StatusCode s = _pbus->Read(_ID, AX12ReadBlockStart, AX12ReadBlockLength, data); 
+    if( s == statusValid )
+    {
+        for (int ix = 0; ix < AX12ReadBlockLength; ix++)
+        {
+            _ReadCache[ix] = data[ix];
+        }
+        _HasReadCache = true;
+    }
+    else
+    {
+        _LastError = s;
+    }
+    return s;
+}