AX12.cpp

00001 /* Copyright (C) 2016 Telesensorics Inc, MIT License
00002  *
00003  * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
00004  * and associated documentation files (the "Software"), to deal in the Software without restriction,
00005  * including without limitation the rights to use, copy, modify, merge, publish, distribute,
00006  * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
00007  * furnished to do so, subject to the following conditions:
00008  *
00009  * The above copyright notice and this permission notice shall be included in all copies or
00010  * substantial portions of the Software.
00011  *
00012  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
00013  * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
00014  * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
00015  * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00016  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
00017  */
00018  
00019 #include "AX12.h"
00020 #include "mbed.h"
00021 
00022 
00023 AX12::AX12( DynamixelBus* pbus, ServoId ID)
00024 {
00025     _pbus = pbus;
00026     _ID = ID;
00027     _LastError = statusValid;
00028     _HasReadCache = false;
00029 }
00030 
00031 
00032 void AX12::ClearCache()
00033 {
00034     _HasReadCache = false;
00035 }
00036 
00037 
00038 StatusCode AX12::Ping()
00039 {
00040     return _pbus->Ping(_ID);
00041 }
00042 
00043 
00044 StatusCode AX12::SetServoId(char newId)
00045 {
00046     int offset = ctID;
00047     CommBuffer data;
00048     data.push_back( newId );
00049     StatusCode s = _pbus->Write(_ID, offset, data); 
00050     if( s == statusValid )
00051     {
00052         _ID = newId;
00053     }
00054     return s;
00055 }
00056 
00057 
00058 StatusCode AX12::SetGoal(float degrees)
00059 {
00060     _LastError = statusValid;
00061     short goal = (short)((1024.0f * (degrees - 30.0f)) / 300.0f);
00062 
00063     CommBuffer data;
00064     data.push_back( goal & 0xff );
00065     data.push_back( goal >> 8 );
00066 
00067     // write the packet, return the error code
00068     int offset = ctGoalPositionL;
00069     return _pbus->Write(_ID, offset, data); 
00070 }
00071 
00072 
00073 StatusCode AX12::SetReplyDelay(int us)
00074 {
00075     _LastError = statusValid;
00076     int val = 250;
00077     if (us < 500)
00078         val = ((us + 1) / 2);
00079 
00080     CommBuffer data;
00081     data.push_back( val & 0xff );
00082 
00083     // write the packet, return the error code
00084     int offset = ctReturnDelayTime;
00085     return _pbus->Write(_ID, offset, data); 
00086 }
00087 
00088 
00089 bool AX12::IsMoving(void)
00090 {
00091     if (!_HasReadCache)
00092     {
00093         _LastError = LoadReadCache();
00094         if (_LastError != statusValid)
00095         {
00096             return false;
00097         }
00098     }
00099     
00100     return _ReadCache[cctMoving] == 1;
00101 }
00102 
00103 
00104 float AX12::GetPosition()
00105 {
00106     if (!_HasReadCache)
00107     {
00108         _LastError = LoadReadCache();
00109         if (_LastError != statusValid)
00110         {
00111             return 0.0f;
00112         }
00113     }
00114     
00115     int16_t value = _ReadCache[cctPresentPositionL] | (_ReadCache[cctPresentPositionH] << 8);
00116     float degrees = 30.0f + ((float)value * 300.0f / 1024.0f);
00117     return degrees;
00118 }
00119 
00120 
00121 int AX12::GetTemperature(void)
00122 {
00123     if (!_HasReadCache)
00124     {
00125         _LastError = LoadReadCache();
00126         if (_LastError != statusValid)
00127         {
00128             return 0;
00129         }
00130     }
00131     
00132     return (int)_ReadCache[cctPresentTemperature];
00133 }
00134 
00135 
00136 float AX12::GetSupplyVoltage(void)
00137 {
00138     if (!_HasReadCache)
00139     {
00140         _LastError = LoadReadCache();
00141         if (_LastError != statusValid)
00142         {
00143             return 0.0f;
00144         }
00145     }
00146 
00147     return (float)_ReadCache[cctPresentVoltage] / 10.0f;
00148 }
00149 
00150 
00151 float AX12::GetLoad(void)
00152 {
00153     if (!_HasReadCache)
00154     {
00155         _LastError = LoadReadCache();
00156         if (_LastError != statusValid)
00157         {
00158             return 0.0f;
00159         }
00160     }
00161 
00162     int16_t value = _ReadCache[cctPresentLoadL] | (_ReadCache[cctPresentLoadH] << 8);
00163     return (float)(value & 0x03ff);
00164 }
00165 
00166 
00167 StatusCode AX12::TorqueEnable(bool enable)
00168 {
00169     _LastError = statusValid;
00170     CommBuffer data;
00171     data.push_back( enable ? 1 : 0 );
00172 
00173     int offset = ctTorqueEnable;
00174     return _pbus->Write(_ID, offset, data); 
00175 }
00176 
00177 
00178 int AX12::GetLastError()
00179 {
00180     return _LastError;
00181 }
00182 
00183 
00184 bool AX12::HasError()
00185 {
00186     return _LastError != statusValid;
00187 }
00188 
00189 
00190 StatusCode AX12::LoadReadCache(void)
00191 {
00192     _LastError = statusValid;
00193     CommBuffer data;
00194     _HasReadCache = false;
00195     
00196     StatusCode s = _pbus->Read(_ID, AX12ReadBlockStart, AX12ReadBlockLength, data); 
00197     if( s == statusValid )
00198     {
00199         for (int ix = 0; ix < AX12ReadBlockLength; ix++)
00200         {
00201             _ReadCache[ix] = data[ix];
00202         }
00203         _HasReadCache = true;
00204     }
00205     else
00206     {
00207         _LastError = s;
00208     }
00209     return s;
00210 }