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