NodeAX12.cpp

00001 // Copyright (c) Microsoft. All rights reserved.
00002 // Licensed under the MIT license. See LICENSE file in the project root for full license information.
00003 
00004 #include "mbed.h"
00005 #include "NodeAX12.h"
00006 
00007 
00008 NodeAX12::NodeAX12(DynamixelBus* pbus, ServoId ID) :
00009                 _Servo(pbus, ID)
00010 {
00011 
00012 }
00013 
00014 
00015 bool NodeAX12::HasMeasure(int measureId)
00016 {
00017     switch (measureId)
00018     {
00019         case NM_Temperature:
00020             return true;
00021             
00022         case NM_Degrees:
00023             return true;
00024         
00025         case NM_Voltage:
00026             return true;
00027             
00028         case NM_Load:
00029             return true;
00030 
00031         default:
00032             return false;
00033     }
00034 }
00035 
00036 float NodeAX12::GetMeasure(int measureId)
00037 {
00038     switch (measureId)
00039     {
00040         case NM_Temperature:
00041             return (float)_Servo.GetTemperature();
00042             
00043         case NM_Degrees:
00044             return (float)_Servo.GetPosition();
00045         
00046         case NM_Voltage:
00047             return (float)_Servo.GetSupplyVoltage();
00048             
00049         case NM_Load:
00050             return (float)_Servo.GetLoad();
00051             
00052         default:
00053             return 0.0f;
00054     }
00055 }
00056 
00057 void NodeAX12::ClearMeasureCache()
00058 {
00059     _Servo.ClearCache();
00060 }
00061 
00062 bool NodeAX12::HasAction(int actionId)
00063 {
00064     switch (actionId)
00065     {
00066         case NA_Ping:
00067             return true;
00068             
00069         case NA_Init:
00070             return true;
00071             
00072         case NA_Rotate:
00073             return true;
00074 
00075         case NA_ClearError:
00076             return true;
00077             
00078         default:
00079             return false;
00080     }
00081 }
00082 
00083 
00084 bool NodeAX12::DoAction(int actionId, float actionValue)
00085 {
00086     StatusCode sc;
00087     
00088     switch (actionId)
00089     {
00090         case NA_Ping:
00091             if (_Servo.Ping() == statusValid)
00092                 return true;
00093             else
00094                 return false;
00095             
00096         case NA_Init:
00097             sc = _Servo.SetReplyDelay(100);
00098             sc = _Servo.TorqueEnable(false);
00099             sc = _Servo.TorqueEnable(true);
00100             return sc == statusValid;
00101 
00102         case NA_Rotate:
00103             sc = _Servo.SetGoal(actionValue);
00104             return sc == statusValid;
00105 
00106         case NA_ClearError:
00107             sc = _Servo.TorqueEnable(false);
00108             sc = _Servo.TorqueEnable(true);
00109             return sc == statusValid;
00110 
00111         default:
00112             return false;
00113     }
00114 }
00115 
00116 NodePartType NodeAX12::GetNodeType()
00117 {
00118     return NT_AX12;
00119 }
00120 
00121 int NodeAX12::GetLastError()
00122 {
00123     return _Servo.GetLastError();
00124 }
00125 
00126 bool NodeAX12::HasError()
00127 {
00128     return _Servo.HasError();
00129 }