Extends push demo with up/down capabilities

bitcraze · Mar 7, 2023 · 013771a · 013771a
1 parent 23af53b
commit 013771a
Show file tree

Hide file tree

Showing 2 changed files with 18 additions and 1 deletion.
diff --git 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
@@ -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);*/