Class: Robobuilder

Inherits:
Object
  • Object
show all
Defined in:
lib/robobuilder_ext.rb

Constant Summary collapse

HEADER = 
"\xFF\xFF\xAA\x55\xAA\x55\x37\xBA"
MAX_READ_TRIES = 
2
MAX_CMD_TRIES = 
5
DELAY = 
0.2
MOTION_TIMEOUT = 
10 * 10
GETUP_A = 
1
GETUP_B = 
2
TURN_LEFT = 
3
MOTION_FORWARD = 
4
TURN_RIGHT = 
5
MOVE_LEFT = 
6
MOTION_BASIC_POSTURE = 
7
MOVE_RIGHT = 
8
ATTACK_LEFT = 
9
MOVE_BACKWARD = 
10
ATTACK_RIGHT = 
11

Class Method Summary collapse

Instance Method Summary collapse

Class Method Details

.new(device_name = '/dev/ttyS0') ⇒ Object 



24
25
26
27
28
29
 
# File 'lib/robobuilder_ext.rb', line 24

def new( device_name = '/dev/ttyS0' )
  retval = orig_new device_name
  # Do some communication to synchronise.
  3.times { retval.serial_number }
  retval
end

.orig_new ⇒ Object 



22
 
# File 'lib/robobuilder_ext.rb', line 22

alias_method :orig_new, :new

Instance Method Details

#a ⇒ Object 



179
180
181
 
# File 'lib/robobuilder_ext.rb', line 179

def a
  motion GETUP_A
end

#accelerometer ⇒ Object 



164
165
166
 
# File 'lib/robobuilder_ext.rb', line 164

def accelerometer
  command( 0x1A, 1 ).unpack( 'sss' )
end

#attack_left ⇒ Object 



211
212
213
 
# File 'lib/robobuilder_ext.rb', line 211

def attack_left
  motion ATTACK_LEFT
end

#attack_right ⇒ Object 



219
220
221
 
# File 'lib/robobuilder_ext.rb', line 219

def attack_right
  motion ATTACK_RIGHT
end

#b ⇒ Object 



183
184
185
 
# File 'lib/robobuilder_ext.rb', line 183

def b
  motion GETUP_B
end

#backward ⇒ Object 



215
216
217
 
# File 'lib/robobuilder_ext.rb', line 215

def backward
  motion MOVE_BACKWARD
end

#basic ⇒ Object 



203
204
205
 
# File 'lib/robobuilder_ext.rb', line 203

def basic
  motion MOTION_BASIC_POSTURE
end

#boundary_read(id) ⇒ Object 



350
351
352
353
 
# File 'lib/robobuilder_ext.rb', line 350

def boundary_read(id)
  raise "id must be in 0 .. 30 (but was #{id})" unless id.between? 0, 30
  rbc 2, (7 << 5) | id, 0x12, 0, 0
end

#boundary_set(id, lower, upper) ⇒ Object 



343
344
345
346
347
348
 
# File 'lib/robobuilder_ext.rb', line 343

def boundary_set(id, lower, upper)
  raise "id must be in 0 .. 30 (but was #{id})" unless id.between? 0, 30
  raise "lower must be in 0 .. 254 (but was #{lower})" unless lower.between? 0, 254
  raise "upper must be in 0 .. 254 (but was #{upper})" unless upper.between? 0, 254
  rbc 2, (7 << 5) | id, 0x11, lower, upper
end

#brake_wck ⇒ Object 



283
284
285
 
# File 'lib/robobuilder_ext.rb', line 283

def brake_wck
  rbc 2, (6 << 5) | 31, 2 << 4
end

#button ⇒ Object

TODO: wait for button



156
157
158
 
# File 'lib/robobuilder_ext.rb', line 156

def button # TODO: wait for button
  command( 0x18, 1 ).unpack( 'S' ).first
end

#checksum(*contents) ⇒ Object 



75
76
77
 
# File 'lib/robobuilder_ext.rb', line 75

def checksum( *contents )
  contents.inject { |a,b| a ^ b }
end

#command(type, *contents) ⇒ Object 



109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
 
# File 'lib/robobuilder_ext.rb', line 109

def command( type, *contents )
  cmd = HEADER + [ type, 0x00, contents.size ].pack( 'CCN' ) +
    contents.pack( 'C' * contents.size ) +
    [ checksum( *contents ) ].pack( 'C' )
  result = nil
  t = 0
  e = nil
  while result == nil and t < MAX_CMD_TRIES
    begin
      e = nil
      write cmd
      result = response type
    rescue Exception => e
      puts "Recovering from error: #{e.to_s}"
      flush
      result = nil
    end
    t += 1
  end
  raise e unless result
  result
end

#direct(on) ⇒ Object 



168
169
170
171
172
173
174
175
176
177
 
# File 'lib/robobuilder_ext.rb', line 168

def direct( on )
  sleep DELAY
  if on
    # Alternatively press and hold button PF2 while turning on RBC power.
    command 0x10, 1
  else
    write "\xFF\xE0\xFB\x01\x00\x1A"
  end
  sleep DELAY
end

#distance ⇒ Object 



152
153
154
 
# File 'lib/robobuilder_ext.rb', line 152

def distance
  command( 0x16, 1 ).unpack( 'n' ).first / 256.0
end

#firmware ⇒ Object 



136
137
138
 
# File 'lib/robobuilder_ext.rb', line 136

def firmware
  command( 0x12, 1 ).unpack 'CC'
end

#forward ⇒ Object 



191
192
193
 
# File 'lib/robobuilder_ext.rb', line 191

def forward
  motion MOTION_FORWARD
end

#hexdump(*contents) ⇒ Object 



79
80
81
 
# File 'lib/robobuilder_ext.rb', line 79

def hexdump( *contents )
  contents.collect { |x| "%02X" % x }.join( ' ' )
end

#left ⇒ Object 



199
200
201
 
# File 'lib/robobuilder_ext.rb', line 199

def left
  motion MOVE_LEFT
end

#motion(n) ⇒ Object 



140
141
142
143
144
145
146
 
# File 'lib/robobuilder_ext.rb', line 140

def motion( n )
  sleep DELAY
  timeout( MOTION_TIMEOUT ) do
    command( 0x14, n ).unpack( 'C' ).first
  end
  self
end

#orig_read ⇒ Object 



44
 
# File 'lib/robobuilder_ext.rb', line 44

alias_method :orig_read, :read

#orig_timeout ⇒ Object 



60
 
# File 'lib/robobuilder_ext.rb', line 60

alias_method :orig_timeout, :timeout

#orig_write ⇒ Object 



33
 
# File 'lib/robobuilder_ext.rb', line 33

alias_method :orig_write, :write

#overload_read(id) ⇒ Object 



334
335
336
337
338
339
340
341
 
# File 'lib/robobuilder_ext.rb', line 334

def overload_read(id)
  raise "id must be in 0 .. 30 (but was #{id})" unless id.between? 0, 30
  retval = rbc 2, (7 << 5) | id, 0x10, 0, 0
  if retval[0] != retval[1]
    raise "Unexpected return value #{retval.inspect} by overload read"
  end
  retval.first
end

#overload_set(id, overload) ⇒ Object 



322
323
324
325
326
327
328
329
330
331
332
 
# File 'lib/robobuilder_ext.rb', line 322

def overload_set(id, overload)
  raise "id must be in 0 .. 30 (but was #{id})" unless id.between? 0, 30
  unless overload.between? 33, 199
    raise "overload must be in 33 .. 199 (but was #{overload})"
  end
  retval = rbc 2, (7 << 5) | id, 0x0F, overload, overload
  if retval[0] != retval[1]
    raise "Unexpected return value #{retval.inspect} by overload set"
  end
  retval.first
end

#passive_wck(id) ⇒ Object 



272
273
274
275
 
# File 'lib/robobuilder_ext.rb', line 272

def passive_wck(id)
  raise "id must be in 0 .. 30 (but was #{id})" unless id.between? 0, 30
  rbc 2, (6 << 5) | id, 1 << 4
end

#position_move(id, target, torque) ⇒ Object 



257
258
259
260
261
262
 
# File 'lib/robobuilder_ext.rb', line 257

def position_move(id, target, torque)
  raise "id must be in 0 .. 30 (but was #{id})" unless id.between? 0, 30
  raise "target must be in 0 .. 254 (but was #{target})" unless target.between? 0, 254
  raise "torque must be in 0 .. 4 (but was #{torque})" unless torque.between? 0, 4
  rbc 2, (torque << 5) | id, target
end

#precision_move(id, target, torque) ⇒ Object

motion data read



363
364
365
366
367
368
369
 
# File 'lib/robobuilder_ext.rb', line 363

def precision_move(id, target, torque)
  raise "id must be in 0 .. 253 (but was #{id})" unless id.between? 0, 253
  raise "target must be in 0 .. 1023 (but was #{target})" unless target.between? 0, 1023
  raise "torque must be in 0 .. 254 (but was #{torque})" unless torque.between? 0, 254
  retval = rbc 2, 7 << 5, 0xC8, id, torque, target >> 6, (target << 1) & 0x7E
  (retval[0] << 6) | (retval[1] >> 1)
end

#precision_read(id) ⇒ Object 



371
372
373
374
375
 
# File 'lib/robobuilder_ext.rb', line 371

def precision_read(id)
  raise "id must be in 0 .. 253 (but was #{id})" unless id.between? 0, 253
  retval = rbc 2, 7 << 5, 0xC9, id, id
  (retval[0] << 6) | (retval[1] >> 1)
end

#rbc(response, *contents) ⇒ Object 



234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
 
# File 'lib/robobuilder_ext.rb', line 234

def rbc(response, *contents)
  cmd = "\xFF" + contents.pack('C' * contents.size) +
    [checksum(*contents) & 0x7F].pack('C')
  #puts cmd.unpack('C' * cmd.size).collect { |x| "0x%X" % x }.join ' '
  result = nil
  t = 0
  e = nil
  while result == nil and t < MAX_CMD_TRIES
    begin
      e = nil
      write cmd
      result = read(response).unpack 'C' * response
    rescue Exception => e
      puts "Recovering from error: #{e.to_s}"
      flush
      result = nil
    end
    t += 1
  end
  raise e unless result
  result
end

#read(n) ⇒ Object 



46
47
48
49
50
51
52
53
54
55
56
57
58
 
# File 'lib/robobuilder_ext.rb', line 46

def read( n )
  t = 0
  retval = ''
  while retval.size < n and t < MAX_READ_TRIES
    retval += orig_read n - retval.size
    t += 1
  end
  if retval.size < n
    raise "Not receiving any response from serial device " +
      "(only received #{retval.size}/#{n} bytes)" 
  end
  retval
end

#remote ⇒ Object

???



160
161
162
 
# File 'lib/robobuilder_ext.rb', line 160

def remote # ???
  command( 0x19, 1 )
end

#response(type) ⇒ Object 



83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
 
# File 'lib/robobuilder_ext.rb', line 83

def response( type )
  header = read HEADER.size
  if header != HEADER
    raise "Expecting #{hexdump( *HEADER.unpack( 'C' * HEADER.size ) )} " +
      "(but received #{hexdump( *header.unpack( 'C' * header.size ) )})"
  end
  result_type = read( 1 ).unpack( 'C' ).first
  if result_type != type
    raise "Result was of type #{ "%02X" % result_type } but command was " +
      "of type #{ "%02X" % type }"
  end
  read 1
  size = read( 4 ).unpack( 'N' ).first
  unless size.between? 1, 16
    raise "Size of result should be between 1 and 16 (but was #{size})"
  end
  contents = read size
  checksum_real = checksum( *contents.unpack( 'C' * contents.size ) )
  checksum_nominal = read( 1 ).unpack( 'C' ).first
  if type != 0x1A and checksum_real != checksum_nominal
    puts "Checksum was #{ "%02X" % checksum_real } but should be " +
      "#{ "%02X" % checksum_nominal }"
  end
  contents
end

#right ⇒ Object 



207
208
209
 
# File 'lib/robobuilder_ext.rb', line 207

def right
  motion MOVE_RIGHT
end

#run(n) ⇒ Object 



223
224
225
226
227
228
229
230
231
232
 
# File 'lib/robobuilder_ext.rb', line 223

def run( n )
  case n
  when 1 .. 10
    motion n + 11
  when 11 .. 20
    motion n + 22
  else
    raise "Program number must be in 1 .. 20 (was #{n})"
  end
end

#runtime_speed_set(id, speed, accel) ⇒ Object 



315
316
317
318
319
320
 
# File 'lib/robobuilder_ext.rb', line 315

def runtime_speed_set(id, speed, accel)
  raise "id must be in 0 .. 30 (but was #{id})" unless id.between? 0, 30
  raise "speed must be in 0 .. 30 (but was #{speed})" unless speed.between? 0, 30
  raise "accel must be in 20 .. 100 (but was #{accel})" unless accel.between? 20, 120
  rbc 2, (7 << 5) | id, 0x17, speed, accel
end

#serial_number ⇒ Object 



132
133
134
 
# File 'lib/robobuilder_ext.rb', line 132

def serial_number
  command( 0x0C, 1 ).to_i
end

#speed_read(id) ⇒ Object 



310
311
312
313
 
# File 'lib/robobuilder_ext.rb', line 310

def speed_read(id)
  raise "id must be in 0 .. 30 (but was #{id})" unless id.between? 0, 30
  rbc 2, (7 << 5) | id, 0x0E, 0, 0
end

#speed_set(id, speed, accel) ⇒ Object

id set



303
304
305
306
307
308
 
# File 'lib/robobuilder_ext.rb', line 303

def speed_set(id, speed, accel)
  raise "id must be in 0 .. 30 (but was #{id})" unless id.between? 0, 30
  raise "speed must be in 0 .. 30 (but was #{speed})" unless speed.between? 0, 30
  raise "accel must be in 20 .. 100 (but was #{accel})" unless accel.between? 20, 120
  rbc 2, (7 << 5) | id, 0x0D, speed, accel
end

#status_read(id) ⇒ Object 



267
268
269
270
 
# File 'lib/robobuilder_ext.rb', line 267

def status_read(id)
  raise "id must be in 0 .. 30 (but was #{id})" unless id.between? 0, 30
  rbc 2, (5 << 5) | id, 0
end

#synchronised_position_move ⇒ Object 



264
265
 
# File 'lib/robobuilder_ext.rb', line 264

def synchronised_position_move
end

#timeout(value = nil) ⇒ Object 



62
63
64
65
66
67
68
69
70
71
72
73
 
# File 'lib/robobuilder_ext.rb', line 62

def timeout( value = nil )
  retval = orig_timeout
  if value
    begin
      self.timeout = value
      yield
    ensure
      self.timeout = retval
    end
  end
  retval
end

#turn_left ⇒ Object 



187
188
189
 
# File 'lib/robobuilder_ext.rb', line 187

def turn_left
  motion TURN_LEFT
end

#turn_right ⇒ Object 



195
196
197
 
# File 'lib/robobuilder_ext.rb', line 195

def turn_right
  motion TURN_RIGHT
end

#voice(n) ⇒ Object 



148
149
150
 
# File 'lib/robobuilder_ext.rb', line 148

def voice( n )
  command( 0x15, n ).unpack( 'C' ).first
end

#wheel_wck(id, speed) ⇒ Object 



277
278
279
280
281
 
# File 'lib/robobuilder_ext.rb', line 277

def wheel_wck(id, speed)
  raise "id must be in 0 .. 30 (but was #{id})" unless id.between? 0, 30
  raise "speed must be in -15 .. 15 (but was #{speed})" unless speed.between? -15, 15
  rbc 2, (6 << 5) | id, ((speed >= 0 ? 3 : 4) << 4) | speed.abs
end

#write(data) ⇒ Object 



35
36
37
38
39
40
41
42
 
# File 'lib/robobuilder_ext.rb', line 35

def write( data )
  n = orig_write( data )
  if n < data.size
    raise "Error writing to serial port (only #{n} of #{data.size} bytes " +
      "were written)" 
  end
  n
end