From 013771a93e4b3e764d797be742392c9728b94a53 Mon Sep 17 00:00:00 2001
From: hmllr <hanmuell@student.ethz.ch>
Date: Tue, 7 Mar 2023 11:07:08 +0100
Subject: [PATCH] Extends push demo with up/down capabilities

---
 examples/demos/app_push_demo/readme.md  |  2 ++
 examples/demos/app_push_demo/src/push.c | 17 ++++++++++++++++-
 2 files changed, 18 insertions(+), 1 deletion(-)

diff --git a/examples/demos/app_push_demo/readme.md b/examples/demos/app_push_demo/readme.md
index bc1d2a2f97..f34592df54 100644
--- a/examples/demos/app_push_demo/readme.md
+++ b/examples/demos/app_push_demo/readme.md
@@ -14,6 +14,8 @@ When an oject (ie. an hand) has passed close to the top sensor, the Crazyflie ta
 If anything is detected on side sensors, the Crazyflie moves in the oposite direction.
 So, it is possible to push the Crazyflie around.
 
+Per default, the Crazyflie will fly on 0.2m. If something is detected closer than 10cm on both sides it will go up, if front and back (or 30cm from the top) it will go down. It will land once it is closer than 10cm to the ground.
+
 ## Build
 
 You must have the required tools to build the [Crazyflie firmware](https://github.com/bitcraze/crazyflie-firmware).
diff --git a/examples/demos/app_push_demo/src/push.c b/examples/demos/app_push_demo/src/push.c
index afa4c4c835..999a46be1c 100644
--- a/examples/demos/app_push_demo/src/push.c
+++ b/examples/demos/app_push_demo/src/push.c
@@ -77,8 +77,10 @@ static const uint16_t stoppedTh = 500;
 
 static const float velMax = 1.0f;
 static const uint16_t radius = 300;
+static const uint16_t radius_up_down = 100;
+static const float up_down_delta = 0.002f;
 
-static const float height_sp = 0.2f;
+static float height_sp = 0.2f;
 
 #define MAX(a,b) ((a>b)?a:b)
 #define MIN(a,b) ((a<b)?a:b)
@@ -132,9 +134,22 @@ void appMain()
       float b_comp = back_o * factor;
       float velFront = b_comp + f_comp;
 
+      // we want to go up when there are obstacles (hands) closer than radius_up_down on both sides
+      if(left < radius_up_down && right < radius_up_down)
+      {
+        height_sp += up_down_delta;
+      }
+
+      // we want to go down when there are obstacles (hands) closer than radius_up_down in front and back (or there is something on top)
+      if((front < radius_up_down && back < radius_up_down) || up < radius)
+      {
+        height_sp -= up_down_delta;
+      }
+
       uint16_t up_o = radius - MIN(up, radius);
       float height = height_sp - up_o/1000.0f;
 
+
       /*DEBUG_PRINT("l=%i, r=%i, lo=%f, ro=%f, vel=%f\n", left_o, right_o, l_comp, r_comp, velSide);
       DEBUG_PRINT("f=%i, b=%i, fo=%f, bo=%f, vel=%f\n", front_o, back_o, f_comp, b_comp, velFront);
       DEBUG_PRINT("u=%i, d=%i, height=%f\n", up_o, height);*/