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 __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 __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 __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
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 __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 __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 __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 __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 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)
import math import time import RPi.GPIO as GPIO from UDPComms import Subscriber, timeout cmd = Subscriber(8590, timeout=5) greenPin = 13 redPin = 12 bluePin = 18 GPIO.setmode(GPIO.BCM) GPIO.setup(redPin, GPIO.OUT) GPIO.setup(greenPin, GPIO.OUT) GPIO.setup(bluePin, GPIO.OUT) pwmRed = GPIO.PWM(redPin, 490) pwmGreen = GPIO.PWM(greenPin, 490) pwmBlue = GPIO.PWM(bluePin, 490) pwmRed.start(0) pwmGreen.start(0) pwmBlue.start(0) maxPower = 50 r = 0 g = 0 b = 1.0
import time
import bluetooth
import json
from pupperpy.BluetoothInterface import BluetoothServer
## Configurable ##
hostMACAddress = 'B8:27:EB:5E:D6:8F' ## MAC address to bluetooth adapter on pi
BLE_PORT = 3
BLE_MSG_SIZE = 1024
MESSAGE_RATE = 20
## End Config ##
PUPPER_COLOR = {"red":0, "blue":0, "green":255}
pupper_pub = Publisher(8830)
pupper_sub = Subscriber(8840, timeout=0.01)
def send_command(values):
left_y = -values["left_analog_y"]
right_y = -values["right_analog_y"]
right_x = values["right_analog_x"]
left_x = values["left_analog_x"]
L2 = values["l2"]
R2 = values["r2"]
R1 = values["r1"]
L1 = values["l1"]
square = values["button_square"]
#!/usr/bin/env python3
import odrive
from odrive.enums import *
from UDPComms import Subscriber, Publisher, timeout
import time
import os
if os.geteuid() != 0:
exit(
"You need to have root privileges to run this script.\nPlease try again, this time using 'sudo'. Exiting."
)
cmd = Subscriber(8830, timeout=0.3)
telemetry = Publisher(8810)
print("finding an odrives...")
middle_odrive = odrive.find_any(serial_number="206230804648")
print("found middle odrive")
front_odrive = odrive.find_any(serial_number="206C35733948")
print("found middle odrive")
back_odrive = odrive.find_any(serial_number="207D35903948")
print("found back odrive")
print("found all odrives")
def clear_errors(odrive):
if odrive.axis0.error:
print("axis 0", odrive.axis0.error)
from UDPComms import Publisher, Subscriber, timeout
from PS4Joystick import Joystick
import time
## you need to git clone the PS4Joystick repo and run `sudo bash install.sh`
## Configurable ##
MESSAGE_RATE = 20
PUPPER_COLOR = {"red": 0, "blue": 0, "green": 255}
joystick_pub = Publisher(8830, 65530)
joystick_subcriber = Subscriber(8840, timeout=0.01)
joystick = Joystick()
joystick.led_color(**PUPPER_COLOR)
while True:
print("running")
values = joystick.get_input()
left_y = -values["left_analog_y"]
right_y = -values["right_analog_y"]
right_x = values["right_analog_x"]
left_x = values["left_analog_x"]
L2 = values["l2_analog"]
R2 = values["r2_analog"]
R1 = values["button_r1"]
L1 = values["button_l1"]
"""
This script interfaces with the sparkfun GPS module in Rover mode
It publishes the rovers location to port 8280
If it receives RTCM correction on port 8290 it will also use them
"""
import time, math, datetime
import serial, pynmea2
from UDPComms import Publisher, Subscriber, timeout
pub_gps = Publisher(8280)
rtcm_sub = Subscriber(8290, timeout=0)
ser = serial.Serial("/dev/serial0", timeout=None, writeTimeout=0)
def project(lat, lon, lat_orig, lon_orig):
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)
try:
while 1:
try:
correction = rtcm_sub.recv()
rad = math.atan2(self.lastSin, self.lastCos) + omega_rad * delta_t
self.lastSin = math.sin(rad)
self.lastCos = math.cos(rad)
self.lastGyro = (self.lastGyro + omega * delta_t) % 360
def get_angle(self):
rad = math.atan2(self.lastSin, self.lastCos)
return math.degrees(rad) % 360
pub = Publisher(8220)
offset = 10 #random starting value mostly correct aroudn stanford
offset_sub = Subscriber(8210, timeout=5)
# I2C connection to IMU
i2c = busio.I2C(board.SCL, board.SDA)
imu = adafruit_lsm9ds1.LSM9DS1_I2C(i2c)
# I2C connection to IMU
compass = HMC6343()
# initialize filter
# TODO find good value
filt = ComplementaryFilter(0.99)
lastGyro = 0
lastMag = 0
lastPub = 0
from __future__ import division
import RPi.GPIO as GPIO
import time
from UDPComms import Subscriber, timeout
import math
target_vel = Subscriber(8410, timeout=1)
pwm_pin = 14 #TODO CAHNGE
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)
from UDPComms import Subscriber, timeout
pan_pin = 14
tilt_pin = 4
GPIO.setmode(GPIO.BCM)
GPIO.setup(pan_pin, GPIO.OUT)
GPIO.setup(tilt_pin, GPIO.OUT)
pan = GPIO.PWM(pan_pin, 50)
tilt = GPIO.PWM(tilt_pin, 50)
pan.start(7.5)
tilt.start(7.5)
sub = Subscriber(8120, timeout=1)
time.sleep(1)
pan_angle = 0
tilt_angle = 0
def clamp(x, ma, mi):
if x > ma:
return ma
elif x < mi:
return mi
else:
return x
import math
import time
import RPi.GPIO as GPIO
GPIO.setmode(GPIO.BCM)
from UDPComms import Subscriber, timeout
cmd = Subscriber(8310, timeout=1)
status = Subscriber(8311, timeout=1)
time.sleep(2)
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()
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:
def __init__(self, port=8830):
#port is definetly subject to change
self.pi_subscriber = Subscriber(port)
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 __init__(self):
self.particle_num = 100
self.particles = [ Particle() for _ in range(self.particle_num) ]
self.odom = Subscriber()
self.lidar = Subscriber()
from UDPComms import Publisher, Subscriber, timeout
from PS4Joystick import Joystick
import time
from enum import Enum
drive_pub = Publisher(8830)
arm_pub = Publisher(8410)
feedback_port = Subscriber(8420)
j = Joystick()
MODES = Enum('MODES', 'SAFE DRIVE ARM')
mode = MODES.SAFE
while True:
values = j.get_input()
if (values['button_ps']):
if values['dpad_up']:
mode = MODES.DRIVE
j.led_color(red=255)
elif values['dpad_right']:
mode = MODES.ARM
j.led_color(blue=255)
elif values['dpad_down']:
mode = MODES.SAFE
j.led_color(green=255)
# overwrite when swiching modes to prevent phantom motions