Diff: AX12.cpp

Revision:

1:d7642b2e155d

Parent:

0:1a48094c99d1

Child:

2:f2da3b1d9988

Child:

3:7b970151bf19

--- a/AX12.cpp	Thu Dec 10 21:45:59 2015 +0000
+++ b/AX12.cpp	Tue Dec 15 04:37:54 2015 +0000
@@ -20,41 +20,88 @@
 
 StatusCode AX12::SetGoal(float degrees)
 {
-    short goal = (short)((1023.0f * degrees) / 300.0f);
+    short goal = (short)((1024.0f * degrees) / 300.0f);
 
     CommBuffer data;
     data.push_back( goal & 0xff );
     data.push_back( goal >> 8 );
 
     // write the packet, return the error code
-    int offset = GoalPositionL;
+    int offset = ctGoalPositionL;
     return _pbus->Write(_ID, offset, data); 
 }
 
 /*****/
 
-bool IsMoving(void)
+bool AX12::IsMoving(void)
 {
-    return false;
+    CommBuffer data;
+    StatusCode s = _pbus->Read(_ID, ctMoving, 1, data); 
+    if( s == statusValid )
+    {
+        return data[0] == 1;
+    }
+    else
+    {
+        return false;
+    }
+}
+
+/*****/
+
+float AX12::GetPosition()
+{
+    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;
+        return degrees;
+    }
+    else
+    {
+        return 0.0f;
+    }
 }
 
 /*****/
 
-float GetPosition()
+int AX12::GetTemperature(void)
 {
-    return 0;
+    CommBuffer data;
+    StatusCode s = _pbus->Read(_ID, ctPresentTemperature, 1, data); 
+    if( s == statusValid )
+    {
+        return (int)data[0];
+    }
+    else
+    {
+        return 0;
+    }
 }
 
 /*****/
 
-float GetTemperature(void)
+float AX12::GetSupplyVoltage(void)
 {
-    return 0;
+    CommBuffer data;
+    StatusCode s = _pbus->Read(_ID, ctPresentVoltage, 1, data); 
+    if( s == statusValid )
+    {
+        return (float)data[0] / 10.0f;
+    }
+    else
+    {
+        return 0.0f;
+    }
 }
 
-/*****/
+StatusCode AX12::TorqueEnable(bool enable)
+{
+    CommBuffer data;
+    data.push_back( enable ? 1 : 0 );
 
-float GetSupplyVoltage(void)
-{
-    return 0;
+    int offset = ctTorqueEnable;
+    return _pbus->Write(_ID, offset, data); 
 }