class SLAM:
pass
def __init__(self):
self.particle_num = 100
self.particles = [ Particle() for _ in range(self.particle_num) ]
self.odom = Subscriber()
self.lidar = Subscriber()
def loop(self):
pass
def process_odometry(self):
movement = self.odom.get()
delta_xy = movement["xy"]/dt
delta_r = movement["angle"]/dt
for particle in self.particles:
particle.move(delta_xy, delta_r)
angle_var = np.degrees(5) * np.random.randn()
xy_var = [0.01 * np.random.randn(), 0]
particle.move(xy_var, angle_var)
# particle.move()
def process_lidar(self):
scan = self.lidar.get()
landmarks = find_landmarks(scan)
for particle in self.particles:
particles.sensor_update(landmarks)
#resample particles
@staticmethod
def find_landmarks(scan) -> List[Bearing]:
xs = []
ys = []
img = np.zeros((500,500))
for _, angle, dist in scan:
a = math.radians(angle)
x = math.sin(a) * dist
y = math.cos(a) * dist
img[x,y] = 1
img
class Arm:
def __init__(self):
self.target_vel = Subscriber(8410)
self.motor_names = ["spinner"]
self.pwm_names = ["null"]
self.CPR = {'spinner':1294}
self.oneShot = 0
self.rc = RoboClaw(find_serial_port(), names = self.motor_names + self.pwm_names,\
addresses = [128])
self.pos = 0
while 1:
start_time = time.time()
self.update()
while (time.time() - start_time) < 0.1:
pass
def condition_input(self,target):
target['x'] = - target['x']
target['yaw'] = 0.01* target['yaw']
# rotates command frame to end effector orientation
angle = self.xyz_positions['yaw']
x = target['x']
y = target['y']
target['x'] = x*math.cos(angle) - y*math.sin(angle)
target['y'] = x*math.sin(angle) + y*math.cos(angle)
return target
def update(self):
try:
print()
print("new iteration")
target = self.target_vel.get()
# TODO: This shouldn't be necessary, how to fix in UDPComms?
target = {bytes(key): value for key, value in target.iteritems()}
print(target)
target_f = target
if(target_f['grip'] == 1 and self.oneShot == 0):
self.pos = self.pos + 1
self.oneShot = 1
elif(target_f['grip'] == -1 and self.oneShot == 0):
self.pos = self.pos - 1
self.oneShot = 1
if(target_f['grip'] == 0):
self.oneShot = 0
if target_f["reset"]:
print ("RESETTING!!!")
self.rc.set_encoder('spinner',0)
self.rc.drive_position('spinner',int(self.pos*self.CPR['spinner']))
except timeout:
print ("No commands")
class Arm:
def __init__(self):
self.target_vel = Subscriber(8410)
self.motor_names = ["spinner"]
self.pwm_names = ["led"]
self.CPR = {'spinner':1294}
self.oneShot = 0
self.rc = RoboClaw(find_serial_port(), names = self.motor_names + self.pwm_names,\
addresses = [128])
self.pos = 0
self.offset = 0
while 1:
start_time = time.time()
self.update()
while (time.time() - start_time) < 0.1:
pass
def update(self):
try:
print()
print("new iteration")
target = self.target_vel.get()
# TODO: This shouldn't be necessary, how to fix in UDPComms?
target = {bytes(key): value for key, value in target.iteritems()}
print(target)
target_f = target
if(target_f['grip'] == -1):
self.rc.drive_duty("led", -30000)
else:
self.rc.drive_duty("led", 0)
if math.fabs(target_f['yaw']) > 0.1:
self.offset += 100*target_f['yaw']
if(target_f['roll'] == 1 and self.oneShot == 0):
self.pos = self.pos + 1
self.oneShot = 1
elif(target_f['roll'] == -1 and self.oneShot == 0):
self.pos = self.pos - 1
self.oneShot = 1
if(target_f['roll'] == 0):
self.oneShot = 0
if target_f["reset"]:
print ("RESETTING!!!")
self.rc.set_encoder('spinner',0)
self.pos = 0
self.offset = 0
self.rc.drive_position('spinner',int(self.pos*self.CPR['spinner'] + self.offset))
except timeout:
print ("No commands")
class Trajectory_Reciever:
def __init__(self, port=8830):
#port is definetly subject to change
self.pi_subscriber = Subscriber(port)
def get_trajectory(self):
#NOTE: form of messaage: {x_vel: 5, y_vel: 3}
try:
msg = self.pi_subscriber.get()
print ("recieved message: ", msg)
return msg['ly'] * 0.5, msg['lx'] * -0.24
except:
print ("error getting the message")
return 0, 0
class Arm:
def __init__(self):
self.target_vel = Subscriber(8410)
self.pwm_names = ["z", "z_clone"]
self.rc = RoboClaw(find_serial_port(),
names=self.pwm_names,
addresses=[131])
try:
while 1:
start_time = time.time()
self.update()
while (time.time() - start_time) < 0.1:
pass
except KeyboardInterrupt:
print('driving z at', 0)
self.rc.drive_duty('z', 0)
raise
except:
print('driving z at', 0)
self.rc.drive_duty('z', 0)
raise
def update(self):
print()
print("new iteration")
try:
target = self.target_vel.get()
# TODO: This shouldn't be necessary, how to fix in UDPComms?
target = {bytes(key): value for key, value in target.iteritems()}
# target_f = self.condition_input(target)
except timeout:
print "TIMEOUT No commands recived"
print('driving z at', 0)
self.rc.drive_duty('z', 0)
except:
print('driving z at', 0)
self.rc.drive_duty('z', 0)
raise
else:
print('Driving z at', int(19000 * target['z']))
self.rc.drive_duty('z', int(-19000 * target['z']))
def main(argv):
idx_camera = int(argv[0])
offset_degrees = float(argv[1])
reference = Subscriber(9010)
detection_results = Publisher(902 + idx_camera)
camera = picamera.PiCamera(sensor_mode=2,
resolution=capture_res,
framerate=10)
cam_params = {}
while True:
try:
cam_params = reference.get()
print(cam_params)
except UDPComms.timeout:
if "range_hue" not in cam_params:
continue
camera.shutter_speed = cam_params["shutter"]
camera.iso = cam_params["iso"]
image = np.empty((capture_res[1] * capture_res[0] * 3), dtype=np.uint8)
camera.capture(image, 'bgr')
image = image.reshape((capture_res[1], capture_res[0], 3))
hsv_ranges = (cam_params["range_hue"], cam_params["range_sat"],
cam_params["range_val"])
heading, distance = find_ball_direct(image, hsv_ranges, offset_degrees)
if (distance > 0):
result = {"heading": heading, "distance": distance}
detection_results.send(result)
print("Found ball!")
print(result)
class Control(object):
STATES = ['off', 'rest', 'meander', 'goto', 'avoid']
SCREEN_MID_X = 150
def __init__(self, target='tennis_ball'):
self.timer = RepeatTimer(1 / MESSAGE_RATE, self._step)
self.control_state = ControllerState()
self.pos = PositionTracker(self.control_state)
self.obj_sensors = ObjectSensors()
self.active = False
self.walking = False
self.user_stop = False
self.cmd_pub = Publisher(CMD_PUB_PORT)
self.cmd_sub = Subscriber(CMD_SUB_PORT)
self.cv_sub = Subscriber(CV_SUB_PORT)
self.joystick = Joystick()
self.joystick.led_color(**PUPPER_COLOR)
self.state = 'off'
self.target = target
self.current_target = None
self.pusher_client = PusherClient()
self._last_sensor_data = (None, None, None)
def move_forward(self, vel=ControllerState.LEFT_ANALOG_Y_MAX):
self.control_state.left_analog_y = vel
def move_backward(self, vel=-ControllerState.LEFT_ANALOG_Y_MAX):
self.control_state.left_analog_y = vel
def move_stop(self):
self.control_state.left_analog_y = 0
def turn_right(self, rate=ControllerState.RIGHT_ANALOG_X_MAX):
self.control_state.right_analog_x = rate
def turn_left(self, rate=-ControllerState.RIGHT_ANALOG_X_MAX):
self.control_state.right_analog_x = rate
def turn_stop(self):
self.control_state.right_analog_x = 0
def start_walk(self):
if self.walking or not self.active:
return
self.reset()
self.control_state.r1 = True
self.toggle_cmd()
self.walking = True
self.control_state.walking = True
self.reset()
def activate(self):
if self.active:
return
self.reset()
self.control_state.l1 = True
self.toggle_cmd()
self.reset()
self.active = True
self.state = 'rest'
self.walking = False
self.control_state.walking = False
if not self.pos.running:
self.pos.run()
def stop_walk(self):
if not self.walking:
return
self.reset()
self.control_state.r1 = True
self.toggle_cmd()
self.walking = False
self.control_state.walking = False
self.reset()
self.state = 'rest'
def reset(self):
self.control_state.reset()
def run_loop(self):
self.timer.start()
def stop_loop(self):
self.timer.cancel()
self.reset()
self.stop_walk()
self.pos.stop()
self.active = False
self.send_cmd()
self.user_stop = True
def toggle_cmd(self):
# For toggle commands, they should be sent several times to ensure they
# are put into effect
for _ in range(3):
self.send_cmd()
time.sleep(1 / MESSAGE_RATE)
def send_cmd(self):
cmd = self.control_state.get_state()
self.cmd_pub.send(cmd)
try:
msg = self.cmd_sub.get()
self.joystick.led_color(**msg["ps4_color"])
except timeout:
pass
def get_sensor_data(self):
try:
obj = self.obj_sensors.read()
pos = self.pos.data
try:
cv = self.cv_sub.get()
except timeout:
cv = []
except:
obj, pos, cv = self._last_sensor_data
self._last_sensor_data = (obj, pos, cv)
return obj, pos, cv
def _step(self):
js_msg = self.joystick.get_input()
# Force stop moving
if js_msg['button_l2']:
self.user_stop = True
self.stop_walk()
return
# User activate
if js_msg['button_l1']:
self.user_stop = False
self.activate()
return
if self.user_stop or not self.active:
self.reset()
self.send_cmd()
self.send_pusher_message()
return
if not self.walking:
self.start_walk()
return
self.update_behavior()
self.send_cmd()
self.send_pusher_message()
def update_behavior(self):
obj, pos, cv = self.get_sensor_data()
if not any(obj.values()):
self.turn_stop()
if any(obj.values()):
# If object, dodge
self.state = 'avoid'
self.dodge(obj)
elif any([x['bbox_label'] == self.target for x in cv]):
# if target, chase
self.state = 'goto'
self.goto(cv)
else:
# if nothing, wander
self.state = 'meander'
self.meander()
def dodge(self, obj):
'''Takes the object sensor data and adjusts the command to avoid any
objects
'''
if obj['left'] and obj['center']:
self.move_stop()
self.turn_right()
elif (obj['right'] and obj['center']) or obj['center']:
self.move_stop()
self.turn_left()
elif obj['left']:
self.move_forward(vel=ControllerState.LEFT_ANALOG_Y_MAX / 2)
self.turn_right()
elif obj['right']:
self.move_forward(vel=ControllerState.LEFT_ANALOG_Y_MAX / 2)
self.turn_left()
elif not any(obj.values()):
self.turn_stop()
def meander(self):
'''moves forward and makes slight turns left and right to search for a
target -- eventually, for now just move forward
'''
self.current_target = None
self.move_forward()
def goto(self, cv):
'''takes a list of bounding boxes, picks the most likely ball and moves
toward it
'''
tmp = [x for x in cv if x['bbox_label'] == self.target]
if len(tmp) == 0:
self.meander()
conf = np.array([x['bbox_confidence'] for x in tmp])
idx = np.argmax(conf)
best = tmp[idx]
center_x = best['bbox_x'] + best['bbox_w'] / 2
if center_x < self.SCREEN_MID_X:
self.turn_left()
elif center_x > self.SCREEN_MID_X:
self.turn_right()
self.move_forward()
self.current_target = best
def send_pusher_message(self):
obj, pos, cv = self.get_sensor_data()
bbox = self.current_target
timestamp = time.time()
if bbox is None:
bbox = {
'bbox_x': None,
'bbox_y': None,
'bbox_h': None,
'bbox_w': None,
'bbox_label': None,
'bbox_confidence': None
}
message = {
'time': timestamp,
'x_pos': pos['x'],
'y_pos': pos['y'],
'x_vel': pos['x_vel'],
'y_vel': pos['y_vel'],
'x_acc': pos['x_acc'],
'y_acc': pos['y_acc'],
'yaw': pos['yaw'],
'yaw_rate': pos['yaw_rate'],
'left_sensor': obj['left'],
'center_sensor': obj['center'],
'right_sensor': obj['right'],
'state': self.state,
**bbox
}
self.pusher_client.send(message)
tilt_angle = 0
def clamp(x, ma, mi):
if x > ma:
return ma
elif x < mi:
return mi
else:
return x
while True:
time.sleep(0.1)
changed = False
try:
msg = sub.get()
pan_angle = clamp(pan_angle + msg['pan'], 90, -90)
tilt_angle = clamp(tilt_angle + msg['tilt'], 90, -24)
if (msg['pan'] != 0) or (msg['tilt'] != 0):
changed = True
except timeout:
pass
if changed:
pan.ChangeDutyCycle(7.5 + pan_angle * 5.5 / 90)
tilt.ChangeDutyCycle(8.5 - tilt_angle * 5.5 / 90)
else:
pan.ChangeDutyCycle(0)
tilt.ChangeDutyCycle(0)
class GPSPanel:
def __init__(self):
self.root = tk.Tk()
self.FONT_HEADER = tkFont.Font(family="Helvetica",
size=14,
weight=tkFont.BOLD)
### LOAD MAPS
self.load_maps()
self.map = self.maps[self.maps.keys()[0]]
# self.map = self.maps['science_overview']
## UDPComms
self.gps = Subscriber(8280, timeout=2)
self.rover_pt = None
self.gyro = Subscriber(8220, timeout=1)
self.arrow = None
self.gps_base = Subscriber(8290, timeout=2)
self.base_pt = None
# publishes the point the robot should be driving to
self.auto_control = {
u"waypoints": [],
u"command": u"off",
u"end_mode": u"none"
}
self.auto_control_pub = Publisher(8310)
self.pub_pt = None
# obstacles from the interface, displayed pink trasparent
self.obstacles = []
self.obstacles_pub = Publisher(9999)
# obstacles from the robots sensors, displayed red tranparent.
self.auto_obstacle_sub = Publisher(9999)
# the path the autonomous module has chosen, drawn as blue lines
self.path_sub = Subscriber(9999)
self.compass_offset_pub = Publisher(8210)
# none, waypoint, obstacle, obstacle_radius
self.mouse_mode = "none"
self.last_mouse_click = (0, 0)
self.temp_obstace = None
### AUTONOMY MODE
self.auto_mode_dis = tk.StringVar()
left_row = 0
tk.Label(self.root,
textvariable=self.auto_mode_dis,
font=("Courier", 44)).grid(row=left_row, column=0)
self.auto_mode_dis.set("")
left_row += 1
ttk.Separator(self.root, orient=VERTICAL).grid(row=left_row,
column=0,
sticky='ew')
### EXISTING WAYPOINT ACTIONS AND LISTBOX
left_row += 1
# frame for holding all components associated with point library
listbox_frame = tk.Frame(self.root)
listbox_frame.grid(row=left_row, column=0)
self.display_curr_waypoint_frame(listbox_frame)
left_row += 1
ttk.Separator(self.root, orient=VERTICAL).grid(row=left_row,
column=0,
sticky='ew')
# stores tuples of (title, point):
# title is the string that's displayed in listbox
# point is a point object instance
self.pointLibrary = []
# what to call the next added point
self.pointIncrement = 0
### CREATE WAYPOINT ACTIONS
left_row += 1
create_frame = tk.Frame(self.root)
create_frame.grid(row=left_row, column=0)
self.display_create_waypoint_frame(create_frame)
self.root.bind("<Escape>", lambda x: self.change_mouse_mode('none'))
left_row += 1
ttk.Separator(self.root, orient=VERTICAL).grid(row=left_row,
column=0,
sticky='ew')
### AUTONOMOUS MODE ACTIONS
auto_frame = tk.Frame(self.root)
left_row += 1
auto_frame.grid(row=left_row, column=0)
self.display_auto_actions_frame(auto_frame)
left_row += 1
ttk.Separator(self.root, orient=VERTICAL).grid(row=left_row,
column=0,
sticky='ew')
self.root.bind("<space>", lambda x: self.change_auto_mode('off'))
### CHANGE MAP ACTIONS
map_frame = tk.Frame(self.root)
left_row += 1
map_frame.grid(row=left_row, column=0)
self.display_change_map_actions_frame(map_frame)
top_col = 1
ttk.Separator(self.root, orient=VERTICAL).grid(row=0,
column=top_col,
sticky='ns')
### MOUSE LOCATION INFO
mouse_info_frame = tk.Frame(self.root)
top_col += 1
mouse_info_frame.grid(row=0, column=top_col)
self.display_mouse_info_frame(mouse_info_frame)
top_col += 1
ttk.Separator(self.root, orient=VERTICAL).grid(row=0,
column=top_col,
sticky='ns')
### ROVER LOCATION INFO
location_frame = tk.Frame(self.root)
top_col += 1
location_frame.grid(row=0, column=top_col)
self.display_location_info_frame(location_frame)
top_col += 1
ttk.Separator(self.root, orient=VERTICAL).grid(row=0,
column=top_col,
sticky='ns')
### COMPASS OFFSET
compass_frame = tk.Frame(self.root)
top_col += 1
compass_frame.grid(row=0, column=top_col)
self.display_compass_offset_frame(compass_frame)
### canvas display
self.canvas = tk.Canvas(self.root,
width=self.map.size[1],
height=self.map.size[0])
self.canvas.grid(row=1, column=1, rowspan=8, columnspan=8)
self.canvas_img = self.canvas.create_image(0,
0,
image=self.map.image,
anchor=tk.NW)
# mouse callbacks
self.canvas.bind("<Button-1>", self.mouse_click_callback)
self.canvas.bind("<Motion>", self.mouse_motion_callback)
self.root.after(50, self.update)
self.root.mainloop()
def load_maps(self):
self.maps = {}
with open('maps/{}/info.csv'.format(MAP_AREA)) as maps_csv:
csv_reader = csv.DictReader(maps_csv, delimiter=',')
for row in csv_reader:
filename = row['filename']
name = filename[:filename.find('.')]
size = (float(row['height']), float(row['width']))
top_left = (float(row['top_left_lat']),
float(row['top_left_lon']))
bottom_right = (float(row['bottom_right_lat']),
float(row['bottom_right_lon']))
self.maps[name] = Map('maps/{}/{}'.format(MAP_AREA, filename), \
size=size, \
top_left=top_left, \
bottom_right=bottom_right)
'''
begin: GUI LAYOUT FUNCTIONS
'''
def display_curr_waypoint_frame(self, frame):
# title
tk.Label(frame, text='EDIT WAYPOINTS',
font=self.FONT_HEADER).grid(row=0, column=0, columnspan=3)
# actions on listbox of points
tk.Button(frame, text='Delete', command=lambda: self.delete_selected_waypoint() ) \
.grid(row=1, column=0)
# reorder: move up
tk.Button(frame, text=u'\u2B06', command=lambda: self.reorder_selected_waypoint(direction=-1) ) \
.grid(row=1, column=1)
# reorder: move down
tk.Button(frame, text=u'\u2B07', command=lambda: self.reorder_selected_waypoint(direction=1) ) \
.grid(row=1, column=2)
# listbox displaying points
self.listbox = tk.Listbox(frame)
self.listbox.grid(row=2, column=0, columnspan=3)
self.listbox.bind('<FocusOut>',
lambda: self.listbox.selection_clear(0, END))
self.end_with_tennis_search = IntVar()
tk.Checkbutton(frame, text="End with tennis ball search", variable=self.end_with_tennis_search) \
.grid(row=3, column=0, columnspan=3)
def display_create_waypoint_frame(self, frame):
# title
tk.Label(frame, text='ADD TO MAP',
font=self.FONT_HEADER).grid(row=0, column=0, columnspan=2)
### click to add waypoint functions
create_click_frame = tk.Frame(frame)
create_click_frame.grid(row=4, column=0)
tk.Label(create_click_frame, text='Click to add:').grid(row=0,
column=0,
columnspan=2)
tk.Button(create_click_frame,
text='Waypoint',
command=lambda: self.change_mouse_mode('waypoint')).grid(
row=1, column=0)
tk.Button(create_click_frame,
text='Rover',
command=lambda: self.new_waypoint(
self.rover_pt, name="Rover")).grid(row=1, column=1)
tk.Button(create_click_frame,
text='Obstacle',
command=lambda: self.change_mouse_mode('obstacle')).grid(
row=2, column=0)
tk.Button(create_click_frame,
text='None',
command=lambda: self.change_mouse_mode('none')).grid(
row=2, column=1)
### manual add waypoint functions
create_manual_frame = tk.Frame(frame)
create_manual_frame.grid(row=5, column=0)
tk.Label(create_manual_frame, text='Manual add:').grid(row=0,
column=0,
columnspan=2)
# text entry labels
tk.Label(create_manual_frame, text='Lat').grid(row=1,
column=0,
sticky=E)
tk.Label(create_manual_frame, text='Lon').grid(row=2,
column=0,
sticky=E)
tk.Label(create_manual_frame, text='Name').grid(row=3,
column=0,
sticky=E)
# text entry boxes
self.lat_entry = tk.Entry(create_manual_frame)
self.lon_entry = tk.Entry(create_manual_frame)
self.name_entry = tk.Entry(create_manual_frame)
self.lat_entry.grid(row=1, column=1)
self.lon_entry.grid(row=2, column=1)
self.name_entry.grid(row=3, column=1)
# button
tk.Button(create_manual_frame,
text='Create Point',
command=self.plot_numeric_point).grid(row=4,
column=0,
columnspan=2)
def display_auto_actions_frame(self, frame):
tk.Label(frame, text='AUTONOMOUS ACTIONS',
font=self.FONT_HEADER).grid(row=0, column=0, columnspan=2)
tk.Button(frame,
text='Plot Course',
command=lambda: self.change_auto_mode('plot')).grid(row=1,
column=0)
tk.Button(frame,
text='Auto',
command=lambda: self.change_auto_mode('auto')).grid(row=1,
column=1)
tk.Button(frame,
text='STOP',
command=lambda: self.change_auto_mode('off')).grid(
row=2, column=0, columnspan=2)
def display_change_map_actions_frame(self, frame):
tk.Label(frame, text="CHANGE MAP",
font=self.FONT_HEADER).grid(row=0, column=0)
map_choices = self.maps.keys()
self.map_str_var = tk.StringVar()
self.map_str_var.set(map_choices[0])
self.map_str_var.trace('w', self.change_map)
tk.OptionMenu(frame, self.map_str_var, *map_choices).grid(row=1,
column=0)
def display_location_info_frame(self, frame):
col = 0
# Rover location
tk.Label(frame, text=' ROVER LOCATION ',
font=self.FONT_HEADER).grid(row=0, column=col, columnspan=2)
tk.Label(frame, text='Lat:').grid(row=1, column=col, sticky=E)
tk.Label(frame, text='Lon:').grid(row=2, column=col, sticky=E)
self.rover_lat_str_var = tk.StringVar()
self.rover_lon_str_var = tk.StringVar()
tk.Label(frame,
textvariable=self.rover_lat_str_var).grid(row=1,
column=col + 1,
sticky=W)
tk.Label(frame,
textvariable=self.rover_lon_str_var).grid(row=2,
column=col + 1,
sticky=W)
col += 2
# GPS status
tk.Label(frame, text=' GPS RTCM ',
font=self.FONT_HEADER).grid(row=0, column=col, columnspan=2)
tk.Label(frame, text='Qual:').grid(row=1, column=col, sticky=E)
tk.Label(frame, text='Age:').grid(row=2, column=col, sticky=E)
self.gps_rtcm_qual_var = tk.StringVar()
self.gps_rtcm_age_var = tk.StringVar()
tk.Label(frame,
textvariable=self.gps_rtcm_qual_var).grid(row=1,
column=col + 1,
sticky=W)
tk.Label(frame,
textvariable=self.gps_rtcm_age_var).grid(row=2,
column=col + 1,
sticky=W)
def display_mouse_info_frame(self, frame):
col = 0
# Mouse location
tk.Label(frame, text=' MOUSE LOCATION ',
font=self.FONT_HEADER).grid(row=0, column=col, columnspan=2)
tk.Label(frame, text='Lat:').grid(row=1, column=col, sticky=E)
tk.Label(frame, text='Lon:').grid(row=2, column=col, sticky=E)
self.mouse_lat_str_var = tk.StringVar()
self.mouse_lon_str_var = tk.StringVar()
tk.Label(frame,
textvariable=self.mouse_lat_str_var).grid(row=1,
column=col + 1,
sticky=W)
tk.Label(frame,
textvariable=self.mouse_lon_str_var).grid(row=2,
column=col + 1,
sticky=W)
col += 2
# Mouse mode
tk.Label(frame, text=' MOUSE MODE ',
font=self.FONT_HEADER).grid(row=0, column=col)
self.mouse_mode_str = tk.StringVar()
self.mouse_mode_str.set(self.mouse_mode)
tk.Label(frame, textvariable=self.mouse_mode_str).grid(row=1,
column=col)
def display_compass_offset_frame(self, frame):
# title
tk.Label(frame, text='COMPASS OFFSET (deg)',
font=self.FONT_HEADER).grid(row=0, column=0, columnspan=2)
self.compass_entry = tk.Entry(frame, width=5)
self.compass_entry.grid(row=1, column=0, sticky=E)
self.compass_offset = 0
self.compass_entry.insert(END, str(self.compass_offset))
tk.Button(frame,
text='Send',
command=lambda: self.update_compass_offset(
self.compass_entry.get())).grid(row=1,
column=1,
sticky=W)
'''
end: GUI LAYOUT FUNCTIONS
'''
def update_compass_offset(self, offset_str):
try:
compass_offset = float(offset_str)
except:
pass
else:
self.compass_offset = compass_offset
def change_mouse_mode(self, mode):
self.mouse_mode = mode
def change_auto_mode(self, mode):
assert (mode == "off") or (mode == 'auto') or (mode == 'plot')
self.auto_control[u'command'] = unicode(mode, "utf-8")
def change_map(self, *args):
# Get new map
self.map = self.maps[self.map_str_var.get()]
# Plot on canvas
self.canvas.config(width=self.map.size[1], height=self.map.size[0])
self.canvas.itemconfig(self.canvas_img, image=self.map.image)
# Replot any waypoints currently onscreen
def update_rover(self):
try:
rover = self.gps.get()
except timeout:
pass
# print("GPS TIMED OUT")
self.rover_lon_str_var.set("No GPS")
self.rover_lat_str_var.set("No GPS")
self.gps_rtcm_qual_var.set("No GPS")
self.gps_rtcm_age_var.set("No GPS")
else:
# TODO: uggly
tmp = None
if self.rover_pt != None:
tmp = self.rover_pt.plot
self.rover_pt = Point.from_gps(self.map, rover['lat'],
rover['lon'])
if tmp is not None:
self.rover_pt.plot = tmp
self.plot_point(self.rover_pt, ROVER_POINT_RADIUS,
ROVER_POINT_COLOR)
if rover['local'][0]:
print("x", rover['local'][1], "y", rover['local'][2])
self.rover_lon_str_var.set(rover['lon'])
self.rover_lat_str_var.set(rover['lat'])
self.gps_rtcm_qual_var.set(rover['qual'])
self.gps_rtcm_age_var.set(rover['age'])
try:
base = self.gps_base.get()
except timeout:
pass
# print("GPSBase TIMED OUT")
else:
self.base_pt = Point.from_gps(self.map, base['lat'], base['lon'])
self.plot_point(self.base_pt, ROVER_POINT_RADIUS, '#ff0000')
# Draw orientation arrow
if self.arrow is not None:
self.canvas.delete(self.arrow)
try:
angle = self.gyro.get()['angle'][0]
except:
pass
else:
# print('angle', angle)
y, x = self.rover_pt.map()
r = 20
self.arrow = self.canvas.create_line(x,
y,
x + r * sin(angle * pi / 180),
y - r * cos(angle * pi / 180),
arrow=tk.LAST)
def update_waypoints(self):
for i, p in enumerate(self.pointLibrary):
_, point = p
if point == None:
continue
point.changeMap(self.map) # update map to be most current map
if i in self.listbox.curselection():
self.plot_selected_point(point)
else:
self.plot_normal_point(point)
# populate UDPComms message with current ordered list of waypoints
msg = [{
u'lat': point.gps()[0],
u'lon': point.gps()[1]
} for _, point in self.pointLibrary if point != None]
self.auto_control[u'waypoints'] = msg
self.auto_control[
u'end_mode'] = 'none' if self.end_with_tennis_search.get(
) == 0 else 'tennis'
def update(self):
try:
self.auto_mode_dis.set(self.auto_control[u'command'].upper())
self.mouse_mode_str.set(self.mouse_mode)
self.update_waypoints()
self.update_rover()
self.auto_control_pub.send(self.auto_control)
# print(self.auto_control[u'end_mode'])
self.compass_offset_pub.send(self.compass_offset)
# self.obstacles_pub.send(self.obstacles)
except:
raise
finally:
self.root.after(50, self.update)
def mouse_motion_callback(self, event):
x, y = event.x, event.y
mouse_point = Point.from_map(self.map, x, y)
self.mouse_lat_str_var.set(round(mouse_point.latitude, 5))
self.mouse_lon_str_var.set(round(mouse_point.longitude, 5))
def mouse_click_callback(self, event):
print "clicked at", event.x, event.y
if self.mouse_mode == "waypoint":
waypoint = Point.from_map(self.map, event.x, event.y)
self.new_waypoint(waypoint)
self.mouse_mode = "none"
elif self.mouse_mode == "obstacle":
self.temp_obstace = Point.from_map(self.map, event.x, event.y)
self.mouse_mode = "obstacle_radius"
elif self.mouse_mode == "obstacle_radius":
assert self.temp_obstace != None
edge_point = Point.from_map(self.map, event.x, event.y)
y, x = edge_point.map()
center_y, center_x = self.temp_obstace.map()
radius = sqrt((center_x - x)**2 + (center_y - y)**2)
self.plot_point(self.temp_obstace,
radius,
"#FF0000",
activestipple='gray50',
width=0)
DIS_SCALE = 1
self.obstacles.append(
Obstacle(self.temp_obstace, DIS_SCALE * radius))
self.temp_obstace = None
self.mouse_mode = "none"
elif self.mouse_mode == "none":
did_click_waypoint = False
### check if we clicked on a waypoint; if so, select it
for i, p in enumerate(self.pointLibrary):
title, point = p
y, x = point.map()
if abs(event.x - x) < WAYPOINT_POINT_RADIUS and abs(
event.y - y) < WAYPOINT_POINT_RADIUS:
print 'you clicked on', title
self.listbox.selection_clear(0, END)
self.listbox.selection_set(i)
did_click_waypoint = True
break
### if clicked on the map but outside of waypoints, clear current selection
if not did_click_waypoint:
self.listbox.selection_clear(0, END)
else:
print "ERROR"
def plot_point(self, point, radius, color, **kwargs):
if point == None:
return
if point.plot != None:
self.del_point(point)
r = radius
y, x = point.map()
point.plot = self.canvas.create_oval(x + r,
y + r,
x - r,
y - r,
fill=color,
**kwargs)
def del_point(self, point):
self.canvas.delete(point.plot)
def plot_numeric_point(self):
new_numeric = Point.from_gps(self.map, float(self.lat_entry.get()),
float(self.lon_entry.get()))
self.new_waypoint(new_numeric, self.name_entry.get())
self.name_entry.delete(0, END)
def new_waypoint(self, point, name=""):
if point == None:
return
self.plot_normal_point(point)
# optional name for the point
if name == "":
title = "Point " + str(self.pointIncrement)
else:
title = name
self.listbox.insert("end", title)
self.pointLibrary.append((title, point))
self.pointIncrement += 1
def delete_selected_waypoint(self):
if len(self.listbox.curselection()) == 0:
print "No waypoint to delete"
return
index = self.listbox.curselection()[0]
_, point = self.pointLibrary[index]
self.del_point(point) # delete from map GUI
del self.pointLibrary[index] # delete from dict
self.listbox.delete(index) # delete from listbox GUI
def reorder_selected_waypoint(self, direction):
if len(self.listbox.curselection()) == 0:
print "No waypoint to reorder"
return
assert direction == -1 or direction == 1
index = self.listbox.curselection()[0]
# if can't move up or down anymore, don't do anything
if (index == 0
and direction == -1) or (index == len(self.pointLibrary) - 1
and direction == 1):
return
# reorder in pointLibrary
title, _ = self.pointLibrary[index]
self.pointLibrary.insert(index + direction,
self.pointLibrary.pop(index))
# reorder in listbox
self.listbox.delete(index)
self.listbox.insert(index + direction, title)
def plot_selected_point(self, point):
self.plot_point(point, WAYPOINT_POINT_RADIUS, WAYPOINT_SELECTED_COLOR)
def plot_normal_point(self, point):
self.plot_point(point, WAYPOINT_POINT_RADIUS, WAYPOINT_DEFAULT_COLOR)
dir_pin = 15
GPIO.setmode(GPIO.BCM)
GPIO.setup(pwm_pin, GPIO.OUT)
GPIO.setup(dir_pin, GPIO.OUT)
pwm = GPIO.PWM(pwm_pin, 50)
GPIO.output(dir_pin, True)
pwm.start(0)
time.sleep(1)
try:
while True:
try:
cmd = target_vel.get()['z']
except timeout:
print "TIMEOUT No commands received"
print('driving z at', 0)
pwm.ChangeDutyCycle(0)
except:
print('driving z at', 0)
pwm.ChangeDutyCycle(0)
raise
else:
if cmd < 0:
GPIO.output(dir_pin, False)
print('down')
else:
GPIO.output(dir_pin, True)
print('up')
class Rover:
def __init__(self):
self.imu = Subscriber(8220, timeout=2)
self.gps = Subscriber(8280, timeout=3)
self.auto_control = Subscriber(8310, timeout=5)
self.lights = Publisher(8311)
self.cmd_vel = Publisher(8830)
self.logger = Logger(8312)
self.aruco = Subscriber(8390, timeout=3)
time.sleep(2) # delay to give extra time for gps message
self.start_gps = self.gps.recv()
self.cmd_sent = False
def project(self, lat, lon):
lat_orig = self.start_gps['lat']
lon_orig = self.start_gps['lon']
RADIUS = 6371 * 1000
lon_per_deg = RADIUS * 2 * math.pi / 360
lat_per_deg = lon_per_deg * math.cos(math.radians(lat_orig))
x = (lon - lon_orig) * lon_per_deg
y = (lat - lat_orig) * lat_per_deg
return (x, y)
def get_pose(self):
gps = self.gps.get()
# heading = math.radians( 360 -self.imu.get()['angle'][0] )
heading = math.radians(self.imu.get()['angle'][0])
return Pose(*self.project(gps['lat'], gps['lon']), heading)
def send_vel(self, forward, twist):
msg = {"f": forward, "t": twist}
if self.cmd_sent:
print("ERROR Multiple commands being sent!")
self.cmd_vel.send(msg)
self.cmd_sent = True
print(msg)
def get_aruco(self):
markers = self.aruco.get()
out = {}
for marker in markers:
idx, dist, angle = marker['id'], marker['dist'], marker['angle']
out[idx] = Bearing(dist, math.radians(angle))
return out
def get_cmd(self):
return self.auto_control.get()
def get_waypoints(self) -> List["Pose"]:
wps = self.auto_control.get()['waypoints']
out = []
for waypoint in wps:
out.append(Pose(*self.project(waypoint['lat'], waypoint['lon'])))
return out
def next_iteration(self):
self.cmd_sent = False
self.logger.next()
if os.geteuid() != 0:
exit("You need to have root privileges to run this script.\nPlease try again, this time using 'sudo'. Exiting.")
a = Subscriber(8830, timeout = 0.3)
odom = Publisher(8820)
print("finding any odrives...")
odrive = odrive.find_any()
print("found an odrive (random)")
odrive.axis0.requested_state = AXIS_STATE_IDLE
odrive.axis1.requested_state = AXIS_STATE_IDLE
while True:
try:
msg = a.get()
print(msg)
odom.send([odrive.axis0.encoder.pos_estimate / odrive.axis0.encoder.config.cpr,
- odrive.axis1.encoder.pos_estimate / odrive.axis1.encoder.config.cpr])
if (msg['t'] == 0 and msg['f'] == 0):
odrive.axis0.requested_state = AXIS_STATE_IDLE
odrive.axis1.requested_state = AXIS_STATE_IDLE
else:
odrive.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
odrive.axis1.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
odrive.axis0.controller.vel_setpoint = (msg['f'] + msg['t'])
odrive.axis1.controller.vel_setpoint = -(msg['f'] - msg['t'])
except timeout:
odrive.axis0.requested_state = AXIS_STATE_IDLE
compass = HMC6343()
# initialize filter
# TODO find good value
filt = ComplementaryFilter(0.99)
lastGyro = 0
lastMag = 0
lastPub = 0
while True:
if time() - lastMag > 0.10:
heading = compass.readHeading()
filt.update_mag(heading)
lastMag = time()
if time() - lastGyro > 0.01:
gyro_x, gyro_y, gyro_z = imu.gyro
filt.update_gyro(-gyro_z)
lastGyro = time()
if time() - lastPub > 0.05:
try:
offset = float(offset_sub.get())
except:
pass
angle = filt.get_angle() + offset
print(angle, heading, filt.lastGyro)
pub.send({'angle': [angle, None, None], 'mag': heading + offset})
lastPub = time()
from UDPComms import Subscriber
import time
import datetime
view_sub = Subscriber(8810)
MSG_INTERVAL = 20
CONTROL_LOG = '/home/cerbaris/pupper_code/control.log'
if __name__ == "__main__":
print('Waiting for messages...')
last_msg = None
while True:
try:
msg = view_sub.get()
if last_msg is None:
last_msg = msg
same = False
else:
same = True
for k, v in msg.items():
if last_msg[k] != v:
same = False
if not same:
print('')
print(datetime.datetime.now())
print(msg)
print('')
curr_time = datetime.datetime.now()
with open(CONTROL_LOG, 'a') as f:
dpadx = values["dpad_right"] - values["dpad_left"]
dpady = values["dpad_up"] - values["dpad_down"]
msg = {
"ly": left_y,
"lx": left_x,
"rx": right_x,
"ry": right_y,
"L2": L2,
"R2": R2,
"R1": R1,
"L1": L1,
"dpady": dpady,
"dpadx": dpadx,
"x": x,
"square": square,
"circle": circle,
"triangle": triangle,
"message_rate": MESSAGE_RATE,
}
joystick_pub.send(msg)
try:
msg = joystick_subcriber.get()
joystick.led_color(**msg["ps4_color"])
except timeout:
pass
time.sleep(1 / MESSAGE_RATE)
class DataLogger(object):
def __init__(self, rate=0.1, imu=None):
self.obj_sensors = ObjectSensors()
if imu is None:
self.imu = IMU()
else:
self.imu = imu
self.cv_sub = Subscriber(CV_PORT, timeout=0.5)
self.cmd_sub = Subscriber(CMD_PORT)
self.data = None
self.data_columns = ['timestamp', 'x_acc', 'y_acc', 'z_acc', 'roll',
'pitch', 'yaw', 'left_obj', 'right_obj',
'center_obj', 'bbox_x', 'bbox_y', 'bbox_h',
'bbox_w', 'bbox_label', 'bbox_confidence',
'robo_x_vel', 'robo_y_vel',
'robo_yaw_rate', 'imu_calibration',
'gyro_calibration', 'accel_calibration',
'mag_calibration']
self.timer = RepeatTimer(rate, self.log)
self.all_img = []
def log(self):
imu = self.imu.read()
obj = self.obj_sensors.read()
try:
# Ben's computer vision service is publishing a list of
# dictionaries, empty list is nothing
cv = self.cv_sub.get()
# print(cv)
if cv == []:
cv = dict.fromkeys(['bbox_x', 'bbox_y', 'bbox_h', 'bbox_w',
'bbox_label', 'bbox_confidence'], np.nan)
else:
tmp = {'time': dt.now().timestamp(), 'img': cv.copy()}
self.all_img.append(tmp)
cv = cv[0]
except BaseException as e:
#print(traceback.format_exc())
cv = dict.fromkeys(['bbox_x', 'bbox_y', 'bbox_h', 'bbox_w',
'bbox_label', 'bbox_confidence'], np.nan)
try:
cmd = self.cmd_sub.get()
except:
print('Not getting control data')
cmd = {'ly': 0, 'lx': 0, 'rx': 0}
x_vel = cmd['ly'] * max_x_velocity
y_vel = cmd['lx'] * -max_y_velocity
yaw_rate = cmd['rx'] * -max_yaw_rate
time = dt.now().timestamp()
row = [time, imu['x_acc'], imu['y_acc'], imu['z_acc'],
imu['roll'], imu['pitch'], imu['yaw'],
obj['left'], obj['right'], obj['center'],
cv['bbox_x'], cv['bbox_y'], cv['bbox_h'],
cv['bbox_w'], cv['bbox_label'], cv['bbox_confidence'],
x_vel, y_vel, yaw_rate, imu['sys_calibration'], imu['gyro_calibration'],
imu['accel_calibration'], imu['mag_calibration']]
self.add_data(row)
def add_data(self, row):
#print(row[0])
if self.data is None:
self.start_time = row[0]
row[0] -= self.start_time
self.data = np.array(row)
else:
row[0] -= self.start_time
self.data = np.vstack([self.data, row])
def save_data(self, fn):
np.save(fn, self.data)
df = self.get_pandas()
df.to_csv(fn.replace('npy', 'csv'))
def save_img_data(self, fn):
out = []
t0 = None
for x in self.all_img:
if t0 is None:
t0 = x['time']
t1 = x['time'] - t0
for y in x['img']:
tmp = y.copy()
tmp['time'] = t1
out.append(tmp.copy())
df = pd.DataFrame(out)
df.to_csv(fn)
def run(self):
print('Running logger...')
self.timer.start()
def stop(self):
self.timer.cancel()
print('Logger stopped')
def load_data(self, fn):
self.data = np.load(fn)
def get_pandas(self):
return pd.DataFrame(self.data, columns=self.data_columns)
greenPin = 20
redPin = 21
GPIO.setup(redPin, GPIO.OUT)
GPIO.setup(greenPin, GPIO.OUT)
GPIO.output(redPin, 0)
GPIO.output(greenPin, 0)
flash_time = time.monotonic()
green_led = False
while 1:
try:
msg = cmd.get()
if msg['command'] == 'auto':
try:
status_msg = status.get()
except timeout:
status_msg = "working"
if status_msg == "done":
GPIO.output(redPin, 0)
if (time.monotonic() - flash_time) > 0.5:
green_led = not green_led
GPIO.output(greenPin, green_led)
flash_time = time.monotonic()
else:
GPIO.output(redPin, 1)
GPIO.output(greenPin, 0)
motor_currents[3] = middle_odrive.axis0.motor.current_control.Iq_setpoint
motor_currents[4] = back_odrive.axis1.motor.current_control.Iq_setpoint
motor_currents[5] = back_odrive.axis0.motor.current_control.Iq_setpoint
motor_vels = [0., 0., 0., 0., 0., 0]
motor_vels[0] = front_odrive.axis1.encoder.vel_estimate
motor_vels[1] = front_odrive.axis0.encoder.vel_estimate
motor_vels[2] = middle_odrive.axis1.encoder.vel_estimate
motor_vels[3] = middle_odrive.axis0.encoder.vel_estimate
motor_vels[4] = back_odrive.axis1.encoder.vel_estimate
motor_vels[5] = back_odrive.axis0.encoder.vel_estimate
return motor_currents, motor_vels
while True:
try:
msg = cmd.get()
# print(msg)
# print(control_state)
currents, vels = get_data(front_odrive, middle_odrive, back_odrive)
print(currents)
# print("measured:", front_odrive.axis0.motor.current_control.Iq_measured)
try:
telemetry.send([middle_odrive.vbus_voltage] + currents)
except:
pass
clear_errors(front_odrive)
clear_errors(middle_odrive)
clear_errors(back_odrive)
if (msg['cur'] == 1):
class Arm:
def __init__(self):
self.target_vel = Subscriber(8410)
self.xyz_names = ["x", "y", "yaw"]
self.motor_names = ["elbow", "shoulder", "yaw"]
self.pwm_names_dummy = ["pitch", "z", "dummy", "roll", "grip"]
self.pwm_names = ["pitch", "z", "roll", "grip"]
self.native_positions = {motor: 0 for motor in self.motor_names}
self.xyz_positions = {axis: 0 for axis in self.xyz_names}
self.elbow_left = True
self.CPR = {
'shoulder': -10.4 * 1288.848,
'elbow': -10.4 * 921.744,
'yaw': float(48) / 28 * 34607
}
#'pitch' : -2 * 34607}
self.SPEED_SCALE = 10
self.rc = RoboClaw(find_serial_port(), names = self.motor_names + self.pwm_names_dummy, \
addresses = [128, 129, 130, 131])
try:
while 1:
start_time = time.time()
self.update()
while (time.time() - start_time) < 0.1:
pass
except KeyboardInterrupt:
self.send_speeds({motor: 0
for motor in self.motor_names},
{motor: 0
for motor in self.pwm_names})
raise
except:
self.send_speeds({motor: 0
for motor in self.motor_names},
{motor: 0
for motor in self.pwm_names})
raise
def condition_input(self, target):
target['x'] = -target['x']
target['pitch'] = -target['pitch']
target['grip'] = -target['grip']
target['z'] = -target['z']
target['yaw'] = 0.01 * target['yaw']
# rotates command frame to end effector orientation
angle = self.xyz_positions['yaw']
x = target['x']
y = target['y']
target['x'] = x * math.cos(angle) - y * math.sin(angle)
target['y'] = x * math.sin(angle) + y * math.cos(angle)
return target
def send_speeds(self, speeds, target):
for motor in self.motor_names:
print('driving', motor, 'at',
int(self.SPEED_SCALE * self.CPR[motor] * speeds[motor]))
if int(self.SPEED_SCALE * self.CPR[motor] * speeds[motor]) == 0:
self.rc.drive_duty(motor, 0)
else:
self.rc.drive_speed(
motor,
int(self.SPEED_SCALE * self.CPR[motor] * speeds[motor]))
for motor in self.pwm_names:
print('driving pwm', motor, 'at', int(20000 * target[motor]))
self.rc.drive_duty(motor, int(20000 * target[motor]))
def get_location(self):
for i, motor in enumerate(self.motor_names):
encoder = self.rc.read_encoder(motor)[1]
print motor, encoder
self.native_positions[
motor] = 2 * math.pi * encoder / self.CPR[motor]
self.xyz_positions = self.native_to_xyz(self.native_positions)
print("Current Native: ", self.native_positions)
print("Current XYZ: ", self.xyz_positions)
def xyz_to_native(self, xyz):
native = {}
distance = math.sqrt(xyz['x']**2 + xyz['y']**2)
angle = math.atan2(xyz['x'], xyz['y'])
offset = math.acos((FIRST_LINK**2 + distance**2 - SECOND_LINK**2) /
(2 * distance * FIRST_LINK))
inside = math.acos((FIRST_LINK**2 + SECOND_LINK**2 - distance**2) /
(2 * SECOND_LINK * FIRST_LINK))
# working
# native['shoulder'] = angle + offset
# native['elbow'] = - (math.pi - inside)
if self.elbow_left:
# is in first working configuration
native['shoulder'] = angle + offset - math.pi
native['elbow'] = -(math.pi - inside)
else:
native['shoulder'] = angle - offset - math.pi
native['elbow'] = (math.pi - inside)
native['yaw'] = xyz['yaw'] + native['shoulder'] + native['elbow']
#native['pitch'] = xyz['pitch']
return native
def native_to_xyz(self, native):
xyz = {}
xyz['x'] = FIRST_LINK * math.sin(
native['shoulder']) + SECOND_LINK * math.sin(native['shoulder'] +
native['elbow'])
xyz['y'] = FIRST_LINK * math.cos(
native['shoulder']) + SECOND_LINK * math.cos(native['shoulder'] +
native['elbow'])
xyz['yaw'] = native['yaw'] - (native['shoulder'] + native['elbow'])
#xyz['pitch'] = native['pitch']
return xyz
def dnative(self, dxyz):
x = self.xyz_positions['x']
y = self.xyz_positions['y']
shoulder_diff_x = y / (x**2 + y**2) - (
x / (FIRST_LINK * math.sqrt(x**2 + y**2)) - x *
(FIRST_LINK**2 - SECOND_LINK**2 + x**2 + y**2) /
(2 * FIRST_LINK * (x**2 + y**2)**(3 / 2))
) / math.sqrt(1 - (FIRST_LINK**2 - SECOND_LINK**2 + x**2 + y**2)**2 /
(4 * FIRST_LINK**2 * (x**2 + y**2)))
shoulder_diff_y = -x / (x**2 + y**2) - (
y / (FIRST_LINK * math.sqrt(x**2 + y**2)) - y *
(FIRST_LINK**2 - SECOND_LINK**2 + x**2 + y**2) /
(2 * FIRST_LINK * (x**2 + y**2)**(3 / 2))
) / math.sqrt(1 - (FIRST_LINK**2 - SECOND_LINK**2 + x**2 + y**2)**2 /
(4 * FIRST_LINK**2 * (x**2 + y**2)))
elbow_diff_x = -x / (
FIRST_LINK * SECOND_LINK *
math.sqrt(1 - (FIRST_LINK**2 + SECOND_LINK**2 - x**2 - y**2)**2 /
(4 * FIRST_LINK**2 * SECOND_LINK**2)))
elbow_diff_y = -y / (
FIRST_LINK * SECOND_LINK *
math.sqrt(1 - (FIRST_LINK**2 + SECOND_LINK**2 - x**2 - y**2)**2 /
(4 * FIRST_LINK**2 * SECOND_LINK**2)))
dnative = {}
dnative['shoulder'] = shoulder_diff_x * dxyz[
'x'] + shoulder_diff_y * dxyz['y']
dnative['elbow'] = elbow_diff_x * dxyz['x'] + elbow_diff_y * dxyz['y']
print("Dxyz : ", dxyz)
print("Dnative: ", dnative)
print(
"new location: ",
self.native_to_xyz({
motor: dnative[motor] + self.native_positions[motor]
for motor in self.motor_names
}))
return dnative
def dnative2(self, dxyz):
h = 0.00000001
# print dxyz, self.xyz_positions
x_plus_h = {
axis: self.xyz_positions[axis] + h * dxyz[axis]
for axis in self.xyz_names
}
f_x_plus_h = self.xyz_to_native(x_plus_h)
f_x = self.xyz_to_native(self.xyz_positions)
dnative = {
motor: (f_x_plus_h[motor] - f_x[motor]) / h
for motor in self.motor_names
}
print("Dxyz : ", dxyz)
print("Dnative: ", dnative)
print(
"new location: ",
self.native_to_xyz({
motor: dnative[motor] + f_x[motor]
for motor in self.motor_names
}))
return dnative
def update(self):
print()
print("new iteration")
self.get_location()
try:
target = self.target_vel.get()
# TODO: This shouldn't be necessary, how to fix in UDPComms?
target = {bytes(key): value for key, value in target.iteritems()}
target_f = self.condition_input(target)
speeds = self.dnative2(target_f)
speeds['elbow'] -= 0.002 * target_f['hat'][0]
speeds['shoulder'] -= 0.002 * target_f['hat'][1]
if speeds['elbow'] == 0 and speeds['shoulder'] == 0:
self.elbow_left = self.native_positions['elbow'] < 0
if target_f["reset"]:
print "RESETTING!!!"
speeds = {motor: 0 for motor in self.motor_names}
target_f = {motor: 0 for motor in self.pwm_names}
self.send_speeds(speeds, target_f)
self.rc.set_encoder("shoulder", 0)
self.rc.set_encoder("elbow", 0)
self.rc.set_encoder("yaw", 0)
except timeout:
print "TIMEOUT No commands recived"
speeds = {motor: 0 for motor in self.motor_names}
target_f = {motor: 0 for motor in self.pwm_names}
except ValueError:
print "ValueError The math failed"
speeds = {motor: 0 for motor in self.motor_names}
speeds['elbow'] -= 0.002 * target_f['hat'][0]
speeds['shoulder'] -= 0.002 * target_f['hat'][1]
except:
speeds = {motor: 0 for motor in self.motor_names}
target_f = {motor: 0 for motor in self.pwm_names}
raise
finally:
print "SPEEDS", speeds, target_f
self.send_speeds(speeds, target_f)
class Pursuit:
def __init__(self):
self.imu = Subscriber(8220, timeout=1)
self.gps = Subscriber(8280, timeout=2)
self.auto_control = Subscriber(8310, timeout=5)
self.cmd_vel = Publisher(8830)
# self.lights = Publisher(8590)
self.servo = Publisher(8120)
self.aruco = Subscriber(8390, timeout=2)
time.sleep(3)
self.start_point = {
"lat": self.gps.get()['lat'],
"lon": self.gps.get()['lon']
}
self.lookahead_radius = 6
self.final_radius = 1.5 # how close should we need to get to last endpoint
self.search_radius = 20 # how far should we look from the given endpoint
self.reached_destination = False # switch modes into tennis ball seach
self.robot = None
self.guess = None # where are we driving towards
self.guess_radius = 3 # if we are within this distance we move onto the next guess
self.guess_time = None
self.last_tennis_ball = 0
self.past_locations = []
self.stuck_time = 0
while True:
self.update()
time.sleep(0.1)
def get_guess(self, endpoint):
x, y = endpoint
rand_x = (2 * random.random() - 1) * self.search_radius
rand_y = (2 * random.random() - 1) * self.search_radius
self.random_corrd = (rand_x, rand_y)
print(rand_x, rand_y)
return (x + rand_x, y + rand_y)
def find_ball(self, cmd):
if cmd['end_mode'] == 'none':
print("REACHED ENDPOINT")
# self.lights.send({'r':0, 'g':1, 'b':0})
self.servo.send({'pan': 0, 'tilt': 90})
self.send_stop()
elif cmd['end_mode'] == 'tennis':
print("Entering search program")
try:
marker = self.aruco.get()
except timeout:
marker = None
last_waypoint = cmd['waypoints'][-1]
endpoint = self.project(last_waypoint['lat'], last_waypoint['lon'])
if marker == None:
if (time.time() - self.last_tennis_ball) < 2:
# if we saw a ball in the last 2 sec
out = {"f": 70, "t": 0}
print('cont to prev seen marker', out)
self.cmd_vel.send(out)
else:
if self.guess == None:
self.guess = self.get_guess(endpoint)
self.guess_time = time.time()
print("NEW RANDOM GUESS - first")
elif self.distance(self.guess) < self.guess_radius:
self.guess = self.get_guess(endpoint)
print("NEW RANDOM GUESS - next")
self.guess_time = time.time()
elif (time.time() - self.guess_time) > 40:
self.guess = self.get_guess(endpoint)
print("NEW RANDOM GUESS - timeout")
self.guess_time = time.time()
self.guess = self.get_guess(endpoint)
print("random corrds", self.random_corrd)
diff_angle = self.get_angle(self.guess)
self.send_velocities(diff_angle)
else:
# self.guess_time = time.time() # will this matter?
self.last_tennis_ball = time.time()
if marker['dist'] < 1.5:
print("REACHED MARKER")
self.send_stop()
else:
self.send_velocities_slow(marker['angle'])
else:
print("incorrect mode")
def update(self):
try:
self.robot = self.gps.get()
cmd = self.auto_control.get()
except:
print('lost control')
return
if cmd['command'] == 'off':
print("off")
self.reached_destination = False
self.start_point['lat'] = self.gps.get()['lat']
self.start_point['lon'] = self.gps.get()['lon']
# self.lights.send({'r':1, 'g':0, 'b':0})
self.servo.send({'pan': 0, 'tilt': -90})
elif (cmd['command'] == 'auto'):
last_waypoint = cmd['waypoints'][-1]
if self.reached_destination:
self.find_ball(cmd)
elif( self.distance(self.project(last_waypoint['lat'], last_waypoint['lon'])) \
< self.final_radius ):
self.reached_destination = True
print("REACHED final End point")
elif self.analyze_stuck():
self.un_stick()
else:
lookahead = self.find_lookahead(cmd['waypoints'])
diff_angle = self.get_angle(lookahead)
self.send_velocities(diff_angle)
else:
raise ValueError
def analyze_stuck(self):
self.past_locations.append([
self.project(self.robot['lat'], self.robot['lon']),
self.imu.get()['angle'][0],
time.time()
])
if len(self.past_locations) < 100:
return False
while len(self.past_locations) > 100:
self.past_locations.pop(0)
if (time.time() - self.stuck_time < 14):
return False
abs_angle = lambda x, y: min(abs(x - y + i * 360) for i in [-1, 0, 1])
# location, t = self.past_locations[0]
# if self.distance(location)/(time.time() - t) < 0.1
locations = [self.past_locations[10 * i + 5][0] for i in range(9)]
angles = [self.past_locations[10 * i + 5][1] for i in range(9)]
max_loc = max([self.distance(loc) for loc in locations])
max_angle = max(
[abs_angle(ang,
self.imu.get()['angle'][0]) for ang in angles])
print("we are", max_loc, "from stcuk and angle", max_angle)
if max_loc < 1 and max_angle < 30:
print("WE ARE STUCK")
# self.stuck_location = location
self.stuck_time = time.time()
return True
return False
def un_stick(self):
start_time = time.time()
while (time.time() - start_time) < 4 and self.auto_control.get(
)['command'] == 'auto':
self.analyze_stuck()
out = {"f": -100, "t": -20}
print('unsticking', out)
self.cmd_vel.send(out)
time.sleep(0.1)
start_time = time.time()
start_angle = self.imu.get()['angle'][0]
while (time.time() - start_time) < 1 and self.auto_control.get(
)['command'] == 'auto':
self.analyze_stuck()
out = {"f": 0, "t": -70}
print('unsticking part 2', out)
self.cmd_vel.send(out)
time.sleep(0.1)
start_time = time.time()
while (time.time() - start_time) < 2 and self.auto_control.get(
)['command'] == 'auto':
self.analyze_stuck()
out = {"f": 70, "t": 0}
print('unsticking part 2', out)
self.cmd_vel.send(out)
time.sleep(0.1)
def find_lookahead(self, waypoints):
start_waypoints = [self.start_point] + waypoints
waypoint_pairs = zip(start_waypoints[::-1][1:],
start_waypoints[::-1][:-1])
i = 0
for p1, p2 in waypoint_pairs:
lookahead = self.lookahead_line(p1, p2)
if lookahead is not None:
print("point intersection", i)
return lookahead
i += 1
else:
print("NO intersection found!")
distances = []
for p1, p2 in waypoint_pairs:
lookahead = self.lookahead_line(p1, p2, project=True)
if lookahead is not None:
distances.append((self.distance(lookahead), lookahead))
for p1 in waypoints:
point = self.project(p1['lat'], p1['lon'])
distances.append((self.distance(point), point))
return min(distances)[1]
def distance(self, p1):
x_start, y_start = p1
# self.robot = self.gps.get()
x_robot, y_robot = self.project(self.robot['lat'], self.robot['lon'])
return math.sqrt((x_start - x_robot)**2 + (y_start - y_robot)**2)
def lookahead_line(self, p1, p2, project=False):
x_start, y_start = self.project(p1['lat'], p1['lon'])
x_end, y_end = self.project(p2['lat'], p2['lon'])
# self.robot = self.gps.get()
x_robot, y_robot = self.project(self.robot['lat'], self.robot['lon'])
if (x_robot - x_end)**2 + (y_robot -
y_end)**2 < self.lookahead_radius**2:
return (x_end, y_end)
a = (x_end - x_start)**2 + (y_end - y_start)**2
b = 2 * ((x_end - x_start) * (x_start - x_robot) + (y_end - y_start) *
(y_start - y_robot))
c = (x_start - x_robot)**2 + (y_start -
y_robot)**2 - self.lookahead_radius**2
if b**2 - 4 * a * c <= 0:
if project:
t = -b / (2 * a)
else:
return None
else:
t = (-b + math.sqrt(b**2 - 4 * a * c)) / (2 * a)
if not t <= 1:
return None
if not t >= 0:
return None
x_look = t * x_end + (1 - t) * x_start
y_look = t * y_end + (1 - t) * y_start
return (x_look, y_look)
def get_angle(self, lookahead):
heading_deg = self.imu.get()['angle'][0]
head_rad = math.radians(heading_deg)
x_robot, y_robot = self.project(self.robot['lat'], self.robot['lon'])
x_look, y_look = lookahead
target = math.atan2(x_look - x_robot, y_look - y_robot)
# anti clockwise positive
return ((target - head_rad + math.pi) % (2 * math.pi) - math.pi)
def send_velocities_slow(self, angle):
# turn_rate = -100*math.tanh(1*angle)
turn_rate = -200 * angle / math.pi
if math.fabs(angle) < math.radians(10):
forward_rate = 90
elif math.fabs(angle) < math.radians(60):
forward_rate = 50
elif math.fabs(angle) < math.radians(140):
forward_rate = 30
else:
forward_rate = 0
out = {"f": forward_rate, "t": turn_rate}
# out = {"f": 70, "t": -150*angle/math.pi }
print(out)
self.cmd_vel.send(out)
def send_velocities(self, angle):
# turn_rate = -100*math.tanh(1*angle)
turn_rate = -200 * angle / math.pi
if math.fabs(angle) < math.radians(10):
forward_rate = 130
elif math.fabs(angle) < math.radians(60):
forward_rate = 80
elif math.fabs(angle) < math.radians(140):
forward_rate = 30
else:
forward_rate = 0
out = {"f": forward_rate, "t": turn_rate}
# out = {"f": 70, "t": -150*angle/math.pi }
print(out)
self.cmd_vel.send(out)
def send_stop(self):
out = {"f": 0, "t": 0}
# out = {"f": 70, "t": -150*angle/math.pi }
print('stoping', out)
self.cmd_vel.send(out)
def project(self, lat, lon):
lat_orig = self.start_point['lat']
lon_orig = self.start_point['lon']
RADIUS = 6371 * 1000
lon_per_deg = RADIUS * 2 * math.pi / 360
lat_per_deg = lon_per_deg * math.cos(math.radians(lat_orig))
x = (lon - lon_orig) * lon_per_deg
y = (lat - lat_orig) * lat_per_deg
return (x, y)
class Arm:
def __init__(self):
GPIO.setmode(GPIO.BCM)
GPIO.setup(SHOULDER_HOME_INPUT, GPIO.IN, pull_up_down = GPIO.PUD_UP)
GPIO.setup(ELBOW_HOME_INPUT, GPIO.IN, pull_up_down = GPIO.PUD_UP)
self.target_vel = Subscriber(8410)
self.xyz_names = ["x", "y","yaw"]
self.output_pub = Publisher(8420)
self.cartesian_motors = ["shoulder","elbow","yaw"]
self.motor_names = ["shoulder","elbow","yaw","roll","grip"]
self.pwm_names = ["pitch"]
self.ordering = ["shoulder","elbow","pitch","yaw","grip","roll"]
self.native_positions = { motor:0 for motor in self.motor_names}
self.currents = { motor:0 for motor in self.motor_names}
self.xyz_positions = { axis:0 for axis in self.xyz_names}
self.elbow_left = True
self.CPR = {'shoulder': -12.08 * 4776.38,
'elbow' : -12.08 * 2442.96,
'yaw' : -float(48)/27 * 34607,
'roll' : 455.185*float(12*53/20),
'grip' : 103.814*float(12*36/27)}
# 'pitch' : -2 * 34607}
self.SPEED_SCALE = 20
self.rc = RoboClaw(find_serial_port(), names = self.ordering,\
addresses = [130,128,129])
self.zeroed = False
self.storageLoc = [0,0]
self.limits = {'shoulder':[-2.18,2.85],
'elbow' : [-4,2.24], #-2.77
'yaw' : [-3.7,3.7] }
self.dock_pos = {'shoulder': 2.76,
'elbow' : -2.51,
'yaw' : -3.01 }
self.dock_speeds = [.01,.006]
self.forcing = False
try:
while 1:
start_time = time.time()
self.update()
while (time.time() - start_time) < 0.1:
pass
except KeyboardInterrupt:
self.send_speeds( {motor: 0 for motor in self.motor_names}, {motor: 0 for motor in self.pwm_names} )
raise
except:
self.send_speeds( {motor: 0 for motor in self.motor_names}, {motor: 0 for motor in self.pwm_names} )
raise
#deadbands & nonlinear controls on speed
def condition_input(self,target):
target['x'] = - target['x']
if(target['pitch'] > .1):
target['pitch'] = -1.23* (target['pitch']-.1)**2
elif(target['pitch'] < -.1):
target['pitch'] = 1.23* (target['pitch']+.1)**2
else:
target['pitch'] = 0
target['grip'] = .04*target['grip']
target['roll'] = .01*target['roll']
target['z'] = - target['z']
if(target['yaw'] > .1):
target['yaw'] = 0.008* (1.1 * (target['yaw']-.1))**3
elif(target['yaw'] < -.1):
target['yaw'] = 0.008* (1.1 * (target['yaw']+.1))**3
else:
target['yaw'] = 0
# rotates command frame to end effector orientation
angle = self.xyz_positions['yaw']
x = -target['x']*abs(target['x'])
y = target['y']*abs(target['y'])
if(target['trueXYZ'] == 0):
target['x'] = x*math.cos(angle) - y*math.sin(angle)
target['y'] = x*math.sin(angle) + y*math.cos(angle)
return target
#output all speeds to all motors
def send_speeds(self, speeds, target):
for motor in self.motor_names:
print('driving', motor, 'at', int(self.SPEED_SCALE * self.CPR[motor] * speeds[motor]))
if int(self.SPEED_SCALE * self.CPR[motor] * speeds[motor]) == 0:
self.rc.drive_duty(motor, 0)
else:
self.rc.drive_speed(motor, int(self.SPEED_SCALE * self.CPR[motor] * speeds[motor]))
for motor in self.pwm_names:
print('driving pwm', motor, 'at', int(20000*target[motor]))
self.rc.drive_duty(motor, int(20000*target[motor]))
#check if motors are homed, get position & currents
def get_status(self):
self.forcing = False
for i,motor in enumerate(self.cartesian_motors):
encoder = self.rc.read_encoder(motor)[1]
print(motor,encoder)
self.native_positions[motor] = 2 * math.pi * encoder/self.CPR[motor]
self.currents[motor] = self.rc.read_current(motor)
if(self.currents[motor] > 1.5):
self.forcing = True
self.xyz_positions = self.native_to_xyz(self.native_positions)
print("Current Native: ", self.native_positions)
print("Current XYZ: ", self.xyz_positions)
def xyz_to_native(self, xyz):
native = {}
distance = math.sqrt(xyz['x']**2 + xyz['y']**2)
angle = -math.atan2(xyz['x'], xyz['y'])
offset = math.acos( ( FIRST_LINK**2 + distance**2 - SECOND_LINK**2 ) / (2*distance * FIRST_LINK) )
inside = math.acos( ( FIRST_LINK**2 + SECOND_LINK**2 - distance**2 ) / (2*SECOND_LINK * FIRST_LINK) )
if self.elbow_left:
# is in first working configuration
native['shoulder'] = angle + offset - math.pi
native['elbow'] = - (math.pi - inside)
else:
native['shoulder'] = angle - offset - math.pi
native['elbow'] = (math.pi - inside)
native['yaw'] = xyz['yaw'] -native['shoulder']-native['elbow']
# native['pitch'] = xyz['pitch']
return native
def native_to_xyz(self, native):
xyz = {}
xyz['x'] = -FIRST_LINK * math.sin(native['shoulder']) - SECOND_LINK * math.sin(native['shoulder'] + native['elbow'])
xyz['y'] = FIRST_LINK * math.cos(native['shoulder']) + SECOND_LINK * math.cos(native['shoulder'] + native['elbow'])
xyz['yaw'] = native['yaw'] + (native['shoulder']+native['elbow'])
# xyz['pitch'] = native['pitch']
return xyz
# analytic jacobian of angles -> XYZ
def dnative(self, dxyz):
x = self.xyz_positions['x']
y = self.xyz_positions['y']
shoulder_diff_x = y/(x**2 + y**2) - (x/(FIRST_LINK*math.sqrt(x**2 + y**2)) - x*(FIRST_LINK**2 - SECOND_LINK**2 + x**2 + y**2)/(2*FIRST_LINK*(x**2 + y**2)**(3/2)))/math.sqrt(1 - (FIRST_LINK**2 - SECOND_LINK**2 + x**2 + y**2)**2/(4*FIRST_LINK**2*(x**2 + y**2)))
shoulder_diff_y = -x/(x**2 + y**2) - (y/(FIRST_LINK*math.sqrt(x**2 + y**2)) - y*(FIRST_LINK**2 - SECOND_LINK**2 + x**2 + y**2)/(2*FIRST_LINK*(x**2 + y**2)**(3/2)))/math.sqrt(1 - (FIRST_LINK**2 - SECOND_LINK**2 + x**2 + y**2)**2/(4*FIRST_LINK**2*(x**2 + y**2)))
elbow_diff_x = -x/(FIRST_LINK*SECOND_LINK*math.sqrt(1 - (FIRST_LINK**2 + SECOND_LINK**2 - x**2 - y**2)**2/(4*FIRST_LINK**2*SECOND_LINK**2)))
elbow_diff_y = -y/(FIRST_LINK*SECOND_LINK*math.sqrt(1 - (FIRST_LINK**2 + SECOND_LINK**2 - x**2 - y**2)**2/(4*FIRST_LINK**2*SECOND_LINK**2)))
dnative = {}
dnative['shoulder'] = shoulder_diff_x * dxyz['x'] + shoulder_diff_y * dxyz['y']
dnative['elbow'] = elbow_diff_x * dxyz['x'] + elbow_diff_y * dxyz['y']
print("Dxyz : ", dxyz)
print("Dnative: ", dnative)
print("new location: ", self.native_to_xyz ( {motor:dnative[motor] + self.native_positions[motor] for motor in self.motor_names}) )
return dnative
#numerical jacobian of angles -> XYZ
def dnative2(self, dxyz):
h = 0.00000001
# print dxyz, self.xyz_positions
x_plus_h = { axis:self.xyz_positions[axis] + h*dxyz[axis] for axis in self.xyz_names}
f_x_plus_h = self.xyz_to_native(x_plus_h)
f_x = self.xyz_to_native(self.xyz_positions)
dnative = {motor:(f_x_plus_h[motor] - f_x[motor])/h for motor in self.cartesian_motors}
print("Dxyz : ", dxyz)
print("Dnative: ", dnative)
print("new location: ", self.native_to_xyz ( {motor:dnative[motor] + f_x[motor] for motor in self.cartesian_motors}) )
return dnative
def sign(self,val):
return int(val > 0) - int(val < 0)
#prevent movement near singularity or if the motor is out of bounds or at home
def check_in_bounds(self, speeds):
inBounds = True
if(abs(self.native_positions['elbow']) < .4):
if(self.native_positions['elbow'] < 0 and self.sign(speeds['elbow']) == 1):
inBounds = False
print("SINGULARITY")
elif(self.native_positions['elbow'] > 0 and self.sign(speeds['elbow']) == -1):
inBounds = False
print("SINGULARITY")
for motor in self.cartesian_motors:
if(self.sign(speeds[motor]) == -1):
if(self.native_positions[motor] < self.limits[motor][0]):
inBounds = False
elif(self.native_positions[motor] > self.limits[motor][1]):
inBounds = False
if(not inBounds):
for motor in self.cartesian_motors:
speeds[motor] = 0
return speeds
def check_limits(self, speeds):
inBounds = True
if(GPIO.input(SHOULDER_HOME_INPUT) and self.sign(speeds['elbow']) == 1):
self.rc.set_encoder('shoulder',SHOULDER_HOME)
inBounds = False
if(GPIO.input(ELBOW_HOME_INPUT) and self.sign(speeds['elbow']) == -1):
self.rc.set_encoder('elbow',ELBOW_HOME*self.cpr['elbow'])
inBounds = False
if(not inBounds):
for motor in self.cartesian_motors:
speeds[motor] = 0
return speeds
#return to our docking position
def dock(self,speeds):
if(abs(self.native_positions['yaw']-self.dock_pos['yaw']-.03)>.01):
speeds['shoulder'] = 0
speeds['elbow'] = 0
speeds['yaw'] = self.dock_speed(self.native_positions['yaw'],self.dock_pos['yaw']+.03,self.dock_speeds[0],self.dock_speeds[1])
elif(abs(self.native_positions['elbow']-self.dock_pos['elbow']+.03)>.01):
speeds['shoulder'] = 0
speeds['elbow'] = self.dock_speed(self.native_positions['elbow'],self.dock_pos['elbow']-.03,self.dock_speeds[0],self.dock_speeds[1])
speeds['yaw'] = 0
elif(abs(self.native_positions['shoulder']-self.dock_pos['shoulder']-.03)>.01):
speeds['shoulder'] = self.dock_speed(self.native_positions['shoulder'],self.dock_pos['shoulder']+.03,self.dock_speeds[0],self.dock_speeds[1])
speeds['elbow'] = 0
speeds['yaw'] = 0
return speeds
#PID for docking speed
def dock_speed(self,curPos,desiredPos,P,maxV):
dir = self.sign(desiredPos-curPos)
output = abs(curPos-desiredPos)*P+.0005
if(output > maxV):
output = maxV
return output*dir
def update(self):
print()
print("new iteration")
self.get_status()
output = {}
print("read status of shoulder", GPIO.input(20))
for d in (self.native_positions,self.xyz_positions):
output.update(d)
#print("HOME STATUS ", self.home_status)
#output.update({"shoulder limit", self.home_status["shoulder"]})
output.update({"forcing":self.forcing})
self.output_pub.send(output)
try:
target = self.target_vel.get()
# TODO: This shouldn't be necessary, how to fix in UDPComms?
target = {bytes(key): value for key, value in target.iteritems()}
target_f = self.condition_input(target)
speeds = self.dnative2(target_f)
if(not self.zeroed):
speeds['elbow'] = 0
speeds['shoulder'] = 0
speeds = self.check_in_bounds(speeds)
speeds['elbow'] -= 0.002 * target_f['hat'][0]
speeds['shoulder'] -= 0.002 * target_f['hat'][1]
speeds['yaw'] += target_f['yaw']
speeds['roll'] = target_f['roll']
speeds['grip'] = target_f['grip'] + speeds['roll']
speeds = self.check_limits(speeds)
if speeds['elbow'] == 0 and speeds['shoulder'] == 0:
self.elbow_left = self.native_positions['elbow'] < 0
if(target_f['dock']):
speeds=self.dock(speeds)
if target_f["reset"]:
print ("RESETTING!!!")
self.zeroed = True
speeds = {motor: 0 for motor in self.motor_names}
target_f = {motor: 0 for motor in self.pwm_names}
self.send_speeds(speeds, target_f)
self.rc.set_encoder("shoulder",0)
self.rc.set_encoder("elbow",0)
self.rc.set_encoder("yaw",0)
elif target_f["resetdock"]:
print ("RESETTING (in dock position)!!!")
self.zeroed = True
speeds = {motor: 0 for motor in self.motor_names}
target_f = {motor: 0 for motor in self.pwm_names}
self.send_speeds(speeds, target_f)
self.rc.set_encoder("shoulder",int(self.CPR["shoulder"]*self.dock_pos['shoulder']/6.28))
self.rc.set_encoder("elbow",int(self.CPR["elbow"]*self.dock_pos['elbow']/6.28))
self.rc.set_encoder("yaw",int(self.CPR["yaw"]*self.dock_pos['yaw']/6.28))
except timeout:
print ("TIMEOUT No commands recived")
speeds = {motor: 0 for motor in self.motor_names}
target_f = {}
target_f = {motor: 0 for motor in self.pwm_names}
except ValueError:
print ("ValueError The math failed")
speeds = {motor: 0 for motor in self.motor_names}
speeds['elbow'] -= 0.002 * target_f['hat'][0]
speeds['shoulder'] -= 0.002 * target_f['hat'][1]
except:
speeds = {motor: 0 for motor in self.motor_names}
target_f = {motor: 0 for motor in self.pwm_names}
raise
finally:
print ("SPEEDS"), speeds, target_f
self.send_speeds(speeds, target_f)
class SLAM:
def __init__(self):
self.viz = Vizualizer()
self.odom = Subscriber(8810, timeout=0.2)
self.lidar = Subscriber(8110, timeout=0.1)
self.robot = Robot()
self.update_time = time.time()
self.dt = None
self.scan = None
self.viz.after(100, self.update)
self.viz.mainloop()
def update(self):
self.dt = time.time() - self.update_time
self.update_time = time.time()
print("dt", self.dt)
self.update_odom()
self.update_lidar()
loop_time = 1000 * (time.time() - self.update_time)
self.viz.after(int(max(100 - loop_time, 0)), self.update)
def update_odom(self):
try:
da, dy = self.odom.get()['single']['odom']
da *= self.dt
dy *= self.dt
t = Transform.fromOdometry(da, (0, dy))
self.robot.drive(t)
self.viz.plot_Robot(self.robot)
except timeout:
print("odom timeout")
pass
def update_lidar(self):
try:
scan = self.lidar.get()
# print("scan", scan)
pc = PointCloud.fromScan(scan)
# lidar in robot frame
pc = pc.move(Transform.fromComponents(0, (-100, 0)))
pc = pc.move(self.robot.get_transform())
if (self.scan == None):
self.scan = pc
self.viz.plot_PointCloud(self.scan, clear=False)
else:
self.viz.clear_PointCloud()
self.viz.plot_PointCloud(self.scan)
self.viz.plot_PointCloud(pc, c="blue")
cloud, transform = self.scan.fitICP(pc)
self.robot.move(transform)
if cloud is not None:
# self.viz.plot_PointCloud(cloud, c="red")
self.scan = self.scan.extend(cloud)
self.viz.plot_PointCloud(cloud, clear=False)
except timeout:
print("lidar timeout")
pass
cross = values['button_cross']
circle = values['button_circle']
triangle = values['button_triangle']
PS = values['button_ps']
# hat directions could be reversed from previous version
hat = [
values["dpad_up"] - values["dpad_down"],
values["dpad_right"] - values["dpad_left"]
]
reset = (PS == 1) and (triangle == 1)
reset_dock = (PS == 1) and (square == 1)
try:
feedback = feedback_port.get()
#feedback = {bytes(key): value for key, value in feedback.iteritems()}
j.led_color(blue=255)
if (feedback['forcing']):
j.led_color(red=255, blue=255)
except timeout:
pass
target_vel = {
"x": l_side,
"y": l_forward,
"z": (r_trigger - l_trigger) / 2,
"yaw": -r_side,
"pitch": r_forward,
"roll": (r_shoulder - l_shoulder),
"grip": cross - square,