def testNewSDO(filename):
from can import ReplyLog
can = CAN(ReplyLog(filename))
can.resetModules()
(nodeID, dataIndex, subIndex) = 10, 16 * 256 + 10, 0
reader = ReadSDO(nodeID, dataIndex, subIndex)
for packet in reader.generator():
if packet != None:
can.sendData(*packet)
reader.update(can.readPacket())
print("RESUT DATA:", reader.result)
def testNewSDO( filename ):
from can import ReplyLog
can=CAN(ReplyLog(filename))
can.resetModules()
( nodeID, dataIndex, subIndex ) = 10, 16*256+10, 0
reader = ReadSDO( nodeID, dataIndex, subIndex )
for packet in reader.generator():
if packet != None:
can.sendData( *packet )
reader.update( can.readPacket() )
print("RESUT DATA:", reader.result)
class Spider3(object):
UPDATE_TIME_FREQUENCY = 20.0 # Hz
# status word
EMERGENCY_STOP = 0x0001
DEVICE_READY = 0x0002 # also releaseState
MASK_HEARTBEAT = 0x8000
def __init__(self, can=None):
if can is None:
self.can = CAN()
# self.can.resetModules()
else:
self.can = can
self.time = 0.0
self.status_word = None # not defined yet
self.wheel_angles = [None]*4 # original CAN data
self.drive_status = [None]*4
self.zero_steering = None # zero position of all 4 wheels
self.alive = 0 # togle with 128
self.speed_cmd = [0, 0]
self.status_cmd = 3
self.extensions = []
self.data_sources = []
self.threads = []
self.modulesForRestart = []
self.led = 0
self.led_time = 0.0
# self.can.sendOperationMode()
def __del__(self):
pass
# self.can.sendPreoperationMode()
# TODO to move out, can.py??
def check_modules(self, packet):
# test if all modules are in Operation mode, if not restart them
id, data = packet
if id & 0xFF0 == 0x700: # heart beat message
moduleId = id & 0xF
assert( len(data) == 1 )
if data[0] != 5:
self.can.printPacket( id, data )
if not moduleId in self.modulesForRestart:
self.modulesForRestart.append( moduleId )
print("RESET", moduleId)
self.can.sendData( 0, [129,moduleId] )
# if moduleId in [0x01, 0x02]:
# if (0x180 | moduleId) in self.encData:
# # The encoder information is invalid during a reset
# del self.encData[0x180 | moduleId]
elif data[0] == 127: # restarted and in preoperation
print("SWITCH TO OPERATION", moduleId)
self.can.sendData( 0, [1,moduleId] )
elif moduleId in self.modulesForRestart:
print("RUNNING", moduleId)
self.modulesForRestart.remove(moduleId)
def register_data_source(self, name, function, extension=None):
self.data_sources.append((name, function))
if extension is not None:
self.extensions.append((name, extension))
def wait(self, duration):
start_time = self.time
while self.time - start_time < duration:
self.update()
def update_status_word(self, packet):
msg_id, data = packet
if msg_id == 0x200:
assert len(data) == 2, packet
prev = self.status_word
self.status_word = struct.unpack('H', bytearray(data))[0]
if prev is not None:
diff = prev ^ self.status_word
is_alive = diff & self.MASK_HEARTBEAT
if is_alive == 0:
# make sure nothing is missing (for now)
print("\n!!!Spider is DEAD!!! (or data are missing)\n")
if (diff & 0x7FFF) != 0:
print("Status %d -> %d" %
(prev & 0x7FFF, self.status_word & 0x7FFF))
elif msg_id == 0x201:
assert len(data) == 8, packet
prev = self.wheel_angles
self.wheel_angles = struct.unpack('HHHH', bytearray(data))
if prev != self.wheel_angles and self.zero_steering is not None:
# note, that self.zero_steering has 200ms update rate,
# i.e. it does not have to be immediately defined
print('Wheels: %.2f' % self.time, [fix_range(a - b) for a, b in zip(self.wheel_angles, self.zero_steering)])
elif msg_id == 0x202:
assert len(data) == 8, packet # drive status + zero positions
self.drive_status = struct.unpack('HHHH', bytearray(data))
drive = self.drive_status[0] - self.drive_status[2], self.drive_status[3] - self.drive_status[1]
if abs(drive[0]) + abs(drive[1]) > 10:
print('Drive:', drive, abs(drive[0]) + abs(drive[1]))
elif msg_id == 0x203:
assert len(data) == 8, packet
prev = self.zero_steering
self.zero_steering = struct.unpack('HHHH', bytearray(data))
# make sure that calibration did not change during program run
assert prev is None or prev == self.zero_steering, (prev, self.zero_steering)
def send_speed(self):
self.can.sendData(0x400, [self.status_cmd, self.alive])
self.can.sendData(0x401, self.speed_cmd) # drive control data drive, steering
self.alive = 128 - self.alive
def send_LED(self):
self.can.sendData(0x410, [1, 2, 3, 4, 5, 6, 7, self.led])
if self.time - self.led_time > 0.5:
self.led = 1 - self.led
self.led_time = self.time
def set_raw_speed(self, fwd_rev_drive, left_right):
print('set_raw_speed', fwd_rev_drive, left_right)
self.speed_cmd = [fwd_rev_drive, left_right]
def update(self):
while True:
packet = self.can.readPacket()
self.check_modules(packet)
self.update_status_word(packet)
for (name,e) in self.extensions:
e(self, packet[0], packet[1])
# make sure that all updates get also termination SYNC (0x80)
# there is no SYNC on Spider3 yet, use 0x200 for now
if packet[0] == 0x200:
break
# send data related to other sources
for (id,fce) in self.data_sources:
data = fce()
if data != None:
for (name,e) in self.extensions:
e(self, id, data)
self.time += 1.0/self.UPDATE_TIME_FREQUENCY
self.send_speed()
self.send_LED()
def stop(self):
"send stop command and make sure robot really stops"
self.set_raw_speed(0, 0)
for i in range(10):
self.update()
class JohnDeere(object):
UPDATE_TIME_FREQUENCY = 5.0 #20.0 # Hz
def __init__(self, can=None):
if can is None:
self.can = CAN()
self.can.resetModules()
else:
self.can = can
self.canproxy = CANProxy(self.can)
self.time = 0.0
self.gas = None
self.steering_angle = 0.0 # in the future None and auto-detect
self.buttonGo = None
self.desired_speed = 0.0
self.filteredGas = None
self.compass = None
self.compassRaw = None
self.compassAcc = None
self.compassAccRaw = None
self.drop_ball = False # TODO move to ro.py only
self.extensions = []
self.data_sources = []
self.modulesForRestart = []
self.can.sendOperationMode()
def __del__(self):
self.can.sendPreoperationMode()
# TODO to move out, can.py??
def check_modules(self, packet):
# test if all modules are in Operation mode, if not restart them
id, data = packet
if id & 0xFF0 == 0x700: # heart beat message
moduleId = id & 0xF
assert (len(data) == 1)
if data[0] != 5:
self.can.printPacket(id, data)
if not moduleId in self.modulesForRestart:
self.modulesForRestart.append(moduleId)
print "RESET", moduleId
self.can.sendData(0, [129, moduleId])
# if moduleId in [0x01, 0x02]:
# if (0x180 | moduleId) in self.encData:
# # The encoder information is invalid during a reset
# del self.encData[0x180 | moduleId]
elif data[0] == 127: # restarted and in preoperation
print "SWITCH TO OPERATION", moduleId
self.can.sendData(0, [1, moduleId])
elif moduleId in self.modulesForRestart:
print "RUNNING", moduleId
self.modulesForRestart.remove(moduleId)
def register_data_source(self, name, function, extension=None):
self.data_sources.append((name, function))
if extension is not None:
self.extensions.append((name, extension))
def update_gas_status(self, (id, data)):
if id == 0x281:
assert (len(data) >= 8)
self.gas = data[1] * 256 + data[0]
self.buttonGo = (data[-1] > 64)
if self.filteredGas is None:
self.filteredGas = self.gas
else:
self.filteredGas = SCALE_NEW * self.gas + (
1.0 - SCALE_NEW) * self.filteredGas
class JohnDeere(object):
UPDATE_TIME_FREQUENCY = 5.0 #20.0 # Hz
def __init__(self, can=None):
if can is None:
self.can = CAN()
self.can.resetModules()
else:
self.can = can
self.canproxy = CANProxy(self.can)
self.time = 0.0
self.buttonGo = None # TODO currently not available (!)
self.desired_speed = 0.0
self.drop_ball = False # TODO move to ro.py only
self.extensions = []
self.data_sources = []
self.modulesForRestart = []
self.can.sendOperationMode()
def __del__(self):
self.can.sendPreoperationMode()
# TODO to move out, can.py??
def check_modules(self, packet):
# test if all modules are in Operation mode, if not restart them
id, data = packet
if id & 0xFF0 == 0x700: # heart beat message
moduleId = id & 0xF
assert( len(data) == 1 )
if data[0] != 5:
self.can.printPacket( id, data )
if not moduleId in self.modulesForRestart:
self.modulesForRestart.append( moduleId )
print "RESET", moduleId
self.can.sendData( 0, [129,moduleId] )
# if moduleId in [0x01, 0x02]:
# if (0x180 | moduleId) in self.encData:
# # The encoder information is invalid during a reset
# del self.encData[0x180 | moduleId]
elif data[0] == 127: # restarted and in preoperation
print "SWITCH TO OPERATION", moduleId
self.can.sendData( 0, [1,moduleId] )
elif moduleId in self.modulesForRestart:
print "RUNNING", moduleId
self.modulesForRestart.remove(moduleId)
def register_data_source(self, name, function, extension=None):
self.data_sources.append((name, function))
if extension is not None:
self.extensions.append((name, extension))
def send_ball_dispenser(self):
if self.drop_ball:
cmd = 127
else:
cmd = 128
self.can.sendData(0x37F, [0, cmd, 0, 0, 0, 0, 0, 0])
def wait(self, duration):
start_time = self.time
while self.time - start_time < duration:
self.update()
def update(self):
while True:
packet = self.can.readPacket()
self.canproxy.update(packet)
# self.update_emergency_stop(packet)
self.check_modules(packet)
for (name,e) in self.extensions:
e(self, packet[0], packet[1])
# make sure that all updates get also termination SYNC (0x80)
if packet[0] == 0x80:
break
# send data related to other sources
for (id,fce) in self.data_sources:
data = fce()
if data != None:
for (name,e) in self.extensions:
e(self, id, data)
self.time += 1.0/self.UPDATE_TIME_FREQUENCY
self.canproxy.set_time(self.time)
self.canproxy.send_speed()
self.send_ball_dispenser()
def stop(self):
"send stop command and make sure robot really stops"
self.desired_speed = 0.0
self.canproxy.stop()
for i in xrange(10):
self.update()
class Spider3(object):
UPDATE_TIME_FREQUENCY = 20.0 # Hz
# status word
EMERGENCY_STOP = 0x0001
DEVICE_READY = 0x0002 # also releaseState
MASK_HEARTBEAT = 0x8000
def __init__(self, can=None):
if can is None:
self.can = CAN()
# self.can.resetModules()
else:
self.can = can
self.time = 0.0
self.status_word = None # not defined yet
self.wheel_angles = [None] * 4 # original CAN data
self.drive_status = [None] * 4
self.zero_steering = None # zero position of all 4 wheels
self.alive = 0 # togle with 128
self.speed_cmd = [0, 0]
self.status_cmd = 3
self.extensions = []
self.data_sources = []
self.threads = []
self.modulesForRestart = []
self.led = 0
self.led_time = 0.0
# self.can.sendOperationMode()
def __del__(self):
pass
# self.can.sendPreoperationMode()
# TODO to move out, can.py??
def check_modules(self, packet):
# test if all modules are in Operation mode, if not restart them
id, data = packet
if id & 0xFF0 == 0x700: # heart beat message
moduleId = id & 0xF
assert (len(data) == 1)
if data[0] != 5:
self.can.printPacket(id, data)
if not moduleId in self.modulesForRestart:
self.modulesForRestart.append(moduleId)
print("RESET", moduleId)
self.can.sendData(0, [129, moduleId])
# if moduleId in [0x01, 0x02]:
# if (0x180 | moduleId) in self.encData:
# # The encoder information is invalid during a reset
# del self.encData[0x180 | moduleId]
elif data[0] == 127: # restarted and in preoperation
print("SWITCH TO OPERATION", moduleId)
self.can.sendData(0, [1, moduleId])
elif moduleId in self.modulesForRestart:
print("RUNNING", moduleId)
self.modulesForRestart.remove(moduleId)
def register_data_source(self, name, function, extension=None):
self.data_sources.append((name, function))
if extension is not None:
self.extensions.append((name, extension))
def wait(self, duration):
start_time = self.time
while self.time - start_time < duration:
self.update()
def update_status_word(self, packet):
msg_id, data = packet
if msg_id == 0x200:
assert len(data) == 2, packet
prev = self.status_word
self.status_word = struct.unpack('H', bytearray(data))[0]
if prev is not None:
diff = prev ^ self.status_word
is_alive = diff & self.MASK_HEARTBEAT
if is_alive == 0:
# make sure nothing is missing (for now)
print("\n!!!Spider is DEAD!!! (or data are missing)\n")
if (diff & 0x7FFF) != 0:
print("Status %d -> %d" %
(prev & 0x7FFF, self.status_word & 0x7FFF))
elif msg_id == 0x201:
assert len(data) == 8, packet
prev = self.wheel_angles
self.wheel_angles = struct.unpack('HHHH', bytearray(data))
if prev != self.wheel_angles and self.zero_steering is not None:
# note, that self.zero_steering has 200ms update rate,
# i.e. it does not have to be immediately defined
print('Wheels: %.2f' % self.time, [
fix_range(a - b)
for a, b in zip(self.wheel_angles, self.zero_steering)
])
elif msg_id == 0x202:
assert len(data) == 8, packet # drive status + zero positions
self.drive_status = struct.unpack('HHHH', bytearray(data))
drive = self.drive_status[0] - self.drive_status[
2], self.drive_status[3] - self.drive_status[1]
if abs(drive[0]) + abs(drive[1]) > 10:
print('Drive:', drive, abs(drive[0]) + abs(drive[1]))
elif msg_id == 0x203:
assert len(data) == 8, packet
prev = self.zero_steering
self.zero_steering = struct.unpack('HHHH', bytearray(data))
# make sure that calibration did not change during program run
assert prev is None or prev == self.zero_steering, (
prev, self.zero_steering)
def send_speed(self):
self.can.sendData(0x400, [self.status_cmd, self.alive])
self.can.sendData(0x401,
self.speed_cmd) # drive control data drive, steering
self.alive = 128 - self.alive
def send_LED(self):
self.can.sendData(0x410, [1, 2, 3, 4, 5, 6, 7, self.led])
if self.time - self.led_time > 0.5:
self.led = 1 - self.led
self.led_time = self.time
def set_raw_speed(self, fwd_rev_drive, left_right):
print('set_raw_speed', fwd_rev_drive, left_right)
self.speed_cmd = [fwd_rev_drive, left_right]
def update(self):
while True:
packet = self.can.readPacket()
self.check_modules(packet)
self.update_status_word(packet)
for (name, e) in self.extensions:
e(self, packet[0], packet[1])
# make sure that all updates get also termination SYNC (0x80)
# there is no SYNC on Spider3 yet, use 0x200 for now
if packet[0] == 0x200:
break
# send data related to other sources
for (id, fce) in self.data_sources:
data = fce()
if data != None:
for (name, e) in self.extensions:
e(self, id, data)
self.time += 1.0 / self.UPDATE_TIME_FREQUENCY
self.send_speed()
self.send_LED()
def stop(self):
"send stop command and make sure robot really stops"
self.set_raw_speed(0, 0)
for i in range(10):
self.update()
class JohnDeere(object):
UPDATE_TIME_FREQUENCY = 5.0 #20.0 # Hz
def __init__(self, can=None, localization=None, config=None):
if can is None:
self.can = CAN()
self.can.resetModules()
else:
self.can = can
self.canproxy = CANProxy(self.can)
self.localization = localization
if config is not None:
global TURN_ANGLE_OFFSET
TURN_ANGLE_OFFSET = config.get('TURN_ANGLE_OFFSET', TURN_ANGLE_OFFSET)
self.time = 0.0
self.buttonGo = None # TODO currently not available (!)
self.drop_ball = False # TODO move to ro.py only
self.extensions = []
self.data_sources = []
self.threads = []
self.modulesForRestart = []
self.can.sendOperationMode()
def __del__(self):
self.can.sendPreoperationMode()
# TODO to move out, can.py??
def check_modules(self, packet):
# test if all modules are in Operation mode, if not restart them
id, data = packet
if id & 0xFF0 == 0x700: # heart beat message
moduleId = id & 0xF
assert( len(data) == 1 )
if data[0] != 5:
self.can.printPacket( id, data )
if not moduleId in self.modulesForRestart:
self.modulesForRestart.append( moduleId )
print "RESET", moduleId
self.can.sendData( 0, [129,moduleId] )
# if moduleId in [0x01, 0x02]:
# if (0x180 | moduleId) in self.encData:
# # The encoder information is invalid during a reset
# del self.encData[0x180 | moduleId]
elif data[0] == 127: # restarted and in preoperation
print "SWITCH TO OPERATION", moduleId
self.can.sendData( 0, [1,moduleId] )
elif moduleId in self.modulesForRestart:
print "RUNNING", moduleId
self.modulesForRestart.remove(moduleId)
def register_data_source(self, name, function, extension=None):
self.data_sources.append((name, function))
if extension is not None:
self.extensions.append((name, extension))
def send_ball_dispenser(self):
if self.drop_ball:
cmd = 127
else:
cmd = 128
self.can.sendData(0x37F, [0, cmd, 0, 0, 0, 0, 0, 0])
def wait(self, duration):
start_time = self.time
while self.time - start_time < duration:
self.update()
def update(self):
prev_enc = self.canproxy.dist_left_raw, self.canproxy.dist_right_raw
while True:
packet = self.can.readPacket()
self.canproxy.update(packet)
# self.update_emergency_stop(packet)
self.check_modules(packet)
for (name,e) in self.extensions:
e(self, packet[0], packet[1])
# make sure that all updates get also termination SYNC (0x80)
if packet[0] == 0x80:
break
if (self.localization is not None and
prev_enc[0] is not None and prev_enc[1] is not None and
self.canproxy.wheel_angle_raw is not None):
dist_left = ENC_SCALE * (self.canproxy.dist_left_raw - prev_enc[0])
dist_right = ENC_SCALE * (self.canproxy.dist_right_raw - prev_enc[1])
angle_left = self.canproxy.wheel_angle_raw * TURN_SCALE + TURN_ANGLE_OFFSET
self.localization.update_odometry(angle_left, dist_left, dist_right)
# send data related to other sources
for (id,fce) in self.data_sources:
data = fce()
if data != None:
for (name,e) in self.extensions:
e(self, id, data)
self.time += 1.0/self.UPDATE_TIME_FREQUENCY
self.canproxy.set_time(self.time)
self.canproxy.send_speed()
self.canproxy.send_LEDs()
self.send_ball_dispenser()
def set_desired_speed(self, speed):
"""set desired speed in meters per second.
speed = None ... disable speed control
... can be called in every cycle without side-effects
"""
self.canproxy.set_desired_speed_raw(int(speed/ENC_SCALE))
def set_desired_steering(self, angle):
"""set desired steering angle of left wheel"""
# angle = sensors['steering'] * TURN_SCALE + TURN_ANGLE_OFFSET # radians
raw = (angle - TURN_ANGLE_OFFSET)/TURN_SCALE
self.canproxy.set_turn_raw(int(raw))
def stop(self):
"send stop command and make sure robot really stops"
self.canproxy.stop()
for i in xrange(10):
self.update()
class JohnDeere(object):
UPDATE_TIME_FREQUENCY = 5.0 #20.0 # Hz
def __init__(self, can=None, localization=None, config=None):
if can is None:
self.can = CAN()
self.can.resetModules()
else:
self.can = can
self.canproxy = CANProxy(self.can)
self.localization = localization
if config is not None:
global TURN_ANGLE_OFFSET
TURN_ANGLE_OFFSET = config.get('TURN_ANGLE_OFFSET', TURN_ANGLE_OFFSET)
self.time = 0.0
self.buttonGo = None # TODO currently not available (!)
self.drop_ball = False # TODO move to ro.py only
self.extensions = []
self.data_sources = []
self.threads = []
self.modulesForRestart = []
self.can.sendOperationMode()
def __del__(self):
self.can.sendPreoperationMode()
# TODO to move out, can.py??
def check_modules(self, packet):
# test if all modules are in Operation mode, if not restart them
id, data = packet
if id & 0xFF0 == 0x700: # heart beat message
moduleId = id & 0xF
assert( len(data) == 1 )
if data[0] != 5:
self.can.printPacket( id, data )
if not moduleId in self.modulesForRestart:
self.modulesForRestart.append( moduleId )
print("RESET", moduleId)
self.can.sendData( 0, [129,moduleId] )
# if moduleId in [0x01, 0x02]:
# if (0x180 | moduleId) in self.encData:
# # The encoder information is invalid during a reset
# del self.encData[0x180 | moduleId]
elif data[0] == 127: # restarted and in preoperation
print("SWITCH TO OPERATION", moduleId)
self.can.sendData( 0, [1,moduleId] )
elif moduleId in self.modulesForRestart:
print("RUNNING", moduleId)
self.modulesForRestart.remove(moduleId)
def register_data_source(self, name, function, extension=None):
self.data_sources.append((name, function))
if extension is not None:
self.extensions.append((name, extension))
def send_ball_dispenser(self):
if self.drop_ball:
cmd = 127
else:
cmd = 128
self.can.sendData(0x37F, [0, cmd, 0, 0, 0, 0, 0, 0])
def wait(self, duration):
start_time = self.time
while self.time - start_time < duration:
self.update()
def update(self):
prev_enc = self.canproxy.dist_left_raw, self.canproxy.dist_right_raw
while True:
packet = self.can.readPacket()
self.canproxy.update(packet)
# self.update_emergency_stop(packet)
self.check_modules(packet)
for (name,e) in self.extensions:
e(self, packet[0], packet[1])
# make sure that all updates get also termination SYNC (0x80)
if packet[0] == 0x80:
break
if (self.localization is not None and
prev_enc[0] is not None and prev_enc[1] is not None and
self.canproxy.wheel_angle_raw is not None):
dist_left = ENC_SCALE * (self.canproxy.dist_left_raw - prev_enc[0])
dist_right = ENC_SCALE * (self.canproxy.dist_right_raw - prev_enc[1])
angle_left = self.canproxy.wheel_angle_raw * TURN_SCALE + TURN_ANGLE_OFFSET
self.localization.update_odometry(angle_left, dist_left, dist_right)
# send data related to other sources
for (id,fce) in self.data_sources:
data = fce()
if data != None:
for (name,e) in self.extensions:
e(self, id, data)
self.time += 1.0/self.UPDATE_TIME_FREQUENCY
self.canproxy.set_time(self.time)
self.canproxy.send_speed()
self.canproxy.send_LEDs()
self.send_ball_dispenser()
def set_desired_speed(self, speed):
"""set desired speed in meters per second.
speed = None ... disable speed control
... can be called in every cycle without side-effects
"""
self.canproxy.set_desired_speed_raw(int(speed/ENC_SCALE))
def set_desired_steering(self, angle):
"""set desired steering angle of left wheel"""
# angle = sensors['steering'] * TURN_SCALE + TURN_ANGLE_OFFSET # radians
raw = (angle - TURN_ANGLE_OFFSET)/TURN_SCALE
self.canproxy.set_turn_raw(int(raw))
def stop(self):
"send stop command and make sure robot really stops"
self.canproxy.stop()
for i in range(10):
self.update()