Subversion Repositories Projects

Rev

Rev 230 | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
230 dhylands 1
/****************************************************************************
2
*
3
*   Copyright (c) 2009 Dave Hylands     <dhylands@gmail.com>
4
*
5
*   This program is free software; you can redistribute it and/or modify
6
*   it under the terms of the GNU General Public License version 2 as
7
*   published by the Free Software Foundation.
8
*
9
*   Alternatively, this software may be distributed under the terms of BSD
10
*   license.
11
*
12
*   See README and COPYING for more details.
13
*
14
****************************************************************************/
15
/**
16
*
17
*   @file   bioloid-reg.c
18
*
19
*   @brief  Provides formatting routines for formatting various registers
20
*           found on bioloid devices.
21
*
22
*****************************************************************************/
23
 
24
/* ---- Include Files ----------------------------------------------------- */
25
 
26
#include <string.h>
27
 
28
#include "Str.h"
29
#include "StrToken.h"
30
#include "Bioloid.h"
31
#include "bioloid-reg.h"
32
#include "Log.h"
33
 
34
void BLD_RegFmtBaud( struct BLD_Reg_s *reg, uint16_t val, char *outStr, size_t maxLen )
35
{
36
    StrPrintf( outStr, maxLen, "%d baud", 2000000uL / ( val + 1 ));
37
}
38
 
39
bool BLD_RegParseBaud( struct BLD_Reg_s *reg, StrTokenizer &line, uint16_t *valp )
40
{
41
    uint16_t    baud;
42
 
43
    if ( !line.NextNum( &baud ))
44
    {
45
        LogError( "Please specify a numeric baud rate\n" );
46
        return false;
47
    }
48
    *valp = 2000000 / baud - 1;
49
    if ( *valp > reg->maxVal )
50
    {
51
        LogError( "Baud value must be less than %u: got: %u\n", reg->maxVal, *valp );
52
        return false;
53
    }
54
 
55
    return true;
56
}
57
 
58
void BLD_RegFmtRDT( struct BLD_Reg_s *reg, uint16_t val, char *outStr, size_t maxLen )
59
{
60
    StrPrintf( outStr, maxLen, "%d usec", 2 * val );
61
}
62
 
63
bool BLD_RegParseRDT( struct BLD_Reg_s *reg, StrTokenizer &line, uint16_t *valp )
64
{
65
    uint16_t    rdt;
66
 
67
    if ( !line.NextNum( &rdt ))
68
    {
69
        LogError( "Please specify a numeric return delay time\n" );
70
        return false;
71
    }
72
    *valp = rdt / 2;
73
    if ( *valp > reg->maxVal )
74
    {
75
        LogError( "RDT value must be less than %u: got: %u\n", reg->maxVal, *valp );
76
        return false;
77
    }
78
 
79
    return true;
80
}
81
 
82
void BLD_RegFmtAngle( struct BLD_Reg_s *reg, uint16_t val, char *outStr, size_t maxLen )
83
{
84
    StrPrintf( outStr, maxLen, "%d degrees", (int)( val * 300ul + 511ul ) / 1023ul );
85
}
86
 
87
bool BLD_RegParseAngle( struct BLD_Reg_s *reg, StrTokenizer &line, uint16_t *valp )
88
{
89
    uint16_t    angle;
90
 
91
    if ( !line.NextNum( &angle ))
92
    {
93
        LogError( "Please specify a numeric angle\n" );
94
        return false;
95
    }
96
    *valp = (unsigned)angle * 0x3ffu / 300u;
97
    if ( angle > 300 )
98
    {
99
        LogError( "Angle must be less than 300: got: %u\n", angle );
100
        return false;
101
    }
102
 
103
    return true;
104
}
105
 
106
void BLD_RegFmtTemp( struct BLD_Reg_s *reg, uint16_t val, char *outStr, size_t maxLen )
107
{
108
    StrPrintf( outStr, maxLen, "%dC", val );
109
}
110
 
111
bool BLD_RegParseTemp( struct BLD_Reg_s *reg, StrTokenizer &line, uint16_t *valp )
112
{
113
    uint16_t    temp;
114
 
115
    if ( !line.NextNum( &temp ))
116
    {
117
        LogError( "Please specify a numeric temperature\n" );
118
        return false;
119
    }
120
    *valp = temp;
121
    if ( *valp > reg->maxVal )
122
    {
123
        LogError( "Temp value must be less than %u: got: %u\n", reg->maxVal, *valp );
124
        return false;
125
    }
126
 
127
    return true;
128
}
129
 
130
void BLD_RegFmtVolt( struct BLD_Reg_s *reg, uint16_t val, char *outStr, size_t maxLen )
131
{
132
    StrPrintf( outStr, maxLen, "%d.%d volts", val / 10, val % 10 );
133
}
134
 
135
bool BLD_RegParseVolt( struct BLD_Reg_s *reg, StrTokenizer &line, uint16_t *valp )
136
{
137
    double  voltage;
138
 
139
    if ( !line.NextNum( &voltage ))
140
    {
141
        LogError( "Please specify a numeric temperature\n" );
142
        return false;
143
    }
144
    *valp = (uint16_t)( voltage / 10.0 );
145
    if ( *valp < reg->minVal )
146
    {
147
        LogError( "Voltage value must be greater than %u: got: %u\n", reg->minVal, *valp );
148
        return false;
149
    }
150
    if ( *valp > reg->maxVal )
151
    {
152
        LogError( "Voltage value must be less than %u: got: %u\n", reg->maxVal, *valp );
153
        return false;
154
    }
155
 
156
    return true;
157
}
158
 
159
void BLD_RegFmtStatusRet( struct BLD_Reg_s *reg, uint16_t val, char *outStr, size_t maxLen )
160
{
161
    StrPrintf( outStr, maxLen, "%s", ( val == 0 ) ? "none" :
162
                                     ( val == 1 ) ? "read" :
163
                                     ( val == 2 ) ? "all" : "???" );
164
}
165
 
166
bool BLD_RegParseStatusRet( struct BLD_Reg_s *reg, StrTokenizer &line, uint16_t *valp )
167
{
168
    const char *token;
169
 
170
    token = line.NextToken();
171
 
172
    if ( stricmp( token, "none" ) == 0 )
173
    {
174
        *valp = 0;
175
        return true;
176
    }
177
 
178
    if ( stricmp( token, "read" ) == 0 )
179
    {
180
        *valp = 1;
181
        return true;
182
    }
183
 
184
    if ( stricmp( token, "all" ) == 0 )
185
    {
186
        *valp = 2;
187
        return true;
188
    }
189
 
190
    LogError( "Status Return must be none, read, or all, found '%s'\n", token );
191
    return false;
192
}
193
 
194
void BLD_RegFmtAlarm( struct BLD_Reg_s *reg, uint16_t val, char *outStr, size_t maxLen )
195
{
196
    StrPrintf( outStr, maxLen, "%s%s%s%s%s%s%s%s",
197
               (( val & Bioloid::ERROR_INSTRUCTION ) != 0 )   ? "Inst "   : "",
198
               (( val & Bioloid::ERROR_OVERLOAD ) != 0 )      ? "OvLoad " : "",
199
               (( val & Bioloid::ERROR_CHECKSUM ) != 0 )      ? "ChkSum " : "",
200
               (( val & Bioloid::ERROR_RANGE ) != 0 )         ? "Range "  : "",
201
               (( val & Bioloid::ERROR_OVERHEATING ) != 0 )   ? "OvHeat " : "",
202
               (( val & Bioloid::ERROR_ANGLE_LIMIT ) != 0 )   ? "AngLim " : "",
203
               (( val & Bioloid::ERROR_INPUT_VOLTAGE ) != 0 ) ? "InVolt " : "",
204
               ( val == Bioloid::ERROR_NONE )                 ? "None "   : "" );
205
}
206
 
207
void BLD_RegFmtOnOff( struct BLD_Reg_s *reg, uint16_t val, char *outStr, size_t maxLen )
208
{
209
    StrPrintf( outStr, maxLen, "%s", val ? "on" : "off" );
210
}
211
 
212
bool BLD_RegParseOnOff( struct BLD_Reg_s *reg, StrTokenizer &line, uint16_t *valp )
213
{
214
    const char *token;
215
 
216
    token = line.NextToken();
217
 
218
    if (( stricmp( token, "on" ) == 0 ) || ( strcmp( token, "1" ) == 0 ))
219
    {
220
        *valp = 1;
221
        return true;
222
    }
223
    if (( stricmp( token, "off" ) == 0 ) || ( strcmp( token, "0" ) == 0 ))
224
    {
225
        *valp = 0;
226
        return true;
227
    }
228
 
229
    LogError( "Expecting on or off, got: '%s'\n", token );
230
    return false;
231
}
232
 
233
void BLD_RegFmtVelocity( struct BLD_Reg_s *reg, uint16_t val, char *outStr, size_t maxLen )
234
{
235
    uint32_t    rpm;
236
 
237
    rpm = (( val * 1140 ) + 511 ) / 1023;
238
 
239
    StrPrintf( outStr, maxLen, "%d.%d RPM",
240
               rpm / 10, rpm % 10 );
241
}
242
 
243
bool BLD_RegParseVelocity( struct BLD_Reg_s *reg, StrTokenizer &line, uint16_t *valp )
244
{
245
    double rpm;
246
 
247
    if ( !line.NextNum( &rpm ))
248
    {
249
        LogError( "Please specify a numeric rpm\n" );
250
        return false;
251
    }
252
    if (( rpm < 0.0 ) || ( rpm > 114.0 ))
253
    {
254
        LogError( "Expecting velocity to be between 0.0 and 114.0: found: %f\n", rpm );
255
        return false;
256
    }
257
 
258
    Log( "rpm = %f\n", rpm );
259
 
260
    *valp = (uint16_t)((( rpm / 114.0 ) * 1023.0 ) + 0.5 );
261
 
262
    return true;
263
}
264
 
265
void BLD_RegFmtLoad( struct BLD_Reg_s *reg, uint16_t val, char *outStr, size_t maxLen )
266
{
267
    StrPrintf( outStr, maxLen, "%s %d",
268
               ( val & ( 1 << 10 )) ? "CW" : "CCW",
269
               val & (( 1 << 10 ) - 1 ));
270
}
271