Subversion Repositories Projects

Rev

Rev 211 | Rev 216 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
211 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   BioloidCommandLine.cpp
18
*
19
*   @brief  This file implements the BioloidCommandLine class, which
20
*           parses command lines and sends the commands to devices on the
21
*   bioloid bus.
22
*
23
****************************************************************************/
24
 
25
// ---- Include Files -------------------------------------------------------
26
 
27
#include <string.h>
213 dhylands 28
#include "Str.h"
29
#include "DumpMem.h"
211 dhylands 30
#include "Log.h"
31
#include "Bioloid.h"
32
#include "BioloidCommandLine.h"
33
 
34
// ---- Public Variables ----------------------------------------------------
35
// ---- Private Constants and Types -----------------------------------------
36
// ---- Private Variables ---------------------------------------------------
37
 
213 dhylands 38
static BLD_DevType_t   *gDevType;
211 dhylands 39
 
213 dhylands 40
static char             gDelim[] = " \r\n\t";
211 dhylands 41
 
213 dhylands 42
static uint8_t          gBuf[ 80 ];
211 dhylands 43
 
44
// ---- Private Function Prototypes -----------------------------------------
45
// ---- Functions -----------------------------------------------------------
46
 
47
/**
48
 * @addtogroup bioloid
49
 * @{
50
 */
51
 
52
//***************************************************************************
53
/**
54
*   Constructor
55
*/
56
 
57
BioloidCommandLine::BioloidCommandLine()
58
    : m_bus( NULL )
59
{
60
}
61
 
62
//***************************************************************************
63
/**
64
*   Destructor
65
*
66
*   virtual
67
*/
68
 
69
BioloidCommandLine::~BioloidCommandLine()
70
{
71
}
72
 
73
//***************************************************************************
74
/**
75
*   Dumps information about the registers
76
*/
77
 
78
void BioloidCommandLine::DumpRegInfo( BLD_DevType_t *devType )
79
{
80
    BLD_Reg_t   *reg;
81
 
82
    Log( "Addr Size Min  Max Name\n" );
83
    Log( "---- ---- ---  --- --------------------\n" );
84
 
85
    for ( reg = devType->reg; reg->name != NULL; reg++ )
86
    {
87
        if (( reg->flags & BLD_REG_FLAG_WR ) == 0 )
88
        {
89
            Log( "0x%02x ro %d          %s\n",
90
                 reg->address, reg->flags & BLD_REG_FLAG_16BIT ? 2 : 1, reg->name );
91
        }
92
        else
93
        {
94
            Log( "0x%02x rw %d %3d %4d %s\n",
95
                 reg->address, reg->flags & BLD_REG_FLAG_16BIT ? 2 : 1,
96
                 reg->minVal, reg->maxVal, reg->name );
97
        }
98
    }
99
}
100
 
101
//***************************************************************************
102
/**
103
*   Parses an offset and some data.
104
*/
105
 
106
bool BioloidCommandLine::ParseOffsetAndData( StrTokenizer &line, uint8_t *offset, uint8_t *numBytes, uint8_t *data, size_t maxLen )
107
{
108
    *numBytes = 0;
109
 
110
    if ( !line.NextNum( gDelim, offset ))
111
    {
112
        LogError( "Invalid offset specified: '%s'\n", line.PrevToken() );
113
        return false;
114
    }
115
 
116
    while (( *numBytes < maxLen ) && line.NextNum( gDelim, &data[ *numBytes ] ))
117
    {
118
        (*numBytes)++;
119
    }
120
 
121
    return true;
122
}
123
 
124
//***************************************************************************
125
/**
126
*   Parses a register name
127
*/
128
 
129
bool BioloidCommandLine::ParseRegisterName( StrTokenizer &line, BLD_DevType_t *devType, BLD_Reg_t **outReg )
130
{
131
    BLD_Reg_t   *reg;
132
    char        *regStr;
133
 
134
    if (( regStr = line.NextToken( gDelim )) == NULL )
135
    {
136
        LogError( "No register specified\n" );
137
        return false;
138
    }
139
 
140
    reg = devType->reg;
141
    while ( reg->name != NULL )
142
    {
143
        if ( strcmp( regStr, reg->name ) == 0 )
144
        {
145
            *outReg = reg;
146
            return true;
147
        }
148
        reg++;
149
    }
150
 
151
    LogError( "Unrecognized register name: '%s'\n", regStr );
152
    return false;
153
}
154
 
155
//***************************************************************************
156
/**
213 dhylands 157
*   Parses the error code and prints the results.
158
*/
159
void BioloidCommandLine::AddErrorStr( Bioloid::Error err, Bioloid::Error mask, char *str, size_t maxLen, const char *errStr )
160
{
161
    if (( err & mask ) != 0 )
162
    {
163
        if ( str[0] != '\0' )
164
        {
165
            StrMaxCat( str, ",", maxLen );
166
        }
167
        StrMaxCat( str, errStr, maxLen );
168
    }
169
}
170
 
171
//***************************************************************************
172
/**
173
*   Prints the error code and prints the results.
174
*/
175
void BioloidCommandLine::PrintError( Bioloid::Error err )
176
{
177
    char   *str = (char *)&gBuf[0];
178
 
179
    str[0] = '\0';
180
 
181
    if ( err == Bioloid::ERROR_NONE )
182
    {
183
        return;
184
    }
185
 
186
    if ( err > 0xff )
187
    {
188
        char    *errStr;
189
 
190
        switch ( err )
191
        {
192
            case Bioloid::ERROR_NOT_DONE:       errStr = "Not Done";        break;
193
            case Bioloid::ERROR_TIMEOUT:        errStr = "Timeout";         break;
194
            case Bioloid::ERROR_TOO_MUCH_DATA:  errStr = "Too Much Data";   break;
195
            default:                            errStr = "***Unknown***";   break;
196
        }
197
 
198
        StrMaxCpy( str, errStr, sizeof( gBuf ));
199
    }
200
    else
201
    {
202
        AddErrorStr( err, Bioloid::ERROR_RESERVED,       str, sizeof( gBuf ), "Reserved" );
203
        AddErrorStr( err, Bioloid::ERROR_INSTRUCTION,    str, sizeof( gBuf ), "Instruction" );
204
        AddErrorStr( err, Bioloid::ERROR_OVERLOAD,       str, sizeof( gBuf ), "Overload" );
205
        AddErrorStr( err, Bioloid::ERROR_CHECKSUM,       str, sizeof( gBuf ), "Checksum" );
206
        AddErrorStr( err, Bioloid::ERROR_RANGE,          str, sizeof( gBuf ), "Range" );
207
        AddErrorStr( err, Bioloid::ERROR_OVERHEATING,    str, sizeof( gBuf ), "Over Heating" );
208
        AddErrorStr( err, Bioloid::ERROR_ANGLE_LIMIT,    str, sizeof( gBuf ), "Angle Limit" );
209
        AddErrorStr( err, Bioloid::ERROR_INPUT_VOLTAGE,  str, sizeof( gBuf ), "Input Voltage" );
210
    }
211
    Log( "%s\n", str );
212
}
213
 
214
//***************************************************************************
215
/**
216
*   Called from the Scan command when a device is found.
217
*/
218
static bool DevFound( BioloidBus *bus, BioloidDevice *dev )
219
{
220
    uint16_t    model;
221
    uint8_t     version;
222
 
223
    dev->Read( 0, &model );
224
    dev->Read( 2, &version );
225
 
226
    Log( "ID: %3d Model: 0x%04x Version: 0x%02x\n", dev->ID(), model, version );
227
 
228
    return true;
229
}
230
 
231
//***************************************************************************
232
/**
211 dhylands 233
*   Processes one line of data
234
*/
235
 
236
bool BioloidCommandLine::ProcessLine( char *lineStr )
237
{
238
    char           *devTypeStr;
239
    BLD_DevType_t  *devType;
240
    char           *endPtr;
241
    char            token[ 20 ];
242
    StrTokenizer    line( lineStr, token, sizeof( token ));
243
 
244
    if ( m_bus == NULL )
245
    {
246
        LogError( "SetBus not called\n" );
247
        return false;
248
    }
249
 
250
    // Pull out the device type
251
 
252
    if (( devTypeStr = line.NextToken( gDelim )) == NULL )
253
    {
254
        // Empty line - ignore
255
 
256
        return true;
257
    }
258
 
259
    // Check for special non-device type commands
260
 
261
    if ( strcmp( devTypeStr, "action" ) == 0 )
262
    {
263
        m_bus->SendAction();
264
 
265
        // The action command is sent as a broadcast, so no response
266
        // is expected.
267
 
268
        return true;
269
    }
213 dhylands 270
    if ( strcmp( devTypeStr, "scan" ) == 0 )
271
    {
272
        if ( !m_bus->Scan( DevFound ))
273
        {
274
            Log( "No devices found\n" );
275
        }
276
 
277
        return true;
278
    }
279
 
211 dhylands 280
    if ( strcmp( devTypeStr, "quit" ) == 0 )
281
    {
282
        return false;
283
    }
284
 
285
    // Since it's not one of those - assume it's a device type
286
 
287
    devType = gDevType;
288
    while (( devType->devTypeStr != NULL )
289
        && ( strcmp( devType->devTypeStr, devTypeStr ) != 0 ))
290
    {
291
        devType++;
292
    }
293
 
294
    if ( devType->devTypeStr == NULL )
295
    {
296
        LogError( "Unrecognized device type: '%s'\n", devTypeStr );
297
        return true;
298
    }
299
 
300
    Bioloid::ID_t id;
301
 
302
    if ( !line.NextNum( gDelim, &id ))
303
    {
304
        if ( strcmp( line.PrevToken(), "reg" ) == 0 )
305
        {
306
            DumpRegInfo( devType );
307
            return true;
308
        }
309
        LogError( "Invalid ID: '%s'\n", line.PrevToken() );
310
        return true;
311
    }
312
    if ( id >= Bioloid::INVALID_ID )
313
    {
314
        LogError( "IDs must be 254 (0xFE) or less\n" );
315
        return true;
316
    }
317
    m_device.SetBusAndID( m_bus, id );
318
 
319
    char *cmdStr;
320
 
321
    if (( cmdStr = line.NextToken( gDelim )) == NULL )
322
    {
323
        LogError( "No command specified for %s %u\n", devType->devTypeStr, id );
324
        return true;
325
    }
326
 
327
    LogVerbose( "DevType: %s ID: %d Cmd: %s\n", devType->devTypeStr, id, cmdStr );
328
 
329
    if ( strcmp( cmdStr, "ping" ) == 0 )
330
    {
213 dhylands 331
        Bioloid::Error err;
332
 
211 dhylands 333
        if ( id == Bioloid::BROADCAST_ID )
334
        {
335
            LogError( "Broadcast ID not valid with ping command\n" );
336
            return true;
337
        }
338
 
213 dhylands 339
        Log( "%s %d ", devType->devTypeStr, id );
340
        if (( err = m_device.Ping()) == Bioloid::ERROR_NONE )
341
        {
342
            Log( "Response Received\n" );
343
        }
344
        else
345
        {
346
            PrintError( err );
347
        }
211 dhylands 348
    }
349
    else
350
    if (( strcmp( cmdStr, "read-data" ) == 0 )
351
    ||  ( strcmp( cmdStr, "rd" ) == 0 ))
352
    {
353
        uint8_t offset;
354
        uint8_t numBytes;
355
 
356
        if ( id == Bioloid::BROADCAST_ID )
357
        {
358
            LogError( "Broadcast ID not valid with read-data command\n" );
359
            return true;
360
        }
361
 
362
        if ( !line.NextNum( gDelim, &offset ))
363
        {
364
            LogError( "Invalid offset specified: '%s'\n", line.PrevToken() );
365
            return true;
366
        }
367
        if ( !line.NextNum( gDelim, &numBytes ))
368
        {
369
            LogError( "Invalid numBytes specified: '%s'\n", line.PrevToken() );
370
            return true;
371
        }
372
 
213 dhylands 373
        PrintError( m_device.Read( offset, gBuf, numBytes ));
374
 
375
        DumpMem( "Read", offset, gBuf, numBytes );
211 dhylands 376
    }
377
    else
378
    if (( strcmp( cmdStr, "write-data" ) == 0 )
379
    ||  ( strcmp( cmdStr, "wd" ) == 0 ))
380
    {
381
        uint8_t offset;
382
        uint8_t numBytes;
383
        uint8_t data[ 20 ];
384
 
385
        if ( !ParseOffsetAndData( line, &offset, &numBytes, data, sizeof( data )))
386
        {
387
            return true;
388
        }
213 dhylands 389
        PrintError( m_device.Write( offset, data, numBytes ));
211 dhylands 390
    }
391
    else
392
    if (( strcmp( cmdStr, "reg-write" ) == 0 )
393
    ||  ( strcmp( cmdStr, "rw" ) == 0 ))
394
    {
395
        uint8_t offset;
396
        uint8_t numBytes;
397
        uint8_t data[ 20 ];
398
 
399
        if ( !ParseOffsetAndData( line, &offset, &numBytes, data, sizeof( data )))
400
        {
401
            return true;
402
        }
213 dhylands 403
        m_device.SendDeferredWrite( offset, data, numBytes );
211 dhylands 404
    }
405
    else
406
    if ( strcmp( cmdStr, "get" ) == 0 )
407
    {
408
        BLD_Reg_t   *reg;
409
 
410
        if ( id == Bioloid::BROADCAST_ID )
411
        {
412
            LogError( "Broadcast ID not valid with get command\n" );
413
            return true;
414
        }
415
 
213 dhylands 416
        if ( strncmp( line.Remainder(), "all", 3 ) == 0 )
211 dhylands 417
        {
213 dhylands 418
            reg = devType->reg;
211 dhylands 419
 
213 dhylands 420
            Log( "Addr Size Value Name\n" );
421
            Log( "---- ---- ----- --------------------\n" );
211 dhylands 422
 
213 dhylands 423
            while ( reg->name != NULL )
424
            {
425
                unsigned    val;
426
 
427
                if ( reg->flags & BLD_REG_FLAG_16BIT )
428
                {
429
                    uint16_t    val16;
430
 
431
                    m_device.Read( reg->address, &val16 );
432
 
433
 
434
                    val = val16;
435
                }
436
                else
437
                {
438
                    uint8_t    val8;
439
 
440
                    m_device.Read( reg->address, &val8 );
441
 
442
                    val = val8;
443
                }
444
 
445
                Log( "0x%02x %s %d %5u %s\n",
446
                    reg->address,
447
                    reg->flags & BLD_REG_FLAG_WR ? "rw" : "ro",
448
                    reg->flags & BLD_REG_FLAG_16BIT ? 2 : 1,
449
                    val,
450
                    reg->name );
451
 
452
                reg++;
453
            }
454
        }
455
        else
456
        {
457
            if ( !ParseRegisterName( line, devType, &reg ))
458
            {
459
                return true;
460
            }
461
 
462
            if (( reg->flags & BLD_REG_FLAG_16BIT ) != 0 )
463
            {
464
                uint16_t    val16;
465
 
466
                PrintError( m_device.Read( reg->address, &val16, sizeof( val16 )));
467
                Log( "Read: %u\n", val16 );
468
            }
469
            else
470
            {
471
                uint16_t    val8;
472
 
473
                PrintError( m_device.Read( reg->address, &val8, sizeof( val8 )));
474
                Log( "Read: %u\n", val8 );
475
            }
476
        }
211 dhylands 477
    }
478
    else
479
    if ( strcmp( cmdStr, "set" ) == 0 )
480
    {
481
        BLD_Reg_t  *reg;
482
        uint16_t    val16;
483
 
484
        if ( !ParseRegisterName( line, devType, &reg ))
485
        {
486
            return true;
487
        }
488
        if (( reg->flags & BLD_REG_FLAG_WR ) == 0 )
489
        {
490
            LogError( "Register %s is read-only\n", reg->name );
491
            return true;
492
        }
493
 
494
        if ( !line.NextNum( gDelim, &val16 ))
495
        {
496
            LogError( "Invalid value specified: '%s'\n", line.PrevToken() );
497
            return true;
498
        }
499
 
500
        if (( val16 < reg->minVal  ) || ( val16 > reg->maxVal ))
501
        {
502
            LogError( "Value %u is out of range (%u - %u)\n", val16, reg->minVal, reg->maxVal );
503
            return true;
504
        }
505
 
213 dhylands 506
        PrintError( m_device.Write( reg->address, &val16, reg->flags & BLD_REG_FLAG_16BIT ? 2 : 1 ));
211 dhylands 507
    }
508
    else
509
    if ( strcmp( cmdStr, "reset" ) == 0 )
510
    {
213 dhylands 511
        m_device.Reset();
211 dhylands 512
    }
513
    else
514
    {
515
        LogError( "Unrecognized command: '%s'\n", cmdStr );
516
    }
517
 
518
    return true;
519
}
520
 
521
//***************************************************************************
522
/**
523
*   Register the devices that we'll recognize
524
*/
525
 
526
void BioloidCommandLine::RegisterDevices( BLD_DevType_t *devType )
527
{
528
    gDevType = devType;
529
}
530