demo project

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

Revision:
15:4bd10f531cdc
Parent:
13:ffeff9b5e513
Child:
19:2f0ec9ac1238
--- a/RobotNode/NodeAX12.cpp	Fri Jan 15 23:10:30 2016 +0000
+++ b/RobotNode/NodeAX12.cpp	Tue Jan 19 19:50:07 2016 +0000
@@ -6,10 +6,7 @@
 NodeAX12::NodeAX12(DynamixelBus* pbus, ServoId ID) :
                 _Servo(pbus, ID)
 {
-    _LastPosition= 0;
-    _LastTemperature = 0;
-    _LastVoltage = 0;
-    _LastLoad = 0;
+
 }
 
 
@@ -36,60 +33,28 @@
 
 float NodeAX12::GetMeasure(int measureId)
 {
-    float val;
     switch (measureId)
     {
         case NM_Temperature:
-            val = (float)_Servo.GetTemperature();
-            if (val != 0.0f)
-                _LastTemperature = val;
-            return val;
+            return (float)_Servo.GetTemperature();
             
         case NM_Degrees:
-            val = (float)_Servo.GetPosition();
-            if (val != 0.0f)
-                _LastPosition = val;
-            return val;
+            return (float)_Servo.GetPosition();
         
         case NM_Voltage:
-            val = (float)_Servo.GetSupplyVoltage();
-            if (val != 0.0f)
-                _LastVoltage = val;
-            return val;
+            return (float)_Servo.GetSupplyVoltage();
             
         case NM_Load:
-            val = (float)_Servo.GetLoad();
-            if (val != 0.0f)
-            {
-                _LastLoad = val;
-            }
-            return val;
-
+            return (float)_Servo.GetLoad();
             
         default:
             return 0.0f;
     }
 }
 
-float NodeAX12::GetLastMeasure(int measureId)
+void NodeAX12::ClearMeasureCache()
 {
-    switch (measureId)
-    {
-        case NM_Temperature:
-            return (float)_LastTemperature;
-            
-        case NM_Degrees:
-            return _LastPosition;
-        
-        case NM_Voltage:
-            return _LastVoltage;
-            
-        case NM_Load:
-            return _LastLoad;
- 
-         default:
-            return 0.0f;
-    }
+    _Servo.ClearCache();
 }
 
 bool NodeAX12::HasAction(int actionId)
@@ -127,6 +92,7 @@
                 return false;
             
         case NA_Init:
+            sc = _Servo.SetReplyDelay(100);
             sc = _Servo.TorqueEnable(false);
             sc = _Servo.TorqueEnable(true);
             return sc == statusValid;