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 sense_and_act(self):
# Antar vi får inn en RGB = (R, G, B)
value = self.sensobs.value
red = value[0] - value[1] - value[2]
degree = value / 255 # 255 er veeeldig rød
# Da vil vi at roboten skal rygge
self.motor_recommendations = motors.forward(
) # Kan feks sette farten til maks sånn at vi kjører den røde gjenstanden ned
# Evt halt-req
return degree
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 sense_and_act(self):
# Value er en array
value = self.sensobs.value
maks = 300
index = -1
for number in value:
if number > maks:
maks = number
index = value[number]
degree = maks / 2000 # 2000 skal den få veldig høy degree
# Da vil vi at roboten skal rygge
if index == 0:
self.motor_recommendations = motors.right()
elif index == 5:
self.motor_recommendations = motors.left()
elif index == -1:
self.motor_recommendations = motors.forward()
else:
self.motor_recommendations = motors.backward()
# Evt halt-req
return degree
time.sleep(0.33)
stop()
# while time.time() < turningTime:
# if isRight != True:
# print("ssssss")
# print("23234324")
# if isRight != True:
# turnRight()
# 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)
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)
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 forward(delay, throttle, methods=['GET']):
delay = int(delay)
throttle = int(throttle)
motors.forward(delay, throttle)
return jsonify({})
import RPi.GPIO as GPIO
import time
import motors
import ultrasound
import sound
GPIO.setmode(GPIO.BCM)
# Main loop for polling dogbot sensors
##motors.reset()
GPIO.setup(21, GPIO.IN)
print (GPIO.input(21))
while(True):
# Checks to see if switch to toggle freemode is on
freeMode = (GPIO.input(21) == 1)
while(freeMode):
if(ultrasound.getDistance() < 80.0):
motors.backup(.5)
motors.turnRight(.364)
else:
motors.forward(.5)
freeMode = (GPIO.input(21) == 1)
if(ultrasound.getDistance() < 20.0):
motors.turnLeft(.7234)
sound.bark()
motors.GPIO.cleanup()
ultrasound.GPIO.cleanup()
exit()
import motors
import db
motors.init()
motors.forward(1)
motors.pivot_left(1)
motors.reverse(1)
motors.pivot_right(1)
motors.forward(1)
conn = db.MyDB()
data = conn.fetch_row("SELECT VERSION()")
print "here:"+data[0]
print("distance in counts", distance_in_counts)
while True:
print("entered the while loop")
#orientation_Adjustment =
#orientation_Adjustment()
if us.avgDistance_front() >= 20:
orientation_Adjustment()
print("average distanve>= 20")
#motors.forward()
#elif us.avgDistance_front()<20:
print("average distanve < 20")
measure_angle = compass.readcompasscalibrated()
if (str(type(measure_angle)) != "<type 'NoneType'>"):
X_0 += ss.getDistance() * math.cos(measure_angle -
theta_required)
Y_0 += ss.getDistance() * math.sin(measure_angle -
theta_required)
motors.stop()
time.sleep(0.1)
print("now we go left")
motors.left()
print("then check for obstacle")
check_for_obstacle()
ss.startTrackingDistance()
motors.forward()
distance_in_counts = (N * math.sqrt((Y_F - Y_0)**2 +
(X_F - X_0)**2)) / cir
ss.setArrival((distance_in_counts + 10))
