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
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
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.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
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")
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
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")
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 = []
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
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):
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)
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 __init__(self):
self.particle_num = 100
self.particles = [ Particle() for _ in range(self.particle_num) ]
self.odom = Subscriber()
self.lidar = Subscriber()
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
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
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:
#!/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)
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"]
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
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"]
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
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 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)
def __init__(self, port=8830):
#port is definetly subject to change
self.pi_subscriber = Subscriber(port)
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)
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 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)