demo project
Dependencies: AX-12A Dynamixel mbed iothub_client EthernetInterface NTPClient ConfigFile SDFileSystem iothub_amqp_transport mbed-rtos proton-c-mbed wolfSSL
Diff: RobotNode/NodeAX12.cpp
- 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;