Diff: AX12.cpp

Revision:

3:7b970151bf19

Parent:

1:d7642b2e155d

Child:

4:56c593ae9a45

--- a/AX12.cpp	Tue Dec 15 04:37:54 2015 +0000
+++ b/AX12.cpp	Wed Dec 23 18:32:37 2015 +0000
@@ -2,11 +2,18 @@
 
 #include "AX12.h"
 #include "mbed.h"
+#include <Terminal.h>
+
+Terminal* AX12Pc = NULL;
 
 AX12::AX12( DynamixelBus* pbus, ServoId ID)
 {
     _pbus = pbus;
     _ID = ID;
+    _LastPosition= 0.0f;
+    _LastTemperature = 0;
+    _LastVoltage = 0;
+    _LastError = statusValid;
 }
 
 /*****/
@@ -20,6 +27,7 @@
 
 StatusCode AX12::SetGoal(float degrees)
 {
+    _LastError = statusValid;
     short goal = (short)((1024.0f * degrees) / 300.0f);
 
     CommBuffer data;
@@ -35,6 +43,7 @@
 
 bool AX12::IsMoving(void)
 {
+    _LastError = statusValid;
     CommBuffer data;
     StatusCode s = _pbus->Read(_ID, ctMoving, 1, data); 
     if( s == statusValid )
@@ -43,6 +52,7 @@
     }
     else
     {
+        _LastError = s;
         return false;
     }
 }
@@ -51,16 +61,19 @@
 
 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 = (float)value * 300.0f / 1024.0f;
+        _LastPosition = degrees;
         return degrees;
     }
     else
     {
+        _LastError = s;
         return 0.0f;
     }
 }
@@ -69,14 +82,18 @@
 
 int AX12::GetTemperature(void)
 {
+    _LastError = statusValid;
     CommBuffer data;
     StatusCode s = _pbus->Read(_ID, ctPresentTemperature, 1, data); 
     if( s == statusValid )
     {
+        _LastTemperature = (int)data[0];
         return (int)data[0];
     }
     else
     {
+        _LastError = s;
+        _LastTemperature = -1;
         return 0;
     }
 }
@@ -85,23 +102,120 @@
 
 float AX12::GetSupplyVoltage(void)
 {
+    _LastError = statusValid;
     CommBuffer data;
     StatusCode s = _pbus->Read(_ID, ctPresentVoltage, 1, data); 
     if( s == statusValid )
     {
-        return (float)data[0] / 10.0f;
+        float volts = (float)data[0] / 10.0f;
+        _LastVoltage = volts;
+        return volts;
     }
     else
     {
+        _LastError = s;
+        _LastVoltage = -1;
         return 0.0f;
     }
 }
 
 StatusCode AX12::TorqueEnable(bool enable)
 {
+    _LastError = statusValid;
     CommBuffer data;
     data.push_back( enable ? 1 : 0 );
 
     int offset = ctTorqueEnable;
     return _pbus->Write(_ID, offset, data); 
 }
+
+bool AX12::HasMeasure(int measureId)
+{
+    switch (measureId)
+    {
+        case NM_Temperature:
+            return true;
+            
+        case NM_Degrees:
+            return true;
+        
+        case NM_Voltage:
+            return true;
+            
+        default:
+            return false;
+    }
+}
+
+float AX12::GetMeasure(int measureId)
+{
+    switch (measureId)
+    {
+        case NM_Temperature:
+            return (float)GetTemperature();
+            
+        case NM_Degrees:
+            return GetPosition();
+        
+        case NM_Voltage:
+            return GetSupplyVoltage();
+            
+        default:
+            return 0.0f;
+    }
+}
+
+float AX12::GetLastMeasure(int measureId)
+{
+    switch (measureId)
+    {
+        case NM_Temperature:
+            return (float)_LastTemperature;
+            
+        case NM_Degrees:
+            return _LastPosition;
+        
+        case NM_Voltage:
+            return _LastVoltage;
+            
+        default:
+            return 0.0f;
+    }
+}
+
+bool AX12::HasAction(int actionId)
+{
+    switch (actionId)
+    {
+        case NA_Rotate:
+            return true;
+
+        default:
+            return false;
+    }
+}
+
+// TODO: error code handling cleanup
+bool AX12::DoAction(int actionId, float actionValue)
+{
+    _LastError = statusValid;
+    switch (actionId)
+    {
+        case NA_Rotate:
+            _LastError = SetGoal(actionValue);
+            return _LastError == statusValid;
+
+        default:
+            return instructionError;
+    }
+}
+
+NodePartType AX12::GetNodeType()
+{
+    return NT_AX12;
+}
+
+unsigned char AX12::GetLastError()
+{
+    return _LastError;
+}