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