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 __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 __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 __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()
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 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()
