def reroute(changepin):
changePin = int(changepin) #changepin to int
if changePin == 1:
motors.frontleft()
elif changePin == 2:
motors.front()
elif changePin == 3:
motors.frontright()
elif changePin == 4:
motors.left()
elif changePin == 5:
motors.stop()
elif changePin == 6:
motors.right()
elif changePin == 7:
motors.backleft()
elif changePin == 8:
motors.back()
elif changePin == 9:
motors.backright()
else:
motors.stop()
response = make_response(redirect(url_for('index')))
return (response)
def run (self):
while True:
GPIO.output(trig, False)
time.sleep(1)
GPIO.output(trig, True)
time.sleep(0.00001)
GPIO.output(trig, False)
while GPIO.input(echo) == 0:
signaloff = time.time()
while GPIO.input(echo) == 1:
signalon = time.time()
timepassed = signalon - signaloff
distance = timepassed * 17000
last3.append(distance)
avg = round(sum(last3) / len(last3))
pingfile = open('/var/www/crest/pingfile.txt', 'w')
pingfile.write(str(avg))
pingfile.close()
if distance > 10 or avg > 10:
# sys.stdout.write("[PING] No obstructions: %.1f" % avg + "cms\r")
# sys.stdout.flush()
print colored("[PING] No obstructions: %.1f" % avg + "cms\r", 'green')
else:
# sys.stdout.write("[PING] PAY ATTENTION: %.1f" % avg + "cms\r")
# sys.stdout.flush()
print colored("[PING] PAY ATTENTION: %.1f" % avg + "cms\r", 'red')
motors.stop()
def check_for_obstacle():
print("now we're in the check_for_obstacle function")
distance_measured = us.avgDistance_front()
while True:
motors.left()
time.sleep(0.5)
if (us.avgDistance_front() - distance_measured) > 0:
motors.stop()
break
return
def CountParking():
if( (not LastParking) and ParkingPin):
ParkingCount++
if(ParkingCount == TargetParkingCount[ParkingIterator]):
stop()
ParkingCount = 0
time.sleep(ParkingDelay)
ParkingIterator++
LastParking = ParkingPin
def turnCycle():
print("assad")
# stop()
# time.sleep(2)
# forward()
# time.sleep(0.5)
stop()
time.sleep(1)
turnRight()
time.sleep(0.33)
stop()
def turnCycle(turn):
# Stop car
stop()
# Wait for 2 second
time.sleep(1)
# Turn car, according to args passed
while time.time() < turningTime:
turn()
break
# Stop car
stop()
def tick():
#front proximity trigger (the higher the number the farther it is)
if proximity.get_value() < 150:
print("Front proximity trigger!!!")
if motors.is_moving_forward():
motors.stop()
#rear proximity trigger (the higher the number the closer it is)
if sensors.get_rear_proximity() > 5:
print("Rear proximity trigger!!!")
if motors.is_moving_backward():
motors.stop()
def detect_lines(frame):
#print ('lines')
if frame == 0:
pixy.change_prog ("line")
line_get_all_features ()
i_count = line_get_intersections (100, intersections)
v_count = line_get_vectors (100, vectors)
try:
max_vectors = filter_vectors(vectors, v_count)
if max_vectors:
move(degrees(get_angle(add_vectors(max_vectors))))
except KeyboardInterrupt:
stop()
def orientation_Adjustment():
global theta_required
if (us.avgDistance_front() < 20):
motors.stop()
return
print("you're now in the function orientation adjustment")
dis = math.sqrt((Y_F - Y_0)**2 + (X_F - X_0)**2)
slope = abs((Y_F - Y_0) / (X_F - X_0))
print(" the slope is ", slope)
theta_required = round(math.degrees(math.atan(slope)), 0)
print("theta required=", theta_required)
#print type(theta_required)
angle = compass.readcompasscalibrated()
print "angle is: " + str(type(angle))
if (str(type(angle)) != "<type 'NoneType'>"):
angle = round(angle, 0)
if abs((angle - theta_required)) < abs((360 + theta_required - angle)):
motors.right()
print("Orie_adju>>> if..right()")
#time.sleep(1)
else:
motors.left()
print("Orie_adju>>> else..Left()")
#time.sleep(2)
#is the angle above == angle below?
#if the angle is the required angle it stops
#then it breaks and return 0 as orien_adj value.
#ultrasonic has no attribute here.
#
while True:
print "or adjust"
angle = compass.readcompasscalibrated()
if (str(type(angle)) == "<type 'NoneType'>"):
continue
angle = round(angle, 0)
print "angle is: " + str(type(angle))
if ((angle - theta_required) <= 20 or (angle - theta_required) >= -20):
#if ((angle - theta_required) == 20) or ((angle - theta_required) == -20):
#if (angle -round(theta_required, 0)==20) or(angle-round(theta_required, 0)==-20):
motors.stop()
#time.sleep(10)
print "Stop: angle = theta_required"
break
motors.forward()
return 0
def reroute(changepin):
changePin = int(changepin) #cast changepin to an int
if changePin == 1:
motors.turnLeft()
elif changePin == 2:
motors.forward()
elif changePin == 3:
motors.turnRight()
elif changePin == 4:
motors.backward()
else:
motors.stop()
response = make_response(redirect(url_for('index')))
return(response)
def reroute(changepin):
changePin = int(changepin) #cast changepin to an int
if changePin == 1:
motors.turnLeft()
elif changePin == 2:
motors.forward()
elif changePin == 3:
motors.turnRight()
elif changePin == 4:
motors.backward()
else:
motors.stop()
response = make_response(redirect(url_for('index')))
return(response)
def reroute(changepin):
changePin = int(changepin)
if changePin == 1:
motors.turnLeft()
elif changePin == 2:
motors.forward()
elif changePin == 3:
motors.turnRight()
elif changePin == 4:
motors.backward()
elif changePin == 5:
motors.stop()
else:
print("Wrong command")
response = make_response(redirect(url_for('index')))
return (response)
def correctDistance(f):
global stagnantStartTime
direction = 'stopped'
start = None
# Loops until the robot is between MIN_RANGE and MAX_RANGE
while True:
distance = piModules.getDistance() # Gets distance between cardboard and robot
if distance >= MAX_RANGE: # Forward
direction = 'forward'
if start is None:
start = time.time()
motors.forward()
elif distance <= MIN_RANGE: # Reverse
direction = 'reverse'
if start is None:
start = time.time()
motors.reverse()
else: # Robot is in "steady" range, write movement to file
end = time.time()
if direction is 'stopped':
if stagnantStartTime is None:
stagnantStartTime = time.time()
elif direction is 'forward':
if stagnantStartTime is not(None):
f.write('S:' + str(end - stagnantStartTime) + '\n')
stagnantStartTime = None
f.write('F:' + str(end - start) + '\n')
elif direction is 'reverse':
if stagnantStartTime is not(None):
f.write('S:' + str(end - stagnantStartTime) + '\n')
stagnantStartTime = None
f.write('B:' + str(end - start) + '\n')
motors.stop()
break
def resetTurning(): turningRight = False turningLeft = False motors.stop()
# isRight = True
turnTiming = 0.41
while True:
if isForward != True:
forward()
# while True:
# if Sonar():
# break
time.sleep(2)
turnCycle()
forward()
time.sleep(2)
stop()
time.sleep(1)
turnLeft()
time.sleep(turnTiming)
stop()
time.sleep(1)
forward()
time.sleep(2)
stop()
time.sleep(1)
turnLeft()
time.sleep(turnTiming)
stop()
time.sleep(1)
forward()
def stop():
motors.stop()
print "Stop"
elif args.out_of_frame or True:
state = "blind"
if len(queue) != 0:
if queue[-1][0] > 0:
motors.rotateCenter(direction=1, power=100)
time.sleep(0.01)
motors.rotateCenter(direction=1, power=50)
elif queue[-1][0] <= 0:
motors.rotateCenter(direction=-1, power=100)
time.sleep(0.01)
motors.rotateCenter(direction=-1, power=50)
else:
motors.rotateCenter(direction=1, power=80)
else:
motors.stop()
else:
motors.stop()
lastSwitchState = False
compass_correcting = 0
# DEBUG
if DEBUG_LEVEL > 0:
OutputFrames = vp.OutputFrame()
outlineCenter, outlineRadius = vp.minEnclosing()
if OutputFrames[0] is not None:
CameraOutput = OutputFrames[0]
if ballCenter is not None and ballDetected:
absoluteBallCenter = (ballCenter[0] + 160,
def exit(self, event):
motors.stop()
return
def my_callback(bump_sensor1):
m.stop()
GPIO.add_event_detect(bump_sensor1, GPIO.RISING, callback=my_callback)
GPIO.add_event_detect(bump_sensor2, GPIO.RISING, callback=my_callback)
GPIO.add_event_detect(bump_sensor3, GPIO.RISING, callback=my_callback)
GPIO.add_event_detect(bump_sensor4, GPIO.RISING, callback=my_callback)
while (1):
x = input()
if x == 'w':
m.forward()
elif x == 'a':
m.left_turn()
time.sleep(0.1)
m.stop()
elif x == 'aa':
m.left_turn()
time.sleep(0.4)
m.stop()
elif x == 'd':
m.right_turn()
time.sleep(0.1)
m.stop()
elif x == 'dd':
m.right_turn()
time.sleep(0.4)
m.stop()
elif x == 'ss':
m.reverse()
elif x == 's':
threadLock.acquire()
bb.disableRC()
bb.disableMotor()
bb.setMotorSteps(0, 0).setMotorSpeed(0, 1).setMotorDirection(0, 0);
bb.setMotorSteps(1, 0).setMotorSpeed(1, 1).setMotorDirection(1, 0);
bb.setMotorSteps(2, 0).setMotorSpeed(2, 1).setMotorDirection(2, 1);
threadLock.release()
motors = motors.Motors(bb)
motors.start()
log.info("EVE running")
class MyHTTPServer(HTTPServer):
def __init__(self, *args, **kw):
HTTPServer.__init__(self, *args, **kw)
self.bb = bb
server = MyHTTPServer(('', 80), webserver.JsonApi)
server.serve_forever()
while True:
time.sleep(1)
except (KeyboardInterrupt, SystemExit):
log.info("Shutting down...")
server.socket.close()
bb.stop()
motors.stop()
sys.exit()
def halt_tracking():
"""Stop tracking and halt the loop."""
print_("Stopping")
dagor_motors.stop()
wait_for_stop(dagor_motors.get_motor('ha'), dots=True)
wait_for_stop(dagor_motors.get_motor('de'), dots=True)
def main():
global simple_turn_coef
global double_turn_coef
global distance_from_sign
print('In main function')
if len(sys.argv) is 6:
#initialize configurable data
simple_turn_coef = float(sys.argv[2])
double_turn_coef = float(sys.argv[3])
config.PWM_FOR_TURNING = int(sys.argv[4])
config.PWM_FOR_STRAIGHT = 55
distance_from_sign = int(sys.argv[5])
if sys.argv[1] == 'start':
try:
state = 'initial'
while state in STATES:
if state == 'initial':
print("--------------Initial state--------------")
initial_setup()
print("Setup done")
sleep(2)
state = 'check_distance'
#end of initial state
elif state == 'check_distance':
print("---------Checking distance state---------")
#calculate distance from sign
remaining_distance = average_distance()
if config.PWM == 0:
if remaining_distance > (0.8 * distance_from_sign):
print(
"Car is stopped and motors are set to on")
motors.forward()
print("Car is moving")
sleep(0.2)
#while distance is less than the desired distance, keep going
while remaining_distance > distance_from_sign:
remaining_distance = distance.compute(
config.GPIO_TRIGGER_FRONT,
config.GPIO_ECHO_FRONT)
if remaining_distance < distance_from_sign:
motors.stop()
sleep(1)
break
else:
print(
"Car is stopped and sign shoud be in front"
)
state = 'check_for_sign'
#end of check_distance state
elif state == 'check_for_sign':
print("--------Checking for a sign state--------")
StartTime = time.time()
StopTime = time.time()
text = 'None'
while True:
StopTime = time.time()
ElapsedTime = StopTime - StartTime
if ElapsedTime > 5:
print("No image found")
state = 'end'
break
else:
#God knows why it has to do this in order to work
for counter in range(1, 10):
print(
str(
traffic_recognition.
findTrafficSign(
camera, lower_blue,
upper_blue)))
sleep(0.2)
print("Now it should work")
text = str(
traffic_recognition.findTrafficSign(
camera, lower_blue, upper_blue))
message = text
if text in TRAFFIC_SIGNS:
#Traffic sign found
message = 'Found sign --------- ' + text
print(message)
#compute time to spin
timer = compute_timer()
#choose direction to follow
if text == 'Turn Right':
motors.right(timer)
print(
"Car should have turned right by now"
)
state = 'check_distance'
break
elif text == 'Turn Left':
motors.left(timer)
print(
"Car should have turned left by now"
)
state = 'check_distance'
break
elif text == 'Move Straight':
print("Car should stop now")
state = 'end'
break
elif text == 'Turn Back':
timer *= double_turn_coef
motors.reverse(timer)
print(
"Car should have turned back by now"
)
state = 'check_distance'
break
else:
print(message)
sleep(1)
#end of check_for_sign state
elif state == 'end':
print("-----------------The End-----------------")
motors.stop()
motors.p1.stop()
motors.p2.stop()
GPIO.cleanup()
cv2.destroyAllWindows()
print("End of program")
sys.exit(0)
#end of end state
else:
#well this is not supposed to ever come up
print("State not implemented")
motors.stop()
motors.p1.stop()
motors.p2.stop()
GPIO.cleanup()
cv2.destroyAllWindows()
print("End of program")
sys.exit(0)
# Reset by pressing CTRL + C
except KeyboardInterrupt:
motors.stop()
motors.p1.stop()
motors.p2.stop()
GPIO.cleanup()
cv2.destroyAllWindows()
print('Autonomus driving stopped')
elif sys.argv[1] == 'testing':
initial_setup()
print("Testing")
print("simple_turn_coef is " + str(simple_turn_coef))
print("double_turn_coef is " + str(double_turn_coef))
timer = compute_timer()
motors.right(timer)
motors.stop()
sleep(2)
timer *= double_turn_coef
motors.reverse(timer)
motors.p1.stop()
motors.p2.stop()
GPIO.cleanup()
print("Test complete")
else:
print("Unknown argument")
elif sys.argv[1] == 'camera':
try:
state = 'initial'
while state in STATES:
if state == 'initial':
print("--------------Initial state--------------")
initial_setup()
print("Setup done")
sleep(2)
state = 'check_distance'
#end of initial state
elif state == 'check_distance':
print("---------Checking distance state---------")
#calculate distance from sign
remaining_distance = average_distance()
if config.PWM == 0:
if remaining_distance > (0.8 * distance_from_sign):
print("Car is stopped and motors are set to on")
print("Car is moving")
sleep(0.2)
#while distance is less than the desired distance, keep going
while remaining_distance > distance_from_sign:
remaining_distance = distance.compute(
config.GPIO_TRIGGER_FRONT,
config.GPIO_ECHO_FRONT)
if remaining_distance < distance_from_sign:
sleep(1)
break
else:
print("Car is stopped and sign shoud be in front")
state = 'check_for_sign'
#end of check_distance state
elif state == 'check_for_sign':
print("--------Checking for a sign state--------")
StartTime = time.time()
StopTime = time.time()
text = 'None'
while True:
StopTime = time.time()
ElapsedTime = StopTime - StartTime
if ElapsedTime > 5:
print("No image found")
state = 'end'
break
else:
for counter in range(1, 10):
traffic_recognition.findTrafficSign(
camera, lower_blue, upper_blue)
print("Now it should work")
text = str(
traffic_recognition.findTrafficSign(
camera, lower_blue, upper_blue))
message = text
if text in TRAFFIC_SIGNS:
#Traffic sign found
message = 'Found sign --------- ' + text
print(message)
state = 'check_distance'
break
else:
print(message)
sleep(4)
#end of check_for_sign state
elif state == 'end':
print("-----------------The End-----------------")
GPIO.cleanup()
cv2.destroyAllWindows()
print("End of program")
sys.exit(0)
#end of end state
else:
#well this is not supposed to ever come up
print("State not implemented")
GPIO.cleanup()
cv2.destroyAllWindows()
print("End of program")
sys.exit(0)
# Reset by pressing CTRL + C
except KeyboardInterrupt:
GPIO.cleanup()
cv2.destroyAllWindows()
print('Autonomus driving stopped')
else:
print("Something went way to wrong")
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 CheckForInterrupts():
while(InterruptPin == HIGH):
stop()
Motors.TakeExit(WayToGo.next)