def control_robot():
global ts, old_id
ts = time.time()
direction = request.args.get('direction', 0)
mousedown = request.args.get('mousedown', 0)
user_age = int(request.args.get('user_age', 0))
user_id = request.args.get('user_id', 0)
for robot in ROBOTS:
if user_id == robot.user_id:
if mousedown == '1':
print('button pressed:', direction)
if direction == 'up':
robot.forward()
elif direction == 'left':
robot.left()
elif direction == 'right':
robot.right()
else:
robot.reverse()
else:
robot.stop()
return jsonify(color=robot.color, endgame=0)
elif user_id == old_id:
return jsonify(color=robot.color, endgame=1)
# if above didn't return, new user:
oldest_robot = max(ROBOTS, key=lambda x: x.age())
old_id = oldest_robot.user_id
oldest_robot.user_id = user_id
oldest_robot.user_age = user_age
oldest_robot.user_time = time.time()
print("new user: {}, {}, assigned {}".format(user_id, user_age,
robot.color))
return jsonify(color=robot.color, endgame=0)
async def handle_command(websocket, path):
while True:
message = await websocket.recv()
message = literal_eval(message)
print(message)
if message['command'] == 'moveForward':
robot.forward()
answer = {'action': 'moveForward'}
elif message['command'] == 'moveBackward':
robot.backward()
answer = {'action': 'moveBackward'}
elif message['command'] == 'turnRight':
robot.right()
answer = {'action': 'turnRight'}
elif message['command'] == 'turnLeft':
robot.left()
answer = {'action': 'turnLeft'}
elif message['command'] == 'stop':
robot.stop()
answer = {'action': 'stop'}
elif message['command'] == 'grabberGrab':
# grabber action grab
answer = {'action': 'grabberGrab', 'value': message['value']}
elif message['command'] == 'grabberTilt':
# grabber action tilt
answer = {'action': 'grabberTilt', 'value': message['value']}
elif message['command'] == 'grabberHeight':
# grabber action height
answer = {'action': 'grabberHeight', 'value': message['value']}
await websocket.send(json.dumps(answer))
def execute(img):
global width
global height
global isObject
global isMoving
global start
image = cv2.resize(img, (300,300))
# compute all detected objects
detections = model(image)
# draw all detections on image
for det in detections[0]:
bbox = det['bbox']
text = str(category_map[det['label']])
cv2.rectangle(image, (int(width * bbox[0]), int(height * bbox[1])), (int(width * bbox[2]), int(height * bbox[3])), (255, 0, 0), 2)
cv2.putText(image, text, (int(width * bbox[0]), int(height * bbox[3])), cv2.FONT_HERSHEY_PLAIN, 1.0, (0, 0, 255), lineType=cv2.LINE_AA)
# select detections that match selected class label
matching_detections = [d for d in detections[0] if d['label'] == 53]
# get detection closest to center of field of view and draw it
det = closest_detection(matching_detections)
if det is not None:
bbox = det['bbox']
cv2.rectangle(image, (int(width * bbox[0]), int(height * bbox[1])), (int(width * bbox[2]), int(height * bbox[3])), (0, 255, 0), 5)
if det is None:
if isObject == True:
start = time.time()
isObject = False
if (time.time() - start) > 4:
robot.stop()
# otherwsie steer towards target
else:
if isMoving == False:
start = time.time()
isMoving = True
center = detection_center(det)
print(center)
# move robot forward and steer proportional target's x-distance from center
if center[0] > 0.2:
print('left')
robot.left()
elif center[0] < -0.2:
print('right')
robot.right()
elif center[0] > -0.2 and center[0] < 0.2:
print('forward')
robot.forward()
if (time.time() - start) > 2:
isMoving = False
return cv2.imencode('.jpg', image)[1].tobytes()
def do_POST(self):
global f
global b
global r
global l
global robot
self._set_headers()
form = cgi.FieldStorage(fp=self.rfile,
headers=self.headers,
environ={'REQUEST_METHOD': 'POST'})
print form.getvalue("forward")
print form.getvalue("backward")
print form.getvalue("left")
print form.getvalue("right")
if form.getvalue("forward") == "Forward":
if f == 0:
robot.stop()
robot.forward()
f = 1
else:
robot.stop()
f = 0
if form.getvalue("backward") == "Backward":
if b == 0:
robot.stop()
robot.backward()
b = 1
else:
robot.stop()
b = 0
if form.getvalue("left") == "Left":
if l == 0:
robot.stop()
robot.left()
l = 1
else:
robot.stop()
l = 0
if form.getvalue("right") == "Right":
if r == 0:
robot.stop()
robot.right()
r = 1
else:
robot.stop()
r = 0
file = open("index.html", "r")
page = file.read()
file.close()
self.wfile.write(page)
def move(direction):
if direction == 'forward':
robot.forward(0.5)
if direction == 'backward':
robot.backward(0.5)
if direction == 'left':
robot.left(0.5)
if direction == 'right':
robot.right(0.5)
return '{}'
def drawSquare(cm):
scaling_factor = 10
for i in range(4):
for j in xrange(4):
robot.forwards(cm/4)
time.sleep(0.1)
moveParticles(cm*scaling_factor/4)
print "drawParticles:" + str(particles)
robot.left(90)
rotateParticles(90)
def path_planning(current_loc, goal):
#need to translate our frame of reference
x_c, y_c, theta_c = current_loc #!!!!!make sure that theta_c is in degrees!
x_g, y_g = goal
d = np.sqrt((x_c-x_g)**2+(y_c-y_g)**2)
#calculate angle of goal point with respect to real frame
sin_theta = y_g/d
cos_theta = x_g/d
#theta is in radians
theta_rad = np.arccos(cos_theta)
#turn theta into degrees
theta_deg = np.rad2deg(theta_rad)
#subtract the angle robot is facing with respect to real frame from angle of goal point
#to obtain abgle the robot needs to turn to
#if this angle is negative, robot has to turn this amount right
#if angle is positive, robot has to turn this amount left
theta_to_move = theta_deg - theta_c
print theta_to_move
if theta_to_move < 0:
#turn right amount theta_to_move
print 'moving right'
robot.right(int(theta_to_move))
else:
print 'moving left'
#turn left amount theta_to_move
robot.left(int(theta_to_move))
#xnew = cos(theta)*d
#ynew = sin(theta)*d
X = abs(x_c - x_g)
Y = abs(y_c - y_g)
d = np.sqrt(X*X + Y*Y)
#after turned into correct direction, move d cm forward
robot.forwards(d)
def check_buttons():
global robot
global start_button
global select_button
if robot != None:
if device.button.up and device.button.left:
robot.upleft()
elif device.button.up and device.button.right:
robot.upright()
elif device.button.down and device.button.left:
robot.downleft()
elif device.button.down and device.button.right:
robot.downright()
elif device.button.left:
robot.left()
elif device.button.right:
robot.right()
elif device.button.up:
robot.up()
elif device.button.down:
robot.down()
else:
robot.center()
if device.button.a:
robot.a_press()
else:
robot.a_up()
if device.button.b:
robot.b_press()
else:
robot.b_up()
if device.button.start:
start_button = True
else:
if start_button:
start_button = False
toggle_robot()
if device.button.select:
select_button = True
else:
if select_button:
select_button = False
select_button = False
robot.toggle_mode()
def action(client, userdata, message):
data = message.payload.decode()
data = json.loads(data)
data = data['action']
print(data)
if data == 'forward':
print('forward')
robot.forward()
elif data == 'stop':
print('stop')
robot.stop()
elif data == 'left':
print('left')
robot.left()
elif data == 'right':
print('right')
robot.right()
elif data == 'backward':
print('backward')
robot.backward()
elif data == 'servo left':
print('servo left')
robot.servo_left()
elif data == 'servo center':
print('servo center')
robot.servo_center()
elif data == 'servo right':
print('servo right')
robot.servo_right()
elif data == 'lights':
print('lights')
robot.lights()
elif data == 'blinkers':
print('blinkers')
robot.blinkers()
elif data == 'voltage':
print('voltage')
voltage = robot.voltage()
voltage = send_json(voltage)
myMQTT.publish('from_robot', voltage, 0)
elif data == 'distance':
print('distance')
distance = robot.distance()
distance = send_json(distance)
myMQTT.publish('from_robot', distance, 0)
else:
pass
def move_div():
#if flask.request.method == 'POST':
if flask.request.form['button'] == 'forward':
print('forward!')
robot.forward()
elif flask.request.form['button'] == 'stop':
print('stop!')
robot.stop()
elif flask.request.form['button'] == 'left':
print('left')
robot.left()
elif flask.request.form['button'] == 'right':
print('right')
robot.right()
elif flask.request.form['button'] == 'backward':
print('backward')
robot.backward()
else:
pass
return flask.render_template('index.html')
def planning(current_loc, goal):
# need to translate our frame of reference
x_c, y_c, theta_c = current_loc #!!!!!make sure that theta_c is in degrees!
x_g, y_g = goal
dx = x_g - x_c
dy = y_g - y_c
d = np.sqrt(dx ** 2 + dy ** 2)
d_theta = np.rad2deg(np.arctan2(dy, dx)) - theta_c
if d_theta > 180:
d_theta -= 360
elif d_theta < -180:
d_theta += 360
robot.left(d_theta)
robot.forwards(d)
return (x_g, y_g, d_theta + theta_c)
def findLineCtrl(self, posInput, setCenter): #2
if not posInput:
robot.robotCtrl.moveStart(speedMove, 'no', 'no')
return
if posInput > (setCenter + findLineError):
#turnRight
robot.right()
self.CVCommand = 'Turning Right'
print('Turning Right')
elif posInput < (setCenter - findLineError):
#turnLeft
robot.left()
self.CVCommand = 'Turning Left'
print('Turning Left')
else:
#forward
robot.forward()
self.CVCommand = 'Forward'
print('Forward')
def navigateToWaypoint(X,Y):
#assuming robot is at (0,0,0)
#xnew = cos(theta)*d
#ynew = sin(theta)*d
d = np.sqrt(X*X + Y*Y)
sin_theta = Y/d
cos_theta = X/d
#theta is in radians
theta_rad = np.arccos(cos_theta)
#turn theta into degrees
theta_deg = theta_rad * 360.0 / (2.0* np.pi)
#call function that rotates robot by theta_deg degrees
robot.left(theta_deg)
#call function to move robot straight for d cm
robot.forwards(d)
except:
pass
#oba=60
print("obstacle at: {}".format(oba))
print("visible at: {}".format(obv))
#thoorama irruku'
if ((x <= 400) and (x >= 200)):
#desti is straight but check for foreign objs
if ((oba < 50) or (oba > 150)):
#go straight
robot.forward(0.5)
elif (x > 290):
robot.left(0.55)
robot.forward(2.5)
robot.right(0.55)
elif (x <= 290):
robot.right(0.55)
robot.forward(2.5)
robot.left(0.55)
elif (x > 400):
robot.right(0.1)
elif (x < 200):
robot.left(0.1)
elif (h > 100):
robot.stopp()
## eyes = eye_cascade.detectMultiScale(roi_gray)
## print('x:{}'.format(x))
cv2.rectangle(img, (x, y), (x + w, y + h), (255, 255, 0), 2)
roi_gray = gray[y:y + h, x:x + w]
roi_color = img[y:y + h, x:x + w]
if (h < 180):
ser.flushInput()
sleep(0.01)
oba = ser.readline()
print("oba: {}".format(oba))
#thoorama irruku'
if ((x <= 300) and (x >= 200)):
#desti is straight but check for foreign objs
if ((oba < 70) or (oba > 110)):
#go straight
robot.forward(0.5)
elif (x > 255):
robot.left(0.5)
elif (x <= 255):
robot.right(0.5)
elif (x > 300):
robot.right(0.5)
elif (x < 200):
robot.left(0.5)
elif (h > 180):
robot.stopp()
## eyes = eye_cascade.detectMultiScale(roi_gray)
## print('x:{}'.format(x))
## if(x==0):
## robot.stopp()
## elif(x<200):
print('Waiting for data...')
data = s.recv(2048)
if not data:
s.close()
state = False
print(data.decode())
if data == 'forward':
print('forward!')
robot.forward()
elif data == 'stop':
print('stop!')
robot.stop()
elif data == 'left':
print('left')
robot.left()
elif data == 'right':
print('right')
robot.right()
elif data == 'backward':
print('backward')
robot.backward()
elif data == 'servo left':
print('servo left')
robot.servo_left()
elif data == 'servo center':
print('servo center')
robot.servo_center()
elif data == 'servo right':
print('servo right')
robot.servo_right()
import robot ''' We import robot. This lets us call all of the functions in robot.py. We set robot_debug to True to test our code without sending it to the robot. Set robot_debug to False when you are ready to run your code on the robot. ''' robot.robot_debug = False robot.robot_speed = 200 robot.forward(5.0) robot.forward(1.0) robot.left(1.5) robot.right(1.0) robot.blink(1.0) robot.forward(4.5) # # robot.robot_speed = 125 # robot.forward(3) # robot.right_rot(.7) # robot.forward(2.5) # robot.left_rot(.7) # robot.forward(5) # robot.forward(4) # robot.left_rot(.7) # robot.forward(3) # # robot.forward(3) # robot.right_rot(.7)
import robot ''' We import robot. This lets us call all of the functions in robot.py. We set robot_debug to True to test our code without sending it to the robot. Set robot_debug to False when you are ready to run your code on the robot. ''' robot.robot_debug = False robot.robot_speed = 150 robot.forward(1.0) robot.forward(1.0) robot.left(1.0) robot.right(1.0) robot.blink(10) robot.forward(1.0) # # robot.robot_speed = 125 # robot.forward(3) # robot.right_rot(.7) # robot.forward(2.5) # robot.left_rot(.7) # robot.forward(5) # robot.forward(4) # robot.left_rot(.7) # robot.forward(3) # # robot.forward(3) # robot.right_rot(.7)
import robot
robot.pause_for(2.00) # s
for count in range(4):
robot.forward(50.0) # cm
robot.left(90.0) # degrees
robot.send_command_list()
import robot ''' We import robot. This lets us call all of the functions in robot.py. We set robot_debug to True to test our code without sending it to the robot. Set robot_debug to False when you are ready to run your code on the robot. ''' robot.robot_debug = False robot.robot_speed = 150 robot.forward(1.0) robot.forward(1.0) robot.left(1.0) robot.right(1.0) robot.blink(10) robot.forward(1.0) # # robot.robot_speed = 125 # robot.forward(3) # robot.right_rot(.7) # robot.forward(2.5) # robot.left_rot(.7) # robot.forward(5) # robot.forward(4) # robot.left_rot(.7) # robot.forward(3) # # robot.forward(3) # robot.right_rot(.7)
def update_motor_powers():
power_l = limit_value((current_y / 32768 * 100) + (current_x / 32768 * 100))
power_r = limit_value((current_y / 32768 * 100) - (current_x / 32768 * 100))
robot.left (power_l)
robot.right(power_r)
import robot print "forward" robot.forward(5) print "right" robot.right(2) print "left" robot.left(2) print "reverse" robot.reverse(2) print "stop"
