diff --git a/crazyswarm_demos/crazyswarm_demos/helix.py b/crazyswarm_demos/crazyswarm_demos/helix.py index 062d8978a..5a65a54f3 100644 --- a/crazyswarm_demos/crazyswarm_demos/helix.py +++ b/crazyswarm_demos/crazyswarm_demos/helix.py @@ -11,13 +11,13 @@ TAKEOFF_DURATION = 2.0 HOVER_DURATION = 2.5 -X_MIN = 0.25 -X_MAX = 2.25 -DIRECTION = 1 +X_MIN = -8.0 +X_MAX = 1.5 +DIRECTION = -1 TIMESTEP = 1 / 10 -RADIUS = 2.0 -PERIOD = 60 -DURATION = 60.0 +RADIUS = 0.75 +PERIOD = 6 * np.pi +DURATION = 30.0 SPEED = np.abs(X_MAX - X_MIN) / DURATION np.random.seed(42) @@ -27,7 +27,7 @@ def main(): swarm = Crazyswarm() timeHelper = swarm.timeHelper allcfs = swarm.allcfs - allcfs.crazyflies[0].setLEDColor(0, 0, 0) + b = 0 colors_of_rgb = ( [255, 0, 0], @@ -36,11 +36,11 @@ def main(): [0, 255, 255], [255, 0, 255], ) - # for cf in allcfs.crazyflies: - # cf.setLEDColor( - # r=colors_of_rgb[b][0], g=colors_of_rgb[b][1], b=colors_of_rgb[b][2] - # ) - # b = (b + 1) % len(allcfs.crazyflies) + for cf in allcfs.crazyflies: + cf.setLEDColor( + r=colors_of_rgb[b][0], g=colors_of_rgb[b][1], b=colors_of_rgb[b][2] + ) + b = (b + 1) % len(allcfs.crazyflies) if DIRECTION < 0: x_offset = X_MAX @@ -48,18 +48,9 @@ def main(): x_offset = X_MIN # Helix moving from positive x to negative x - # hypotrochoid(groupState, 8/8, 0.9, 2/8, 0.6) - # R = 3 - # r = 2.8 - # d = 0.6 - # s = 0.75 - R = 2 - r = 0.25 - d = 0.25 - s = 2 - fz = lambda t: DIRECTION * t * SPEED + x_offset - fy = lambda t: 1 * np.sin(t / 8) + 0.25 * np.sin(t * 2.5) - fx = lambda t: 1 * np.cos(t / 8) + 0.25 * np.cos(t * 2.5) + fx = lambda t: DIRECTION * t * SPEED + x_offset + fy = lambda t: RADIUS * np.sin(t / (PERIOD / (2 * np.pi))) + fz = lambda t: RADIUS * np.cos(t / (PERIOD / (2 * np.pi))) + 1.25 # Sort CFs by decreasing X coordinate, ties are broken with a random number sorted_cfs = reversed( @@ -74,11 +65,11 @@ def main(): for p, r, cf in sorted_cfs: cfs.append(cf) allcfs.crazyflies = cfs - # hypotrochoid(groupState, 7.5, 4, 3, 0.035) + # Evenly space crazyflies around the circle, if many crazyflies are used, an x offset might be necessary period_offset = np.linspace(0, PERIOD, len(allcfs.crazyflies), endpoint=False) starting_positions = [ - (fx(offset), fy(offset), fz(0), 0) for offset in period_offset + (fx(0), fy(offset), fz(offset), 0) for offset in period_offset ] allcfs.takeoff(targetHeight=1.0, duration=2.0) @@ -88,51 +79,56 @@ def main(): for cf, pos, c in zip(allcfs.crazyflies, starting_positions, colors): # line 56 ERROR "Crazyflie object has no attribute 'set LED'" # cf.setLED(*c) - cf.goTo(pos, 0, 3.0) + cf.goTo(pos, 0, 5.0) timeHelper.sleep(2.0) timeHelper.sleep(4.0) - for cf in allcfs.crazyflies: - cf.setLEDColor(255, 0, 0) t = 0 while t <= DURATION + 1.0: for cf, offset in zip(allcfs.crazyflies, period_offset): if t <= DURATION: - pos = (fx(t + offset), fy(t + offset), fz(t)) + pos = (fx(t), fy(t + offset), fz(t + offset)) else: - end_pos = (fx(DURATION + offset), fy(DURATION), fz(DURATION + offset)) + end_pos = (fx(DURATION), fy(DURATION + offset), fz(DURATION + offset)) pos = end_pos cf.cmdPosition(pos) t += TIMESTEP timeHelper.sleepForRate(1 / TIMESTEP) # Return to starting positions - # t = 0 - # while t <= 7.5: - # for i, cf in enumerate(allcfs.crazyflies): - # if t <= i * 0.75: - # pos = ( - # fx(DURATION), - # fy(DURATION + period_offset[i]), - # fz(DURATION + period_offset[i]), - # ) - # else: - # end_pos = ( - # fx(DURATION), - # fy(DURATION + period_offset[i]), - # fz(DURATION + period_offset[i]), - # ) - # init_pos = np.array(cf.initialPosition) + np.array([0, 0, 1.0]) - # pos = (1 - t / 7.5) * np.array(end_pos) + (t / 7.5) * np.array(init_pos) - # cf.cmdPosition(pos) - # t += TIMESTEP - # timeHelper.sleepForRate(1 / TIMESTEP) + t = 0 + while t <= 7.5: + for i, cf in enumerate(allcfs.crazyflies): + if t <= i * 0.75: + pos = ( + fx(DURATION), + fy(DURATION + period_offset[i]), + fz(DURATION + period_offset[i]), + ) + else: + end_pos = ( + fx(DURATION), + fy(DURATION + period_offset[i]), + fz(DURATION + period_offset[i]), + ) + init_pos = np.array(cf.initialPosition) + np.array([0, 0, 1.0]) + pos = (1 - t / 7.5) * np.array(end_pos) + (t / 7.5) * np.array(init_pos) + cf.cmdPosition(pos) + t += TIMESTEP + timeHelper.sleepForRate(1 / TIMESTEP) # Land + t = 0 + while t <= 3.0: + for i, cf in enumerate(allcfs.crazyflies): + end_pos = np.array(cf.initialPosition) + np.array([0, 0, 1.0]) + init_pos = np.array(cf.initialPosition) + pos = (1 - t / 3.0) * end_pos + (t / 3.0) * init_pos + cf.cmdPosition(pos) + t += TIMESTEP + timeHelper.sleepForRate(1 / TIMESTEP) for cf in allcfs.crazyflies: - cf.notifySetpointsStop() - cf.setLEDColor(0, 0, 0) - cf.land(0.04, 2.5) + cf.stop() print("Landed!")