diff --git a/rally2.py b/rally2.py index 7cd2e40..9a2b374 100644 --- a/rally2.py +++ b/rally2.py @@ -81,7 +81,7 @@ def look_around(noah, particles, cam, est_pose): time.sleep((15 * TURN_T)/1000) noah.stop() for p in particles: - p.setTheta(p.theta + np.deg2rad(15)) + p.setTheta(p.theta - np.deg2rad(15)) # The estimate of the robots current pose est_pose = particle.estimate_pose(particles) @@ -97,7 +97,7 @@ def turn_towards_landmark(noah, particles, est_pose, landmark): angle = np.rad2deg(np.arctan(relative_pos[1]/relative_pos[0])) turn_angle = np.mod(angle - (np.rad2deg(current_theta) - 180), 360) for p in particles: - p.setTheta(p.theta + np.deg2rad(turn_angle)) + p.setTheta(p.theta - np.deg2rad(turn_angle)) if turn_angle < 180: noah.go_diff(POWER, POWER, 0, 1) time.sleep((abs(turn_angle) * TURN_T)/1000) @@ -273,7 +273,7 @@ def turn_90_degrees(noah, est_pose, particles): if clockwise: p.setTheta(p.theta - np.deg2rad(90)) else: - p.setTheta(p.theta - np.deg2rad(90)) + p.setTheta(p.theta + np.deg2rad(90)) particle.add_uncertainty(particles, SIGMA, SIGMA_THETA) _, est_pose, particles = drive_until_stopped(noah, particles) return est_pose, particles