From fde6592dec8610aa55cdba42e087b39cbe12ac53 Mon Sep 17 00:00:00 2001
From: Ulysse Cura <ulysse.cura@gmail.com>
Date: Sun, 19 Jan 2025 16:25:30 +0100
Subject: [PATCH] =?UTF-8?q?Ben,=20=C3=A7a=20marche=20pas?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 IO.hpp     | 275 ++++++++++++++++++++++++++++++++++++++++++++++++-----
 Images.hpp |  64 +++++++++++++
 Main.ino   | 159 +++++++++++++++++--------------
 3 files changed, 402 insertions(+), 96 deletions(-)
 create mode 100644 Images.hpp

diff --git a/IO.hpp b/IO.hpp
index 63d0f10..5497a14 100644
--- a/IO.hpp
+++ b/IO.hpp
@@ -1,33 +1,91 @@
+#include "esp32-hal.h"
+#include <CodeCell.h>
+#include <ESP32Servo.h>
+#include <Adafruit_SSD1306.h>
+#include <cmath>
+
 #define PIN_TIRETTE 7
+#define PIN_BUTTON_COLOR 6
+#define PIN_MOTOR1 1
+#define PIN_MOTOR2 2
+#define PIN_SERVO 5
 
-class Motor;
+#define DANCING_ACTION_DELTA_ANGLE 2
 
-class Caca {
-  Caca(int quatitee) : kg(quantitee)
-  {}
+#define KD 100
+#define ANGULAR_SPEED 45 // °/s
 
-  int kg;
+enum class Axes {
+  X,
+  Y,
+  Z
 };
 
-Caca my_caca {21};
-
-class IO {
+class Motor {
  public:
-  int init()
+  int init(int pin)
   {
-    pinMode(PIN_TIRETTE, INPUT_PULLUP);
+    m_pin = pin;
 
-    for(int i {0}; i < 2; i++)
-    {
-      motors[i].init();
-    }
+    pinMode(m_pin, OUTPUT);
+
+    analogWriteFrequency(m_pin, 5000);
+    analogWriteResolution(m_pin, 12);
+
+    analogWrite(m_pin, 0);
 
     return 0;
   }
 
+  void setSpeed(int speed)
+  {
+    Serial.printf("Motor pin : %d\n", m_pin);
+    analogWrite(m_pin, speed);
+  }
+
+ private:
+  int m_pin;
+  int m_channel;
+};
+
+class IO {
+ public:
+  IO(CodeCell *code_cell) : m_code_cell(code_cell)
+  {}
+
+  int init()
+  {
+    int nb_errors {0};
+
+    pinMode(PIN_TIRETTE, INPUT_PULLUP);
+    pinMode(PIN_BUTTON_COLOR, INPUT_PULLUP);
+
+    m_code_cell->Motion_RotationRead(m_init_x, m_init_y, m_init_z);
+
+    m_initMotors();
+    m_initServo();
+
+    nb_errors += m_initScreen();
+
+    return nb_errors;
+  }
+
   int update()
   {
-    bool m_is_tirette_pulled = (digitalRead(PIN_TIRETTE) == LOW);
+    m_is_tirette_pulled = (digitalRead(PIN_TIRETTE) == LOW);
+    m_is_color_blue = (digitalRead(PIN_BUTTON_COLOR) == HIGH);
+
+    if(m_is_motor_control_activated)
+    {
+      m_updateMotorsControl();
+    }
+
+    if(m_is_dancing)
+    {
+      m_updateDancingAction();
+    }
+
+    m_updateScreen();
 
     return 0;
   }
@@ -37,16 +95,183 @@ class IO {
     return m_is_tirette_pulled;
   }
 
- private:
-  bool m_is_tirette_pulled {false};
-
-  Motor motors[2];
-};
-
-class Motor {
-  int init()
+  bool isColorBlue()
   {
-    
+    return m_is_color_blue;
+  }
+
+  void motorControlOn()
+  {
+    m_is_motor_control_activated = true;
+  }
+
+  void motorControlOff()
+  {
+    m_is_motor_control_activated = false;
+
+    m_motors[0].setSpeed(0);
+    m_motors[1].setSpeed(0);
+  }
+
+  void setDir(float dir)
+  {
+    static unsigned long prev_time {millis()};
+    float err_dir = dir - m_dir;
+    float max_angular_displacement = ANGULAR_SPEED * (millis() - prev_time);
+
+    if(std::abs(err_dir) < max_angular_displacement)
+    {
+      m_dir = dir;
+    }
+    else
+    {
+      m_dir += max_angular_displacement * (err_dir < 0) ? -1 : 1;
+    }
+
+    prev_time = millis();
+  }
+
+  void setSpeed(float speed)
+  {
+    m_speed = speed;
+  }
+  
+  float getAngle(Axes axis)
+  {
+    float x, y, z;
+
+    m_code_cell->Motion_RotationRead(x, y, z);
+
+    switch(axis)
+    {
+     case Axes::X:
+      return x - m_init_x;
+
+     case Axes::Y:
+      return y - m_init_y;
+
+     default:
+      return z - m_init_z;
+    }
+  }
+
+  void startDancingAction()
+  {
+    m_is_dancing = true;
+  }
+
+  Adafruit_SSD1306 *getScreen()
+  {
+    return &m_screen;
+  }
+
+  Motor m_motors[2];
+
+ private:
+  // Init Motors
+  void m_initMotors()
+  {
+    m_motors[0].init(PIN_MOTOR1);
+    m_motors[1].init(PIN_MOTOR2);
+  }
+
+  // Init Servo
+  void m_initServo()
+  {
+    m_servo.setPeriodHertz(50);
+    m_servo.attach(PIN_SERVO, 500, 2400);
+
+    m_servo.write(87);
+  }
+
+  int m_initScreen()
+  {
+    if(!m_screen.begin(SSD1306_SWITCHCAPVCC, 0x3C))
+    {
+      Serial.println("SSD1306 allocation failed.");
+      return -1;
+    }
+
+    m_screen.fillScreen(SSD1306_BLACK);
+
     return 0;
   }
-};
\ No newline at end of file
+
+  void m_updateDancingAction()
+  {
+    static unsigned long prev_time {millis()};
+    static unsigned long delta_time {0};
+    static int actual_angle {87};
+    static int8_t actual_dir {-1};
+
+    delta_time += millis() - prev_time;
+
+    if(delta_time >= 10)
+    {
+      delta_time = 0;
+
+      actual_angle += actual_dir * DANCING_ACTION_DELTA_ANGLE;
+    }
+
+    if(actual_angle <= 43)
+    {
+      actual_dir = 1;
+    }
+    else if(actual_angle >= 130)
+    {
+      actual_dir = -1;
+    }
+
+    m_servo.write(actual_angle);
+
+    prev_time = millis();
+  }
+
+  void m_updateMotorsControl()
+  {
+    float actual_angle = getAngle(Axes::Z);
+    float error = m_dir - actual_angle;
+
+    float correction = error * KD;
+
+    int m1_speed = static_cast<int>(m_speed + correction);
+    int m2_speed = static_cast<int>(m_speed - correction);
+
+    if(m1_speed < 0)
+    {
+      m1_speed = 0;
+      m2_speed = static_cast<int>(m_speed - 2 * correction);
+    }
+    else if(m2_speed < 0)
+    {
+      m2_speed = 0;
+      m1_speed = static_cast<int>(m_speed + 2 * correction);
+    }
+
+    Serial.printf("M1_Speed : %d\n", m1_speed);
+    Serial.printf("M2_Speed : %d\n", m2_speed);
+
+    m_motors[0].setSpeed(m1_speed);
+    m_motors[1].setSpeed(m2_speed);
+  }
+
+  void m_updateScreen()
+  {
+    m_screen.display();
+  }
+
+  bool m_is_motor_control_activated {false};
+  bool m_is_tirette_pulled {false};
+  bool m_is_color_blue {false};
+  bool m_is_dancing {false};
+
+  Servo m_servo;
+
+  Adafruit_SSD1306 m_screen {128, 64, &Wire, -1};
+
+  CodeCell *m_code_cell;
+
+  float m_init_x, m_init_y, m_init_z;
+  float m_dir {0};
+  float m_speed {0};
+};
diff --git a/Images.hpp b/Images.hpp
new file mode 100644
index 0000000..3a90c91
--- /dev/null
+++ b/Images.hpp
@@ -0,0 +1,64 @@
+namespace Imgs
+{
+  constexpr const unsigned char riombotique [] PROGMEM = {
+    0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x0f, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x78, 0x03, 0xe0, 0x00, 
+    0x00, 0x00, 0x00, 0xc0, 0x00, 0x38, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x0c, 0x00, 0x00, 0x00, 
+    0x02, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00, 0x04, 0x00, 0x07, 0x86, 0x00, 0x00, 0x00, 0x04, 0x06, 
+    0x0f, 0xc2, 0x00, 0x00, 0x00, 0x04, 0x19, 0x98, 0x62, 0x00, 0x00, 0x00, 0x04, 0x20, 0x50, 0x22, 
+    0x00, 0x00, 0x00, 0x14, 0x20, 0x50, 0x22, 0xc0, 0x00, 0x00, 0x34, 0x20, 0x50, 0x22, 0xe0, 0x00, 
+    0x00, 0x74, 0x20, 0x58, 0x62, 0xa0, 0x00, 0x00, 0xf4, 0x10, 0x8f, 0xc2, 0xb0, 0x00, 0x00, 0xf4, 
+    0x0f, 0x07, 0x82, 0xb0, 0x00, 0x00, 0x74, 0x00, 0x00, 0x02, 0xa0, 0x00, 0x00, 0x34, 0x00, 0x00, 
+    0x02, 0xc0, 0x00, 0x00, 0x14, 0x00, 0x00, 0x02, 0x80, 0x00, 0x00, 0x04, 0x00, 0x00, 0x02, 0x00, 
+    0x00, 0x00, 0x04, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00, 0x0c, 0x00, 0x00, 0x00, 
+    0x03, 0xff, 0xff, 0xf8, 0x00, 0x00, 0x00, 0x01, 0xff, 0xff, 0xf0, 0x00, 0x00, 0x00, 0x02, 0x00, 
+    0x00, 0x0c, 0x00, 0x00, 0x00, 0x04, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00, 0x01, 
+    0x00, 0x00, 0x00, 0x08, 0x07, 0xff, 0x00, 0x80, 0x00, 0x00, 0x18, 0x07, 0xff, 0x00, 0x80, 0x00, 
+    0x01, 0x98, 0x07, 0xff, 0x00, 0xc0, 0x00, 0x03, 0xd8, 0x00, 0x00, 0x00, 0xc4, 0x00, 0x03, 0x58, 
+    0x00, 0x00, 0x00, 0xde, 0x00, 0x02, 0x38, 0x00, 0x00, 0x00, 0xfa, 0x00, 0x02, 0x38, 0x00, 0x00, 
+    0x00, 0xe2, 0x00, 0x06, 0x78, 0x00, 0x00, 0x00, 0xe1, 0x00, 0x04, 0x78, 0x00, 0x00, 0x00, 0xe1, 
+    0x00, 0x04, 0x78, 0x00, 0x00, 0x00, 0xe1, 0x00, 0x0c, 0x78, 0x00, 0x00, 0x00, 0xe1, 0x00, 0x08, 
+    0xf8, 0x00, 0x00, 0x00, 0xf0, 0x80, 0x08, 0xf8, 0x00, 0x00, 0x00, 0xf0, 0x80, 0x08, 0x98, 0x00, 
+    0x00, 0x00, 0xf0, 0x80, 0x11, 0x98, 0x00, 0x00, 0x00, 0xf0, 0x40, 0x19, 0x88, 0x00, 0x00, 0x00, 
+    0x98, 0x40, 0x3f, 0x0c, 0x00, 0x00, 0x01, 0x0c, 0xc0, 0x36, 0x04, 0x00, 0x00, 0x01, 0x07, 0xe0, 
+    0x22, 0x04, 0x00, 0x00, 0x01, 0x07, 0x20, 0x22, 0x06, 0x00, 0x00, 0x02, 0x04, 0x20, 0x62, 0x02, 
+    0x00, 0x00, 0x02, 0x04, 0x20, 0x44, 0x02, 0x00, 0x00, 0x06, 0x02, 0x30, 0x44, 0x03, 0x00, 0x00, 
+    0x04, 0x02, 0x10, 0x44, 0x01, 0x00, 0x60, 0x04, 0x02, 0x30, 0xc4, 0x01, 0x80, 0x60, 0x08, 0x03, 
+    0xf0, 0xc8, 0x01, 0x80, 0x00, 0x18, 0x03, 0xb0, 0xf8, 0x00, 0xff, 0xff, 0xf0, 0x01, 0x20, 0x98, 
+    0x00, 0x7f, 0xff, 0xe0, 0x01, 0xe0, 0x98, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x90, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xf0, 0x00, 0x00, 0x00, 0x07, 0xff, 0xff, 0xfc, 0x00, 0x00
+  };
+
+  constexpr const unsigned char poivron_robotique [] PROGMEM = {
+    0x00, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x60, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x02, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 
+    0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x82, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x42, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x21, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x21, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x21, 0x00, 0x00, 0x00, 0x01, 0xfe, 0x00, 0x21, 0x00, 0x00, 0x00, 0x06, 0x19, 
+    0x80, 0x21, 0x00, 0x00, 0x00, 0x08, 0x06, 0x60, 0x42, 0x00, 0x00, 0x00, 0x10, 0x01, 0x10, 0x42, 
+    0x00, 0x00, 0x00, 0x20, 0x00, 0x88, 0x42, 0x00, 0x00, 0x00, 0x20, 0x00, 0x44, 0x82, 0x00, 0x00, 
+    0x00, 0x40, 0x00, 0x42, 0x84, 0x00, 0x00, 0x00, 0x80, 0x01, 0xfb, 0x04, 0x00, 0x00, 0x00, 0x80, 
+    0x02, 0x06, 0x04, 0x00, 0x00, 0x01, 0x00, 0x00, 0x01, 0x86, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 
+    0x49, 0x80, 0x00, 0x02, 0x00, 0x00, 0x00, 0x28, 0x60, 0x00, 0x02, 0x00, 0x00, 0x00, 0x18, 0x10, 
+    0x00, 0x04, 0x00, 0x00, 0x00, 0x07, 0x08, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0xc8, 0x00, 0x04, 
+    0x00, 0x80, 0x00, 0x00, 0x24, 0x00, 0x08, 0x01, 0x00, 0x00, 0x00, 0x13, 0x00, 0x08, 0x01, 0x00, 
+    0x00, 0x00, 0x08, 0xc0, 0x10, 0x02, 0x00, 0x00, 0x00, 0x08, 0x20, 0x10, 0x02, 0x00, 0x00, 0x10, 
+    0x04, 0x20, 0x10, 0x04, 0x00, 0x00, 0x10, 0x04, 0x10, 0x20, 0x04, 0x00, 0x00, 0x10, 0x00, 0x10, 
+    0x20, 0x08, 0x00, 0x00, 0x20, 0x00, 0x10, 0x20, 0x08, 0x00, 0x00, 0x20, 0x00, 0x10, 0x20, 0x08, 
+    0x00, 0x00, 0x40, 0x00, 0x10, 0x40, 0x10, 0x00, 0x00, 0x48, 0x00, 0x10, 0x40, 0x10, 0x00, 0x00, 
+    0x8c, 0x00, 0x10, 0x40, 0x10, 0x00, 0x00, 0x9e, 0x01, 0x20, 0x40, 0x20, 0x00, 0x01, 0x1e, 0x03, 
+    0x20, 0x40, 0x20, 0x00, 0x01, 0x1f, 0x07, 0x40, 0x80, 0x20, 0x00, 0x02, 0x0c, 0x0e, 0x40, 0x80, 
+    0x40, 0x00, 0x02, 0x00, 0x00, 0x80, 0x80, 0x40, 0x00, 0x04, 0x00, 0x01, 0x00, 0x80, 0x40, 0x00, 
+    0x04, 0x00, 0x01, 0x00, 0x80, 0x80, 0x00, 0x08, 0x00, 0x02, 0x00, 0x80, 0x80, 0x00, 0x08, 0x3f, 
+    0x84, 0x00, 0x80, 0x80, 0x00, 0x10, 0xdf, 0xc4, 0x00, 0x80, 0x80, 0x00, 0x11, 0xdf, 0xe8, 0x00, 
+    0x81, 0x00, 0x00, 0x27, 0xff, 0xb0, 0x00, 0x81, 0x00, 0x00, 0x2f, 0xff, 0xb0, 0x00, 0x81, 0x00, 
+    0x00, 0x5f, 0xff, 0xe0, 0x00, 0x42, 0x00, 0x00, 0x5f, 0xff, 0xc0, 0x00, 0x42, 0x00, 0x00, 0xbf, 
+    0xff, 0xc0, 0x00, 0x42, 0x00, 0x00, 0xbf, 0xff, 0x80, 0x00, 0x22, 0x00, 0x01, 0x3f, 0xff, 0x00, 
+    0x00, 0x12, 0x00, 0x01, 0x3f, 0xfe, 0x00, 0x00, 0x0e, 0x00, 0x02, 0x1f, 0xfc, 0x00, 0x00, 0x02, 
+    0x00, 0x04, 0x1f, 0xf8, 0x00, 0x00, 0x01, 0x00, 0x08, 0x0f, 0xf0, 0x00, 0x00, 0x00, 0x80, 0x10, 
+    0x01, 0xe0, 0x00, 0x00, 0x00, 0x40, 0x20, 0x00, 0x80, 0x00, 0x00, 0x00, 0x3f, 0xc0, 0x03, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x30, 0x1c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0f, 0xe0, 0x00, 0x00, 0x00
+  };
+}
\ No newline at end of file
diff --git a/Main.ino b/Main.ino
index 8ff4d68..9f09442 100644
--- a/Main.ino
+++ b/Main.ino
@@ -1,14 +1,12 @@
-#include <stdexcept>
-#include <CodeCell.h>
-
 #include "IO.hpp"
+#include "Images.hpp"
 
 #define UNIT_TESTS
 
 /*
   Etapes :
    - 1ere etape : avancer jusqu'a la montée / condition : si montée detecté : etape suivante
-   - 2eme etape : avncer j'usqua la fin de la montée / condition : si fin de montée detecté : etape suivante
+   - 2eme etape : avancer j'usqua la fin de la montée / condition : si fin de montée detecté : etape suivante
    - 3eme etape : tourner de 90* / condition : si action terminé : etape suivante
    - 4eme etape : avancer jusqu'au bors du plateau / condition : si choc de fin detecté : etape suivante
    - 5eme etape : faire tourner actionneurs pour figurine
@@ -35,32 +33,55 @@ enum class State {
 
 // CodeCell implementation
 CodeCell code_cell;
-IO my_IO {};
+IO my_IO {&code_cell};
 
 void setup()
 {
   // Initialise serial
   Serial.begin(115200);
 
+  Serial.println("Starting Initialisation");
+
   // Initialise the CodeCell for angle and tap detectIOn
-  code_cell.Init(MOTION_GYRO + MOTION_TAP_DETECTOR + MOTION_ACCELEROMETER);
+  code_cell.Init(MOTION_ROTATION);
 
-  Serial.println("CACA");
+  // Initialise IO
+  int nb_errors = my_IO.init();
 
-  // Initialise
-  my_IO.init();
+  if(nb_errors)
+  {
+    Serial.printf("%d errors occured during IO init.", nb_errors);
+    while(true);
+  }
+
+  my_IO.getScreen()->drawBitmap(0, 0, Imgs::riombotique, 52, 64, WHITE);
+  my_IO.getScreen()->drawBitmap(75, 0, Imgs::poivron_robotique, 52, 64, WHITE);
+
+  //my_IO.motorControlOn();
+
+  Serial.println("Initalisation Finished");
+
+#ifdef UNIT_TESTS
+  Serial.println("UNIT_TESTS");
+#endif
 }
 
 #ifdef UNIT_TESTS
 
 void loop()
 {
-  if(code_cell.Run())
-  {
-    my_IO.update();
+  my_IO.update();
 
-    my_IO.isTirettePulled();
-  }
+  Serial.print("Is Tirette Pulled : ");
+  Serial.println(my_IO.isTirettePulled());
+
+  Serial.print("Is Color Blue : ");
+  Serial.println(my_IO.isColorBlue());
+
+  my_IO.m_motors[0].setSpeed(3200);
+  my_IO.m_motors[1].setSpeed(0);
+  //my_IO.setDir(0);
+  //my_IO.setSpeed(1024.0f);
 }
 
 #else
@@ -69,76 +90,72 @@ void loop()
 {
   static State actual_state {State::ForwardToRamp};
 
-  // Runs  every 100ms - Put your code here
-  if(code_cell.Run())
+  switch(actual_state)
   {
-    switch(actual_state)
-    {
-      case State::WaitingForTirette:
-        if(IO::ext_controls.isTirettePulled())
-        {
-          actual_state = State::WaitingTimer;
-        }
-        break;
+    case State::WaitingForTirette:
+      if(my_IO.isTirettePulled())
+      {
+        actual_state = State::WaitingTimer;
+      }
+      break;
 
-      case State::WaitingTimer:
-        static unsigned long initial_time = millis();
+    case State::WaitingTimer:
+      static unsigned long initial_time = millis();
 
-        if(millis() - initial_time >= 90000)
-        {
-          actual_state = State::ForwardToRamp;
-        }
-        break;
-      
-      case State::ForwardToRamp:
-        IO::motors.forward();
+      if(millis() - initial_time >= 90000)
+      {
+        actual_state = State::ForwardToRamp;
+      }
+      break;
+    
+    case State::ForwardToRamp:
+      my_IO.forward();
 
-        if(IO::gyro.getAngle() > 9)
-        {
-          actual_state = State::ForwardToScene;
-        }
-        break;
+      if(my_IO.getAngle() > 9)
+      {
+        actual_state = State::ForwardToScene;
+      }
+      break;
 
-      case State::ForwardToScene:
-        IO::motors.forward();
+    case State::ForwardToScene:
+      IO::motors.forward();
 
-        if(IO::gyro.getAngle() < 1)
-        {
-          actual_state = IO::ext_controls.isSelectedColorBlue() ? State::Turn90Blue : State::Turn90Yellow;
-        }
-        break;
+      if(IO::gyro.getAngle() < 1)
+      {
+        actual_state = IO::ext_controls.isSelectedColorBlue() ? State::Turn90Blue : State::Turn90Yellow;
+      }
+      break;
 
-      case State::Turn90Blue:
-        IO::motors.turnLeft();
+    case State::Turn90Blue:
+      IO::motors.turnLeft();
 
-        if(IO::gyro.getAngle() > 90)
-        {
-          actual_state = State::ForwardToSceneEdge;
-        }
-        break;
+      if(IO::gyro.getAngle() > 90)
+      {
+        actual_state = State::ForwardToSceneEdge;
+      }
+      break;
 
-      case State::Turn90Yellow:
-        IO::motors.turnRight();
+    case State::Turn90Yellow:
+      IO::motors.turnRight();
 
-        if(IO::gyro.getAngle() > 90)
-        {
-          actual_state = State::ForwardToSceneEdge;
-        }
-        break;
+      if(IO::gyro.getAngle() > 90)
+      {
+        actual_state = State::ForwardToSceneEdge;
+      }
+      break;
 
-      case State::ForwardToSceneEdge:
-        IO::motors.forward();
+    case State::ForwardToSceneEdge:
+      IO::motors.forward();
 
-        if(IO::gyro.tapDetected())
-        {
-          actual_state = State::Dancing;
-        }
-        break;
+      if(IO::gyro.getAngle() < 0)
+      {
+        actual_state = State::Dancing;
+      }
+      break;
 
-      case State::Dancing:
-        IO::ext_controls.startSpecialAction();
-        break;
-    }
+    case State::Dancing:
+      IO::ext_controls.startSpecialAction();
+      break;
   }
 }