NikolajDanger/Roboteksperimentarium
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

✨

Browse Source
This commit is contained in:
NikolajDanger
2022-11-02 14:14:22 +01:00
parent 9d8e95d711
commit 21236301e0
1 changed files with 3 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
rally2.py
View File

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