def init_motors():
dagor_motors.init()
dagor_motors.require_stopped('ha')
dagor_motors.require_stopped('de')
dagor_motors.enable('ha')
dagor_motors.enable('de')
dagor_motors.set_speed('ha', 0)
dagor_motors.set_speed('de', 0)
def reset():
global current_speed
r.color_mapping.clear()
r.vote_count.clear()
r.left.clear()
r.right.clear()
r.state["last-left-count-upon-action"] = 0
r.state["last-right-count-upon-action"] = 0
r.state["current-position"] = position_default
r.redis_conn.delete("status-lock")
motors.init()
motors.barrel_rotation_off()
motors.turn_motors_on()
current_speed = default_speed
motors.set_speed(current_speed)
return "reset"
#right is master, left is slave master_power = .25 slave_power = -.25 right_num_revs = 0 left_num_revs = 0 kp = .5 #distance modifier d_mod = .95 turns= angle/360.0*math.pi*diameter/wheel_circumference dist = distance/wheel_circumference encoders.init() encoders.clear() motors.init() en_left, en_right = encoders.read() SETTINGS_FILE = "RTIMULib" def adjustMotorPowers(): global slave_power global en_left global en_right global kp error = en_right + en_left slave_power -= error/kp encoders.clear() time.sleep(.1)
def go(given_angle, given_distance):
angle = given_angle
distance = given_distance
curr_time = time.time()
next_state = False
#constants in feet
diameter = 11.25
wheel_circumference = 16.72
#right is master, left is slave
global slave_power
global master_power
master_power = .25
slave_power = -.25
global right_num_revs
right_num_revs = 0
global left_num_revs
left_num_revs = 0
global kp
kp = .5
#distance modifier
d_mod = .95
turns = angle / 360.0 * math.pi * diameter / wheel_circumference
dist = distance / wheel_circumference
encoders.init()
encoders.clear()
motors.init()
global en_right
global en_left
en_left, en_right = encoders.read()
while True:
try:
if (abs(right_num_revs) + abs(left_num_revs)
) / 2.0 >= turns * d_mod and not next_state:
print slave_power
print master_power
curr_time = time.time()
next_state = True
if time.time() > (curr_time + 0) and next_state:
break
motors.speed(slave_power, master_power)
adjustMotorPowers()
readEncoder()
except KeyboardInterrupt:
break
motors.speed(0, 0)
time.sleep(1)
kill_constant = 5
right_num_revs = 0.0
left_num_revs = 0.0
slave_power = 0.233
master_power = 0.25
while True:
if (abs(right_num_revs) + abs(left_num_revs)) / 2.0 >= dist * d_mod:
break
try:
if (abs(left_num_revs) - abs(right_num_revs)) > kill_threshold:
master_power += .95 * master_power + .05 * kill_constant
print(str(right_num_revs) + " " + str(left_num_revs))
motors.speed(slave_power, master_power)
adjustMotorPowers_straight()
readEncoder_straight()
print "Slave Speed: " + str(slave_power), "Master Speed: " + str(
master_power)
except KeyboardInterrupt:
break
motors.cleanup()
#balance on two points using two propellers across from eachother, divided by the axis of rotation
import motors
import LSM303D as accel
import time
import lowpass as lp
bus = accel.bus_init()
gain = accel.a_init(bus)
zero = motors.init()
motors.test()
m1 = zero
m2 = zero
m3 = zero
m4 = zero
axl = [0,0,0,0,0];
time.sleep(4)
print('Go!')
for x in xrange(0,600):
axl.insert(0, accel.a_get(bus, gain)[1]/1000)
axl.pop()
dvt = lp.deriv
axl_filt = lp.simple(axl)
def _main(args):
only_run = False
if args['api']:
if args['run']:
api.run()
return
if args['init']:
dagor_focus.rehome()
dagor_track.run()
if args['configure']:
dagor_motors.init()
dagor_motors.configure()
return
got_get_arg = any(
(args['celest'], args['local'], args['altaz'], args['chirality']))
if args['get'] and got_get_arg:
values = {}
template = ''
if args['celest']:
if args['human']:
template = "ra={ra}\nde={de}"
else:
template = "{ra} {de}"
values = dagor_position.get_celest()
if not args['float']:
values = {
'ra': format_hours(values['ra']),
'de': format_degrees(values['de']),
}
elif args['local']:
if args['human']:
template = "ha={ha}\nde={de}"
else:
template = "{ha} {de}"
values = dagor_position.get_local()
if not args['float']:
values = {
'ha': format_hours(values['ha']),
'de': format_degrees(values['de']),
}
elif args['altaz']:
if args['human']:
template = "alt={alt}\naz={az}"
else:
template = "{alt} {az}"
values = dagor_position.get_altaz()
if not args['float']:
values = {
'alt': format_degrees(values['alt']),
'az': format_degrees(values['az']),
}
elif args['chirality']:
template = "{chirality}"
values = {'chirality': dagor_position.get_chirality()}
print(template.format(**values))
return
if args['goto'] and not args['focus']:
track = False
stop_on_target = True
quick = True if args['quick'] else False
target_celest = None
chirality = None # default: None (keep chirality)
if args['ce']:
chirality = dagor_position.CHIRAL_E
if args['cw']:
chirality = dagor_position.CHIRAL_W
if args['cc']:
chirality = dagor_position.CHIRAL_CLOSEST
if args['home']:
chirality = dagor_position.HOME_CHIRALITY
if args['ce']:
chirality = dagor_position.CHIRAL_E
target_celest = dagor_position.altaz_to_celest(
dagor_position.HOME_ALTAZ)
quick = True
stop_on_target = True
if args['home2']:
chirality = dagor_position.HOME_N_CHIRALITY
if args['cw']:
chirality = dagor_position.CHIRAL_W
target_celest = dagor_position.altaz_to_celest(
dagor_position.HOME_N_ALTAZ)
quick = True
stop_on_target = True
if args['park']:
chirality = dagor_position.PARK_CHIRALITY
if args['ce']:
chirality = dagor_position.CHIRAL_E
target_celest = dagor_position.altaz_to_celest(
dagor_position.PARK_ALTAZ)
quick = True
stop_on_target = True
elif args['altaz']:
track = True if args['track'] else False
stop_on_target = not track
altaz_end = {
'alt': parse_degrees(args['<ALT>']),
'az': parse_degrees(args['<AZ>']),
}
if chirality is None:
chirality = dagor_position.CHIRAL_CLOSEST
target_celest = dagor_position.altaz_to_celest(altaz_end)
elif args['local']:
track = True if args['track'] else False
stop_on_target = not track
local_end = {
'ha': parse_hours(args['<HA>']),
'de': parse_degrees(args['<DE>']),
}
target_celest = dagor_position.local_to_celest(local_end)
elif args['celest'] or args['stellarium']:
if args['celest']:
target_celest = {
'ra': parse_hours(args['<RA>']),
'de': parse_degrees(args['<DE>']),
}
elif args['stellarium']:
# read stdin:
stellarium_ra_dec = None
target_prefix = "RA/Dec ({}): ".format(
configuration.TRACKING["stellarium_mode"])
print("Paste object info from"
" Stellarium then press Enter twice:")
while True:
input_ = raw_input().strip()
if input_ == '':
break
if input_.startswith(target_prefix):
stellarium_ra_dec = input_[len(target_prefix):]
if not stellarium_ra_dec:
sys.stderr.write(
'No line starts with "{}" in the pasted data.\n'
.format(target_prefix))
exit(1)
stellarium_ra, stellarium_de = stellarium_ra_dec.split('/', 1)
target_celest = {
'ra': parse_hours(stellarium_ra),
'de': parse_degrees(stellarium_de),
}
track = False if args['notrack'] else True
stop_on_target = not track
elif args['internal']:
internal_end = {
'ha': args['<int_HA>'],
'de': args['<int_DE>'],
}
target_celest = dagor_position.internal_to_celest(internal_end)
track = True if args['track'] else False
stop_on_target = True
elif args['this']:
# stat tracking current coordinates
track = True
stop_on_target = False
quick = False
target_celest = None
elif args['run']:
# stat tracking current coordinates
only_run = True
# start track console:
if only_run:
dagor_track.run()
else:
dagor_track.speed_tracking(
target_celest,
chirality=chirality,
target_is_static=not track,
stop_on_target=stop_on_target,
rough=quick,
force=args['force'],
)
return
if args['sync'] and args['console']:
dagor_track.sync_console()
return
if args['stop']:
dagor_motors.init()
dagor_motors.stop()
return
if args['manual']:
dagor_motors.set_manual()
return
if args['set']:
if args['celest']:
if args['<RA_DE>']:
rade = args['<RA_DE>']
try:
rade = rade.replace('RA ', '').replace(' Dec ', '')
arg_ra, arg_de = rade.split(',')
except Exception:
raise ValueError('Cannot parse format')
else:
arg_ra = args['<RA>']
arg_de = args['<DE>']
ra = parse_hours(arg_ra)
de = parse_degrees(arg_de)
dagor_position.set_internal(
dagor_position.celest_to_internal(
{'ra': ra, 'de': de}),
args['blind'])
if args['altaz']:
arg_alt = args['<ALT>']
arg_az = args['<AZ>']
alt = parse_degrees(arg_alt)
az = parse_degrees(arg_az)
chirality = None
if args['ce']:
chirality = dagor_position.CHIRAL_E
if args['cw']:
chirality = dagor_position.CHIRAL_W
dagor_position.set_internal(
dagor_position.altaz_to_internal(
{'alt': alt, 'az': az},
chirality
),
args['blind'])
return
if args['focus']:
if args['get']:
print(dagor_focus.get_position())
elif args['set']:
dagor_focus.set_position(args['<N>'])
elif args['goto']:
dagor_focus.goto(args['<N>'])
elif args['rehome']:
dagor_focus.rehome()
return
if args['motors']:
if args['reset']:
dagor_motors.reset(args['ha'], args['de'])
if args['status']:
dagor_motors.status_str(args['ha'] or None, args['de'] or None)
return
if args['dome']:
if args['up']:
dagor_dome.dome_up()
if args['down']:
dagor_dome.dome_down()
if args['stop']:
dagor_dome.dome_stop()
if args['open']:
dagor_dome.dome_open()
if args['close']:
dagor_dome.dome_close()
return
if args['lights']:
n = None
if args['0']:
n = 0
elif args['1']:
n = 1
elif args['2']:
n = 2
elif args['3']:
n = 3
if n is None:
print(dagor_lights.get_lights())
else:
dagor_lights.set_lights(n)
return
if args['fans']:
n = None
if args['0']:
n = 0
elif args['1']:
n = 1
elif args['2']:
n = 2
if n is None:
print(dagor_fans.get_fan(1))
else:
dagor_fans.set_fan(1, n)
return
def go(given_angle, given_distance): angle = given_angle distance = given_distance curr_time = time.time() next_state = False #constants in feet diameter = 11.25 wheel_circumference = 16.72 #right is master, left is slave global slave_power global master_power master_power = .25 slave_power = -.25 global right_num_revs right_num_revs= 0 global left_num_revs left_num_revs = 0 global kp kp = .5 #distance modifier d_mod = .95 turns= angle/360.0*math.pi*diameter/wheel_circumference dist = distance/wheel_circumference encoders.init() encoders.clear() motors.init() global en_right global en_left en_left, en_right = encoders.read() while True: try: if(abs(right_num_revs)+abs(left_num_revs))/2.0 >= turns*d_mod and not next_state: print slave_power print master_power curr_time=time.time() next_state = True if time.time()>(curr_time + 0) and next_state: break motors.speed(slave_power, master_power) adjustMotorPowers() readEncoder() except KeyboardInterrupt: break motors.speed(0,0) time.sleep(1) kill_constant = 5 right_num_revs = 0.0 left_num_revs = 0.0 slave_power = 0.233 master_power = 0.25 while True: if (abs(right_num_revs)+abs(left_num_revs))/2.0 >= dist*d_mod: break try: if(abs(left_num_revs)-abs(right_num_revs)) > kill_threshold: master_power += .95*master_power + .05*kill_constant print(str(right_num_revs)+" "+str(left_num_revs)) motors.speed(slave_power, master_power) adjustMotorPowers_straight() readEncoder_straight() print "Slave Speed: "+str(slave_power), "Master Speed: "+str(master_power) except KeyboardInterrupt: break motors.cleanup()
