时钟！

8By8/8By8.ino

@@ -20,7 +20,45 @@
 #define MAG_UPDATE 0x08
 #define READ_UPDATE 0x80
 
-#define DEBUG false
+#define DEBUG true
+#define TRACE false
+
+// 3 width, 4 height
+
+const uint16_t numbers[10] = {
+    0xF6F, 0x592, 0xE57, 0xE8F, 0x979, 0xF8F, 0xF3F, 0xE54, 0xFEF, 0xFCF,
+};
+
+// 8 width, 7 height
+
+const uint64_t letters[26]{
+    0x3C66667E666666, // A
+    0x7C66667C66667C, // B
+    0x3C66606060663C, // C
+    0x7C66666666667C, // D
+    0x7E60607C60607E, // E
+    0x7E60607C606060, // F
+    0x3C6660606E663C, // G
+    0x6666667E666666, // H
+    0x3C18181818183C, // I
+    0x1E0C0C0C6C6C38, // J
+    0x666C7870786C66, // K
+    0x6060606060607E, // L
+    0x63777F6B636363, // M
+    0x63737B6F676363, // N
+    0x3C66666666663C, // O
+    0x7C6666667C6060, // P
+    0x3C6666666E3C06, // Q
+    0x7C66667C786C66, // R
+    0x3C66603C06663C, // S
+    0x7E5A1818181818, // T
+    0x6666666666663C, // U
+    0x66666666663C18, // V
+    0x6363636B7F7763, // W
+    0x6363361C366363, // X
+    0x6666663C181818, // Y
+    0x7E060C1830607E, // Z
+};
 
 struct Triple {
     float x;
@@ -34,6 +72,10 @@ private:
     static volatile byte s_cDataUpdate;
 
     static void AutoScanSensor() {
+        if (DEBUG) {
+            Serial.println("Autoscan sensor");
+        }
+
         int iRetry;
 
         for (int i = 0; i < sizeof(SensorReader::c_uiBaud) /
@@ -51,7 +93,7 @@ private:
                 }
                 if (SensorReader::s_cDataUpdate != 0) {
                     Serial.print(SensorReader::c_uiBaud[i]);
-                    Serial.print(" baud find sensor\r\n\r\n");
+                    Serial.println(" baud find sensor");
                     return;
                 }
                 iRetry--;
@@ -94,7 +136,7 @@ private:
     }
 
 public:
-    static Triple angle, acceleration;
+    static Triple angle, acceleration, gyro;
 
     static bool init() {
         WitInit(WIT_PROTOCOL_NORMAL, 0x50);
@@ -109,18 +151,30 @@ public:
         }
         else {
             Serial1.begin(SensorReader::c_uiBaud[WIT_BAUD_9600]);
-            // Serial.println("9600 Baud rate modified successfully");
+            if (DEBUG) {
+                Serial.println("9600 Baud rate modified successfully");
+            }
         }
 
-        if (WitSetContent(RSW_ANGLE | RSW_ACC) != WIT_HAL_OK) {
-            Serial.println("Set send content: angle, acc Error");
+        if (WitSetContent(RSW_ANGLE | RSW_ACC | RSW_GYRO) != WIT_HAL_OK) {
+            Serial.println("Set send content: angle, acc, and GYRO Error");
             return false;
         }
+        else {
+            if (DEBUG) {
+                Serial.println("Set send content: angle, acc, and GYRO");
+            }
+        }
 
-        if (WitSetOutputRate(RRATE_2HZ) != WIT_HAL_OK) {
+        if (WitSetOutputRate(RRATE_5HZ) != WIT_HAL_OK) {
             Serial.print("Set report rate failed");
             return false;
         }
+        else {
+            if (DEBUG) {
+                Serial.println("Set report rate success");
+            }
+        }
 
         return true;
     }
@@ -138,8 +192,6 @@ public:
                     sReg[Roll + 2] / 32768.0f * 180.0f; // Unit: deg
 
                 SensorReader::s_cDataUpdate &= ~ANGLE_UPDATE;
-
-                SensorReader::s_cDataUpdate = 0;
             }
 
             if (SensorReader::s_cDataUpdate & ACC_UPDATE) {
@@ -149,9 +201,23 @@ public:
                     sReg[AX + 1] / 32768.0f * 16.0f * 9.8f;
                 SensorReader::acceleration.z =
                     sReg[AX + 2] / 32768.0f * 16.0f * 9.8f;
+                SensorReader::s_cDataUpdate &= ~ACC_UPDATE;
             }
+
+            if (SensorReader::s_cDataUpdate & GYRO_UPDATE) {
+                SensorReader::gyro.x = sReg[GX] / 32768.0f * 2000.0f;
+                SensorReader::gyro.y = sReg[GX + 1] / 32768.0f * 2000.0f;
+                SensorReader::gyro.z = sReg[GX + 2] / 32768.0f * 2000.0f;
+                SensorReader::s_cDataUpdate &= GYRO_UPDATE;
+            }
+
+            SensorReader::s_cDataUpdate = 0;
         }
     }
+
+    static float get_angle_z() {
+        return SensorReader::angle.z;
+    }
 };
 
 class Cube {
@@ -233,10 +299,13 @@ public:
             return;
         }
         brightness %= 4;
-        LED_status[z][x] =
-            (LED_status[z][x] & (~(3 << (y * 2)))) | (brightness << (y * 2));
         LED_brightness[z][x] = (LED_brightness[z][x] & (~(3 << (y * 2)))) |
                                (brightness << (y * 2));
+        if (LED_blinking_status[z][x] >> y & 1) {
+            return;
+        }
+        LED_status[z][x] =
+            (LED_status[z][x] & (~(3 << (y * 2)))) | (brightness << (y * 2));
     }
 
     static int get_status(int x, int y, int z) {
@@ -283,6 +352,136 @@ public:
             break;
         }
     }
+
+    static void draw_num(int x, int y, int z, int num, int direction,
+                         int brightness) {
+        // 0 = look along x
+        // 1 = look reverse x
+        // 2 = look along y
+        // 3 = look reverse y
+        if (num > 9 || num < 0) {
+            return;
+        }
+        switch (direction) {
+        case 0: {
+            for (int i = 0; i < 3; i++) {
+                for (int j = 0; j < 4; j++) {
+                    if ((numbers[num] >> (11 - (i + j * 3))) & 1) {
+                        Cube::set_status(x, y - i, z - j, brightness);
+                    }
+                    else {
+                        Cube::set_status(x, y - i, z - j, 0);
+                    }
+                }
+            }
+            break;
+        }
+        case 1: {
+            for (int i = 0; i < 3; i++) {
+                for (int j = 0; j < 4; j++) {
+                    if ((numbers[num] >> (11 - (i + j * 3))) & 1) {
+                        Cube::set_status(x, y + i, z - j, brightness);
+                    }
+                    else {
+                        Cube::set_status(x, y + i, z - j, 0);
+                    }
+                }
+            }
+            break;
+        }
+        case 2: {
+            for (int i = 0; i < 3; i++) {
+                for (int j = 0; j < 4; j++) {
+                    if ((numbers[num] >> (11 - (i + j * 3))) & 1) {
+                        Cube::set_status(x + i, y, z - j, brightness);
+                    }
+                    else {
+                        Cube::set_status(x + i, y, z - j, 0);
+                    }
+                }
+            }
+            break;
+        }
+        case 3: {
+            for (int i = 0; i < 3; i++) {
+                for (int j = 0; j < 4; j++) {
+                    if ((numbers[num] >> (11 - (i + j * 3))) & 1) {
+                        Cube::set_status(x - i, y, z - j, brightness);
+                    }
+                    else {
+                        Cube::set_status(x - i, y, z - j, 0);
+                    }
+                }
+            }
+            break;
+        }
+        }
+    }
+
+    static void draw_letter(int x, int y, int z, int num, int direction,
+                            int brightness) {
+        // 0 = look along x
+        // 1 = look reverse x
+        // 2 = look along y
+        // 3 = look reverse y
+        if (num > 25 || num < 0) {
+            return;
+        }
+        switch (direction) {
+        case 0: {
+            for (int i = 0; i < 8; i++) {
+                for (int j = 0; j < 7; j++) {
+                    if ((numbers[num] >> (55 - (i + j * 8))) & 1) {
+                        Cube::set_status(x, y - i, z - j, brightness);
+                    }
+                    else {
+                        Cube::set_status(x, y - i, z - j, 0);
+                    }
+                }
+            }
+            break;
+        }
+        case 1: {
+            for (int i = 0; i < 8; i++) {
+                for (int j = 0; j < 7; j++) {
+                    if ((numbers[num] >> (55 - (i + j * 8))) & 1) {
+                        Cube::set_status(x, y + i, z - j, brightness);
+                    }
+                    else {
+                        Cube::set_status(x, y + i, z - j, 0);
+                    }
+                }
+            }
+            break;
+        }
+        case 2: {
+            for (int i = 0; i < 8; i++) {
+                for (int j = 0; j < 7; j++) {
+                    if ((numbers[num] >> (55 - (i + j * 8))) & 1) {
+                        Cube::set_status(x + i, y, z - j, brightness);
+                    }
+                    else {
+                        Cube::set_status(x + i, y, z - j, 0);
+                    }
+                }
+            }
+            break;
+        }
+        case 3: {
+            for (int i = 0; i < 8; i++) {
+                for (int j = 0; j < 7; j++) {
+                    if ((numbers[num] >> (55 - (i + j * 8))) & 1) {
+                        Cube::set_status(x - i, y, z - j, brightness);
+                    }
+                    else {
+                        Cube::set_status(x - i, y, z - j, 0);
+                    }
+                }
+            }
+            break;
+        }
+        }
+    }
 };
 
 enum Operators {
@@ -487,7 +686,7 @@ public:
         char operator_chars[] = "+-*/^()";
         int operator_pos = -1;
         while (true) {
-            if (DEBUG) {
+            if (TRACE) {
                 Serial.println("A");
                 Serial.print("Operator pos: ");
                 Serial.println(operator_pos);
@@ -523,7 +722,7 @@ public:
             }
 
             // Find the operator position
-            if (DEBUG) {
+            if (TRACE) {
                 Serial.println("B");
                 Serial.print("Operator pos: ");
                 Serial.println(operator_pos);
@@ -555,7 +754,7 @@ public:
             }
 
             // Understand the number and push to stack
-            if (DEBUG) {
+            if (TRACE) {
                 Serial.println("C");
                 Serial.print("Operator pos: ");
                 Serial.println(operator_pos);
@@ -575,7 +774,7 @@ public:
             }
 
             // Understand the operator
-            if (DEBUG) {
+            if (TRACE) {
                 Serial.println("D");
                 Serial.print("Read operator: ");
                 Serial.println(current_operator.display());
@@ -618,7 +817,7 @@ public:
                 break;
             }
 
-            if (DEBUG) {
+            if (TRACE) {
                 Serial.println("E");
                 Serial.print("Operator pos: ");
                 Serial.println(operator_pos);
@@ -640,6 +839,20 @@ public:
     }
 };
 
+enum Modes {
+    CalculateAndDraw,
+    Cube,
+    Rain,
+    Clock,
+    Words,
+};
+
+enum ClockStatus {
+    Normal,
+    AdjustingHour,
+    AdjustingMinute,
+};
+
 int Cube::layer_count = 0;
 int Cube::brightness_count = 0;
 uint16_t Cube::LED_status[8][8] = {0};
@@ -652,21 +865,34 @@ double *Symbol::x_value_ptr, *Symbol::y_value_ptr;
 const uint32_t SensorReader::c_uiBaud[8] = {0,     4800,  9600,   19200,
                                             38400, 57600, 115200, 230400};
 volatile byte SensorReader::s_cDataUpdate;
-Triple SensorReader::angle, SensorReader::acceleration;
+Triple SensorReader::angle, SensorReader::acceleration, SensorReader::gyro;
 
-double x, y;
+double symbol_x, symbol_y;
 const String allowed_chars = "0123456789+-*/^()xy";
 String input = "";
 int x_offset = 0, y_offset = 0, z_offset = 0;
 double zoom = 1.0;
-float z_ref = 0.0;
+float x_ref = 0.0, y_ref = 0.0, z_ref = 0.0;
+bool waiting_for_command = true;
+
+Modes current_mode = Modes::Rain;
+bool can_change_mode = true;
+
+byte cube_size = 1;
+byte cube_step = 1;
+
+bool rain_create_new = false;
+
+int clock_hour = 0, clock_minute = 0, clock_sec = 0, clock_sec_flip = 0;
+ClockStatus clock_status;
+bool clock_waiting_for_new_command = true;
 
 void setup() {
     for (int i = 22; i < 46; i++) {
         pinMode(i, OUTPUT);
     }
 
-    pinMode(JOYSTICK_SWITCH, INPUT);
+    pinMode(JOYSTICK_SWITCH, INPUT_PULLUP);
     pinMode(JOYSTICK_VRX, INPUT);
     pinMode(JOYSTICK_VRY, INPUT);
 
@@ -674,21 +900,32 @@ void setup() {
     Timer1.setPeriod(TIME_PER_LAYER_IN_US);
 
     Serial.begin(9600);
-    Symbol::x_value_ptr = &x;
-    Symbol::y_value_ptr = &y;
+
+    if (DEBUG) {
+        Serial.println("Into setup");
+    }
+
+    Symbol::x_value_ptr = &symbol_x;
+    Symbol::y_value_ptr = &symbol_y;
 
     if (!SensorReader::init()) {
         Serial.println("Initialize sensor failed.");
     }
+    else {
+        if (DEBUG) {
+            Serial.println("Initialize sensor success.");
+        }
+    }
 
     Timer1.attachInterrupt(Cube::display);
 
     cli();
+    // timer 4 is for blinking
     TCCR4A = 0; // set entire TCCR1A register to 0
     TCCR4B = 0; // same for TCCR1B
     TCNT4 = 0;  // initialize counter value to 0
     // set compare match register for 1hz increments
-    OCR4A = 7812 / 1; // = (16*10^6) / (1*1024) - 1 (must be <65536)
+    OCR4A = 6500 / 1; // = (16*10^6) / (1*1024) - 1 (must be <65536)
     // 15625 = 1 sec
     // turn on CTC mode
     TCCR4B |= (1 << WGM12);
@@ -697,110 +934,414 @@ void setup() {
     // enable timer compare interrupt
     TIMSK4 |= (1 << OCIE4A);
 
+    // timer 5 is for read sensor data
     TCCR5A = 0;
     TCCR5B = 0;
     TCNT5 = 0;
-    OCR5A = 10000 / 1;
+    OCR5A = 3125 / 1;
     TCCR5B |= (1 << WGM12);
     TCCR5B |= (1 << CS12) | (1 << CS10);
     TIMSK5 |= (1 << OCIE5A);
     sei();
 
+    attachInterrupt(digitalPinToInterrupt(JOYSTICK_SWITCH), on_joystick_button,
+                    FALLING);
+
+    if (DEBUG) {
+        Serial.println("ALL ON");
+    }
+
+    for (int i = 0; i < 8; i++) {
+        for (int j = 0; j < 8; j++) {
+            for (int k = 0; k < 8; k++) {
+                Cube::set_status(i, j, k, 3);
+            }
+        }
+    }
+
+    delay(5000);
+    Cube::clear();
+
     // Wait for z_ref to have value;
-    delay(500);
+    while (SensorReader::angle.z == 0.0 || SensorReader::angle.y == 0.0 ||
+           SensorReader::angle.x == 0.0) {
+        Serial.print("");
+        continue;
+    }
+
+    x_ref = SensorReader::angle.x;
+    y_ref = SensorReader::angle.y;
     z_ref = SensorReader::angle.z;
+
+    if (DEBUG) {
+        Serial.print("X_ref: ");
+        Serial.println(x_ref);
+        Serial.print("Y_ref: ");
+        Serial.println(y_ref);
+        Serial.print("Z_ref: ");
+        Serial.println(z_ref);
+    }
+
+    Serial.println("Setup complete.");
 }
 
 void loop() {
-    Serial.print("Zref: ");
-    Serial.println(z_ref);
+    if (can_change_mode && clock_status == ClockStatus::Normal) {
+        if (analogRead(JOYSTICK_VRY) > 974) {
+            current_mode = (enum Modes)(current_mode + 1) % 5;
+            can_change_mode = false;
+            Cube::clear();
+            Serial.print("Changed to mode: ");
+            Serial.println(display_mode_name(current_mode));
+        }
+        else if (analogRead(JOYSTICK_VRY) < 50) {
+            current_mode = (enum Modes)(current_mode + 4) % 5;
+            can_change_mode = false;
+            Cube::clear();
+            Serial.print("Changed to mode: ");
+            Serial.println(display_mode_name(current_mode));
+        }
+    }
+    else {
+        if (analogRead(JOYSTICK_VRY) > 400 && analogRead(JOYSTICK_VRY) < 624) {
+            can_change_mode = true;
+        }
+    }
+    switch (current_mode) {
+    case Modes::CalculateAndDraw:
+        calculate_and_draw();
+        break;
+    case Modes::Clock:
+        clock();
+        break;
+    case Modes::Cube:
+        cube();
+        break;
+    case Modes::Rain:
+        rain();
+        break;
+    case Modes::Words:
+        words();
+        break;
+    default:
+        current_mode = Modes::Rain;
+    }
+}
 
-    Serial.println("Input expression");
-    while (true) {
-        if (Serial.available()) {
-            byte input_char = Serial.read();
-            // Serial.print("Pos: ");
-            // Serial.println(allowed_chars.indexOf(input_char));
-            if (allowed_chars.indexOf(input_char) != -1) {
-                input.concat((char)input_char);
-                // Serial.print("String: ");
-                // Serial.println(input);
-                continue;
-            }
+ISR(TIMER4_COMPA_vect) {
+    Cube::do_blinking();
+}
 
-            if (input_char == 0x3) {
-                break;
-            }
+ISR(TIMER5_COMPA_vect) {
+    SensorReader::read_data_from_sensor_interrupt();
 
-            if (input_char == 0x4) {
-                input = "";
-                Serial.println("Clear input string");
+    if (clock_status == ClockStatus::Normal) {
+        clock_sec_flip++;
+        clock_sec += clock_sec_flip / 4;
+        clock_minute += clock_sec / 60;
+        clock_hour += clock_minute / 60;
+
+        clock_sec_flip %= 4;
+        clock_sec %= 60;
+        clock_minute %= 60;
+        clock_hour %= 24;
+    }
+}
+
+void cube() {
+    Cube::clear();
+    Cube::draw_line(3 - cube_size, 3 - cube_size, 3 - cube_size,
+                    2 * cube_size + 2, 0, 3);
+    Cube::draw_line(3 - cube_size, 4 + cube_size, 3 - cube_size,
+                    2 * cube_size + 2, 0, 3);
+    Cube::draw_line(3 - cube_size, 3 - cube_size, 4 + cube_size,
+                    2 * cube_size + 2, 0, 3);
+    Cube::draw_line(3 - cube_size, 4 + cube_size, 4 + cube_size,
+                    2 * cube_size + 2, 0, 3);
+
+    Cube::draw_line(3 - cube_size, 3 - cube_size, 3 - cube_size,
+                    2 * cube_size + 2, 1, 3);
+    Cube::draw_line(4 + cube_size, 3 - cube_size, 3 - cube_size,
+                    2 * cube_size + 2, 1, 3);
+    Cube::draw_line(3 - cube_size, 3 - cube_size, 4 + cube_size,
+                    2 * cube_size + 2, 1, 3);
+    Cube::draw_line(4 + cube_size, 3 - cube_size, 4 + cube_size,
+                    2 * cube_size + 2, 1, 3);
+
+    Cube::draw_line(3 - cube_size, 3 - cube_size, 3 - cube_size,
+                    2 * cube_size + 2, 2, 3);
+    Cube::draw_line(3 - cube_size, 4 + cube_size, 3 - cube_size,
+                    2 * cube_size + 2, 2, 3);
+    Cube::draw_line(4 + cube_size, 3 - cube_size, 3 - cube_size,
+                    2 * cube_size + 2, 2, 3);
+    Cube::draw_line(4 + cube_size, 4 + cube_size, 3 - cube_size,
+                    2 * cube_size + 2, 2, 3);
+
+    delay(75);
+    cube_size += cube_step;
+    if (cube_size >= 3) {
+        cube_step = -1;
+    }
+    else if (cube_size <= 0) {
+        cube_step = 1;
+    }
+}
+
+void rain() {
+    for (int z = 0; z < 7; z++) {
+        for (int x = 0; x < 8; x++) {
+            for (int y = 0; y < 8; y++) {
+                int upper_state = Cube::get_status(x, y, z + 1);
+                if (upper_state > 0) {
+                    Cube::set_status(x, y, z, upper_state);
+                    Cube::set_status(x, y, z + 1, upper_state - 1);
+                }
+                else {
+                    Cube::set_status(x, y, z, 0);
+                }
             }
         }
-        if (SensorReader::angle.x > 40.0) {
-            Serial.println("a");
+    }
+
+    if (rain_create_new) {
+        int num = random(64);
+        Cube::set_status(num / 8, num % 8, 7, 3);
+        rain_create_new = false;
+    }
+    else {
+        rain_create_new = true;
+    }
+}
+
+void clock() {
+    int VRX_status = analogRead(JOYSTICK_VRX);
+    if (clock_waiting_for_new_command) {
+        if (clock_status == ClockStatus::AdjustingHour) {
+            if (VRX_status < 50) {
+                clock_hour = (clock_hour + 23) % 24;
+                clock_waiting_for_new_command = false;
+            }
+            else if (VRX_status > 974) {
+                clock_hour = (clock_hour + 1) % 24;
+                clock_waiting_for_new_command = false;
+            }
+        }
+        else if (clock_status == ClockStatus::AdjustingMinute) {
+            if (VRX_status < 50) {
+                clock_minute = (clock_minute + 59) % 60;
+                clock_waiting_for_new_command = false;
+            }
+            else if (VRX_status > 974) {
+                clock_minute = (clock_minute + 1) % 60;
+                clock_waiting_for_new_command = false;
+            }
+        }
+    }
+    else {
+        if (VRX_status > 400 && VRX_status < 624) {
+            clock_waiting_for_new_command = true;
+        }
+    }
+
+    Cube::draw_num(0, 1, 7, clock_hour / 10, 2, 3);
+    Cube::draw_num(4, 1, 7, clock_hour % 10, 2, 3);
+    Cube::draw_num(0, 0, 3, clock_minute / 10, 2, 3);
+    Cube::draw_num(4, 0, 3, clock_minute % 10, 2, 3);
+}
+
+void words() {
+    return;
+}
+
+void calculate_and_draw() {
+    bool need_reevaluate = false;
+
+    if (Serial.available()) {
+        if (DEBUG) {
+            Serial.println(input);
+        }
+        byte input_char = Serial.read();
+        // Serial.print("Pos: ");
+        // Serial.println(allowed_chars.indexOf(input_char));
+        if (allowed_chars.indexOf(input_char) != -1) {
+            input.concat((char)input_char);
+            // Serial.print("String: ");
+            // Serial.println(input);
+        }
+
+        if (input_char == '=') {
+            need_reevaluate = true;
+        }
+
+        if (input_char == 'c') {
+            input = "";
+            Serial.println("Cleared input string.");
+            return;
+        }
+
+        if (input_char == 'h') {
+            Serial.print("Current expression: ");
+            if (input.length() == 0) {
+                Serial.println("(none)");
+            }
+            else {
+                Serial.println(input);
+            }
+            Serial.println("Use c to clear input.");
+        }
+        // Prioritize user input.
+    }
+
+    if (waiting_for_command) {
+        if (SensorReader::angle.x - x_ref > 40.0) {
+            if (DEBUG) {
+                Serial.println("a");
+            }
             y_offset += 1;
-            break;
+            need_reevaluate = true;
+            waiting_for_command = false;
         }
-        else if (SensorReader::angle.x < -40.0) {
-            Serial.println("b");
+        else if (SensorReader::angle.x - x_ref < -40.0) {
+            if (DEBUG) {
+                Serial.println("b");
+            }
             y_offset -= 1;
-            break;
+            need_reevaluate = true;
+            waiting_for_command = false;
         }
 
-        if (SensorReader::angle.y > 40.0) {
-            Serial.println("c");
+        if (SensorReader::angle.y - y_ref > 40.0) {
+            if (DEBUG) {
+                Serial.println("c");
+            }
             x_offset -= 1;
-            break;
+            need_reevaluate = true;
+            waiting_for_command = false;
         }
-        else if (SensorReader::angle.y < -40.0) {
-            Serial.println("d");
+        else if (SensorReader::angle.y - y_ref < -40.0) {
+            if (DEBUG) {
+                Serial.println("d");
+            }
             x_offset += 1;
-            break;
+            need_reevaluate = true;
+            waiting_for_command = false;
         }
 
         if (SensorReader::angle.z - z_ref > 40.0) {
-            Serial.println("e");
+            if (DEBUG) {
+                Serial.println(SensorReader::angle.z);
+                Serial.println("e");
+            }
             zoom /= 2.0;
-            break;
+            need_reevaluate = true;
+            waiting_for_command = false;
         }
         else if (SensorReader::angle.z - z_ref < -40.0) {
-            Serial.println(SensorReader::angle.z);
-            Serial.println("f");
+            if (DEBUG) {
+                Serial.println(SensorReader::angle.z);
+                Serial.println("f");
+            }
             zoom *= 2.0;
-            break;
+            need_reevaluate = true;
+            waiting_for_command = false;
+        }
+
+        if (analogRead(JOYSTICK_VRX) < 50) {
+            z_offset -= 1;
+            need_reevaluate = true;
+            waiting_for_command = false;
+        }
+        else if (analogRead(JOYSTICK_VRX) > 974) {
+            z_offset += 1;
+            need_reevaluate = true;
+            waiting_for_command = false;
         }
 
         if (abs(SensorReader::acceleration.x) +
                 abs(SensorReader::acceleration.y) +
                 abs(SensorReader::acceleration.z) >
             120.0) {
+            if (DEBUG) {
+                Serial.print("g");
+            }
             x_offset = 0;
             y_offset = 0;
             z_offset = 0;
             zoom = 1;
+            Serial.println("Reset zoom and translate.");
+            need_reevaluate = true;
+            waiting_for_command = false;
+        }
+
+        if (abs(SensorReader::gyro.x) + abs(SensorReader::gyro.y) +
+                abs(SensorReader::gyro.z) >
+            240) {
+            if (DEBUG) {
+                Serial.println("h");
+                Serial.print(SensorReader::gyro.x);
+                Serial.print(" ");
+                Serial.print(SensorReader::gyro.y);
+                Serial.print(" ");
+                Serial.println(SensorReader::gyro.z);
+            }
+            x_offset = 0;
+            y_offset = 0;
+            z_offset = 0;
+            zoom = 1;
+            Serial.println("Reset zoom and translate.");
+            need_reevaluate = true;
+            waiting_for_command = false;
+        }
+    }
+    else {
+        if (DEBUG) {
+            Serial.print(abs(SensorReader::angle.x - x_ref) < 20.0 ? "true"
+                                                                   : "false");
+            Serial.print(" ");
+            Serial.print(abs(SensorReader::angle.y - y_ref) < 20.0 ? "true"
+                                                                   : "false");
+            Serial.print(" ");
+            Serial.print(abs(SensorReader::angle.z - z_ref) < 20.0 ? "true"
+                                                                   : "false");
+            Serial.print(" ");
+            Serial.println(analogRead(JOYSTICK_VRX));
+        }
+
+        if (abs(SensorReader::angle.x - x_ref) < 20.0 &&
+            abs(SensorReader::angle.y - y_ref) < 20.0 &&
+            abs(SensorReader::angle.z - z_ref) < 20.0 &&
+            analogRead(JOYSTICK_VRX) > 400 && analogRead(JOYSTICK_VRX) < 648) {
+            waiting_for_command = true;
+            Serial.println("Waiting for new command");
         }
     }
 
-    Serial.print("Read: ");
+    if (!need_reevaluate || input.length() == 0) {
+        return;
+    }
+
+    Serial.print("Expression to be evaluated: ");
     Serial.println(input);
 
-    Serial.print("Offset: ");
-    Serial.print(x_offset);
-    Serial.print(" ");
-    Serial.print(y_offset);
-    Serial.print(" ");
-    Serial.println(z_offset);
-    Serial.print("Zoom: ");
-    Serial.println(zoom);
+    if (DEBUG) {
+        Serial.print("Offset: ");
+        Serial.print(x_offset);
+        Serial.print(" ");
+        Serial.print(y_offset);
+        Serial.print(" ");
+        Serial.println(z_offset);
+        Serial.print("Zoom: ");
+        Serial.println(zoom);
+    }
+
     Calculator calculator(input);
 
     Cube::clear();
 
     for (int i = 0; i < 8; i++) {
         for (int j = 0; j < 8; j++) {
-            x = (i + x_offset) * zoom;
-            y = (j + y_offset) * zoom;
+            symbol_x = (i + x_offset) * zoom;
+            symbol_y = (j + y_offset) * zoom;
             double result = calculator.evaluate();
             int z;
             if (!(isinf(result) || isnan(result))) {
@@ -819,23 +1360,87 @@ void loop() {
 
             // Display origin point
             Cube::set_status(round(-x_offset), round(-y_offset),
-                             round(-z_offset), 3);
+                             round(z_offset), 3);
             Cube::set_blinking(round(-x_offset), round(-y_offset),
-                               round(-z_offset));
+                               round(z_offset));
         }
     }
-
-    // delay(1000);
-
-    // int z = round(calculator.evaluate());
-    // Serial.println(z);
-    // Serial.print("Here");
 }
 
-ISR(TIMER4_COMPA_vect) {
-    Cube::do_blinking();
+void on_joystick_button() {
+    if (current_mode == Modes::CalculateAndDraw) {
+        x_ref = SensorReader::angle.x;
+        y_ref = SensorReader::angle.y;
+        z_ref = SensorReader::angle.z;
+        Serial.println("Reset position.");
+        return;
+    }
+    if (current_mode == Modes::Clock) {
+        switch (clock_status) {
+        case ClockStatus::Normal: {
+            if (DEBUG) {
+                Serial.println("Start adjusting hour");
+            }
+            for (int i = 0; i < 8; i++) {
+                for (int j = 4; j < 8; j++) {
+                    // Cube::set_blinking(i, 1, j);
+                    Cube::LED_blinking_status[j][i] = 0xff;
+                }
+            }
+            // Cube::LED_blinking_status[0][0] = 0x1;
+            clock_status = ClockStatus::AdjustingHour;
+            break;
+        }
+        case ClockStatus::AdjustingHour: {
+            if (DEBUG) {
+                Serial.println("Start adjucting minute");
+            }
+            for (int i = 0; i < 8; i++) {
+                for (int j = 4; j < 8; j++) {
+                    // Cube::unset_blinking(i, 1, j);
+                    Cube::LED_blinking_status[j][i] = 0;
+                }
+                for (int j = 0; j < 4; j++) {
+                    // Cube::set_blinking(i, 0, j);
+                    Cube::LED_blinking_status[j][i] = 0xff;
+                }
+            }
+            clock_status = ClockStatus::AdjustingMinute;
+            break;
+        }
+        case ClockStatus::AdjustingMinute: {
+            if (DEBUG) {
+                Serial.println("Normal mode");
+            }
+            for (int i = 0; i < 8; i++) {
+                for (int j = 0; j < 4; j++) {
+                    Cube::LED_blinking_status[j][i] = 0;
+                }
+            }
+            clock_sec = 0;
+            clock_sec_flip = 0;
+            clock_status = ClockStatus::Normal;
+            break;
+        }
+        default:
+            clock_status = ClockStatus::Normal;
+        }
+    }
 }
 
-ISR(TIMER5_COMPA_vect) {
-    SensorReader::read_data_from_sensor_interrupt();
+String display_mode_name(Modes mode_name) {
+    switch (mode_name) {
+    case Modes::CalculateAndDraw:
+        return String("Draw function graph");
+    case Modes::Cube:
+        return String("Cube");
+    case Modes::Rain:
+        return String("Rain");
+    case Modes::Clock:
+        return String("Clock");
+    case Modes::Words:
+        return String("Display Words");
+    default:
+        return String("Unknown");
+    }
 }

HardwareDesign/HardwareDesign.ino

@@ -1,240 +1,74 @@
 #include "REG.h"
 #include "wit_c_sdk.h"
 
-// #define ACC_UPDATE 0x01
-// #define GYRO_UPDATE 0x02
-// #define ANGLE_UPDATE 0x04
-// #define MAG_UPDATE 0x08
-// #define READ_UPDATE 0x80
-
-// char s_cDataUpdate = 0;
-// const uint32_t c_uiBaud[8] = {0,     4800,  9600,   19200,
-//                               38400, 57600, 115200, 230400};
-
-// float fAcc[3], fGyro[3], fAngle[3];
-
-// bool setup_ok = true;
-
-// static void SensorUartSend(uint8_t *p_data, uint32_t uiSize);
-// static void SensorDataUpdate(uint32_t uiReg, uint32_t uiRegNum);
-// static void Delayms(uint16_t ucMs);
-// static void AutoScanSensor();
-
-// void setup() {
-//   Serial.begin(115200);
-//   WitInit(WIT_PROTOCOL_NORMAL, 0x50);
-//   WitSerialWriteRegister(SensorUartSend);
-//   WitRegisterCallBack(SensorDataUpdate);
-//   WitDelayMsRegister(Delayms);
-//   AutoScanSensor();
-
-//   // Ensure the setting for the sensor is the default
-//   if (WitSetBandwidth(BANDWIDTH_256HZ) != WIT_HAL_OK) {
-//     Serial.print("Set Bandwidth Error\r\n");
-//     setup_ok = false;
-//     return;
-//   }
-
-//   // if (WitSetUartBaud(WIT_BAUD_115200) != WIT_HAL_OK) {
-//   //   Serial.print("Set baud error\r\n");
-//   //   setup_ok = false;
-//   //   return;
-//   // } else {
-//   //   Serial1.begin(115200);
-//   // }
-
-//   if (WitSetOutputRate(RRATE_1HZ) != WIT_HAL_OK) {
-//     Serial.print("Can't set report rate to 10Hz!\r\n");
-//     setup_ok = false;
-//     return;
-//   }
-
-//   if (WitSetContent(RSW_ACC | RSW_GYRO | RSW_ANGLE | RSW_MAG) != WIT_HAL_OK)
-//   {
-//     Serial.print("Set RSW Error\r\n");
-//     setup_ok = false;
-//     return;
-//   }
-// }
-
-// void loop() {
-//   while (Serial1.available()) {
-//     WitSerialDataIn(Serial1.read());
-//   }
-
-//   // if (s_cDataUpdate) {
-//   //   for (int i = 0; i < 3; i++) {
-//   //     fAcc[i] = sReg[AX + i] / 32768.0f * 16.0f;      // Unit: g(9.8m/s^2)
-//   //     fGyro[i] = sReg[GX + i] / 32768.0f * 2000.0f;   // Unit: deg/s
-//   //     fAngle[i] = sReg[Roll + i] / 32768.0f * 180.0f; // Unit: deg
-//   //   }
-
-//   //   if (s_cDataUpdate & ACC_UPDATE) {
-//   //     Serial.print("acc:");
-//   //     Serial.print(fAcc[0], 3);
-//   //     Serial.print(" ");
-//   //     Serial.print(fAcc[1], 3);
-//   //     Serial.print(" ");
-//   //     Serial.print(fAcc[2], 3);
-//   //     Serial.print("\r\n");
-//   //     s_cDataUpdate &= ~ACC_UPDATE;
-//   //   }
-//   //   if (s_cDataUpdate & GYRO_UPDATE) {
-//   //     Serial.print("gyro:");
-//   //     Serial.print(fGyro[0], 1);
-//   //     Serial.print(" ");
-//   //     Serial.print(fGyro[1], 1);
-//   //     Serial.print(" ");
-//   //     Serial.print(fGyro[2], 1);
-//   //     Serial.print("\r\n");
-//   //     s_cDataUpdate &= ~GYRO_UPDATE;
-//   //   }
-//   //   if (s_cDataUpdate & ANGLE_UPDATE) {
-//   //     Serial.print("angle:");
-//   //     Serial.print(fAngle[0], 3);
-//   //     Serial.print(" ");
-//   //     Serial.print(fAngle[1], 3);
-//   //     Serial.print(" ");
-//   //     Serial.print(fAngle[2], 3);
-//   //     Serial.print("\r\n");
-//   //     s_cDataUpdate &= ~ANGLE_UPDATE;
-//   //   }
-//   //   if (s_cDataUpdate & MAG_UPDATE) {
-//   //     Serial.print("mag:");
-//   //     Serial.print(sReg[HX]);
-//   //     Serial.print(" ");
-//   //     Serial.print(sReg[HY]);
-//   //     Serial.print(" ");
-//   //     Serial.print(sReg[HZ]);
-//   //     Serial.print("\r\n");
-//   //     s_cDataUpdate &= ~MAG_UPDATE;
-//   //   }
-//   //   s_cDataUpdate = 0;
-//   // }
-// }
-
-// static void SensorUartSend(uint8_t *p_data, uint32_t uiSize) {
-//   Serial1.print("Sensor says: ");
-//   Serial1.write(p_data, uiSize);
-//   Serial1.flush();
-// };
-
-// static void SensorDataUpdate(uint32_t uiReg, uint32_t uiRegNum) {
-//   int i;
-//   for (i = 0; i < uiRegNum; i++) {
-//     switch (uiReg) {
-//     case AZ:
-//       s_cDataUpdate |= ACC_UPDATE;
-//       break;
-//     case GZ:
-//       s_cDataUpdate |= GYRO_UPDATE;
-//       break;
-//     case HZ:
-//       s_cDataUpdate |= MAG_UPDATE;
-//       break;
-//     case Yaw:
-//       s_cDataUpdate |= ANGLE_UPDATE;
-//       break;
-//     default:
-//       s_cDataUpdate |= READ_UPDATE;
-//       break;
-//     }
-//     uiReg++;
-//   }
-//   Serial.println(s_cDataUpdate);
-// };
-
-// static void Delayms(uint16_t ucMs) { delay(ucMs); }
-
-// static void AutoScanSensor() {
-//   int i, iRetry;
-
-//   for (i = 0; i < sizeof(c_uiBaud) / sizeof(c_uiBaud[0]); i++) {
-//     Serial1.begin(c_uiBaud[i]);
-//     Serial1.flush();
-//     iRetry = 2;
-//     s_cDataUpdate = 0;
-//     do {
-//       WitReadReg(AX, 3);
-//       delay(200);
-//       while (Serial1.available()) {
-//         WitSerialDataIn(Serial1.read());
-//       }
-//       if (s_cDataUpdate != 0) {
-//         Serial.print(c_uiBaud[i]);
-//         Serial.print(" baud find sensor\r\n\r\n");
-//         return;
-//       }
-//       iRetry--;
-//     } while (iRetry);
-//   }
-//   Serial.print("can not find sensor\r\n");
-//   Serial.print("please check your connection\r\n");
-// }
-
-/*
-Test on MEGA 2560. use WT901CTTL sensor
-
-WT901CTTL     MEGA 2560a
-    VCC <--->  5V/3.3V
-    TX  <--->  19(TX1)
-    RX  <--->  18(RX1)
-    GND <--->  GND
-*/
-
 #define ACC_UPDATE 0x01
 #define GYRO_UPDATE 0x02
 #define ANGLE_UPDATE 0x04
 #define MAG_UPDATE 0x08
 #define READ_UPDATE 0x80
-static volatile char s_cDataUpdate = 0, s_cCmd = 0xff;
 
-static void CmdProcess(void);
-static void AutoScanSensor(void);
-static void SensorUartSend(uint8_t *p_data, uint32_t uiSize);
-static void SensorDataUpdata(uint32_t uiReg, uint32_t uiRegNum);
-static void Delayms(uint16_t ucMs);
+char s_cDataUpdate = 0;
 const uint32_t c_uiBaud[8] = {0,     4800,  9600,   19200,
                               38400, 57600, 115200, 230400};
 
-int i;
 float fAcc[3], fGyro[3], fAngle[3];
 
+bool setup_ok = true;
+
+static void SensorUartSend(uint8_t *p_data, uint32_t uiSize);
+static void SensorDataUpdate(uint32_t uiReg, uint32_t uiRegNum);
+static void Delayms(uint16_t ucMs);
+static void AutoScanSensor();
+
 void setup() {
-  // put your setup code here, to run once:
   Serial.begin(115200);
   WitInit(WIT_PROTOCOL_NORMAL, 0x50);
   WitSerialWriteRegister(SensorUartSend);
-  WitRegisterCallBack(SensorDataUpdata);
+  WitRegisterCallBack(SensorDataUpdate);
   WitDelayMsRegister(Delayms);
   AutoScanSensor();
 
-  if (WitStartAccCali() == WIT_HAL_OK) {
-    Serial.print("Start Calibri");
-    delay(1000);
-    Serial.print("Stop calibri");
-    WitStopAccCali();
-  } else {
-    Serial.print("Cannot start calibri acc\r\n");
+  // Ensure the setting for the sensor is the default
+  if (WitSetBandwidth(BANDWIDTH_256HZ) != WIT_HAL_OK) {
+    Serial.print("Set Bandwidth Error\r\n");
+    setup_ok = false;
+    return;
   }
 
-  if (WitSetUartBaud(WIT_BAUD_9600) != WIT_HAL_OK)
-    Serial.print("\r\nSet Baud Error\r\n");
-  else {
-    Serial1.begin(c_uiBaud[WIT_BAUD_9600]);
-    Serial.print(" 115200 Baud rate modified successfully\r\n");
+  // if (WitSetUartBaud(WIT_BAUD_115200) != WIT_HAL_OK) {
+  //   Serial.print("Set baud error\r\n");
+  //   setup_ok = false;
+  //   return;
+  // } else {
+  //   Serial1.begin(115200);
+  // }
+
+  if (WitSetOutputRate(RRATE_1HZ) != WIT_HAL_OK) {
+    Serial.print("Can't set report rate to 10Hz!\r\n");
+    setup_ok = false;
+    return;
+  }
+
+  if (WitSetContent(RSW_ACC | RSW_GYRO | RSW_ANGLE | RSW_MAG) != WIT_HAL_OK)
+  {
+    Serial.print("Set RSW Error\r\n");
+    setup_ok = false;
+    return;
   }
 }
+
 void loop() {
   while (Serial1.available()) {
     WitSerialDataIn(Serial1.read());
   }
+
   if (s_cDataUpdate) {
-    for (i = 0; i < 3; i++) {
-      fAcc[i] = sReg[AX + i] / 32768.0f * 16.0f * 9.8f; // Unit: m/s^2
-      fGyro[i] = sReg[GX + i] / 32768.0f * 2000.0f; // Unit: deg/s
+    for (int i = 0; i < 3; i++) {
+      fAcc[i] = sReg[AX + i] / 32768.0f * 16.0f;      // Unit: g(9.8m/s^2)
+      fGyro[i] = sReg[GX + i] / 32768.0f * 2000.0f;   // Unit: deg/s
       fAngle[i] = sReg[Roll + i] / 32768.0f * 180.0f; // Unit: deg
     }
+    Serial.print("Here");
     if (s_cDataUpdate & ACC_UPDATE) {
       Serial.print("acc:");
       Serial.print(fAcc[0], 3);
@@ -282,9 +116,9 @@ void loop() {
 static void SensorUartSend(uint8_t *p_data, uint32_t uiSize) {
   Serial1.write(p_data, uiSize);
   Serial1.flush();
-}
-static void Delayms(uint16_t ucMs) { delay(ucMs); }
-static void SensorDataUpdata(uint32_t uiReg, uint32_t uiRegNum) {
+};
+
+static void SensorDataUpdate(uint32_t uiReg, uint32_t uiRegNum) {
   int i;
   for (i = 0; i < uiRegNum; i++) {
     switch (uiReg) {
@@ -306,9 +140,12 @@ static void SensorDataUpdata(uint32_t uiReg, uint32_t uiRegNum) {
     }
     uiReg++;
   }
-}
+  // Serial.println(s_cDataUpdate);
+};
 
-static void AutoScanSensor(void) {
+static void Delayms(uint16_t ucMs) { delay(ucMs); }
+
+static void AutoScanSensor() {
   int i, iRetry;
 
   for (i = 0; i < sizeof(c_uiBaud) / sizeof(c_uiBaud[0]); i++) {
@@ -333,3 +170,165 @@ static void AutoScanSensor(void) {
   Serial.print("can not find sensor\r\n");
   Serial.print("please check your connection\r\n");
 }
+
+/*
+Test on MEGA 2560. use WT901CTTL sensor
+
+WT901CTTL     MEGA 2560a
+    VCC <--->  5V/3.3V
+    TX  <--->  19(TX1)
+    RX  <--->  18(RX1)
+    GND <--->  GND
+*/
+
+// #define ACC_UPDATE 0x01
+// #define GYRO_UPDATE 0x02
+// #define ANGLE_UPDATE 0x04
+// #define MAG_UPDATE 0x08
+// #define READ_UPDATE 0x80
+// static volatile char s_cDataUpdate = 0, s_cCmd = 0xff;
+
+// static void CmdProcess(void);
+// static void AutoScanSensor(void);
+// static void SensorUartSend(uint8_t *p_data, uint32_t uiSize);
+// static void SensorDataUpdata(uint32_t uiReg, uint32_t uiRegNum);
+// static void Delayms(uint16_t ucMs);
+// const uint32_t c_uiBaud[8] = {0,     4800,  9600,   19200,
+//                               38400, 57600, 115200, 230400};
+
+// int i;
+// float fAcc[3], fGyro[3], fAngle[3];
+
+// void setup() {
+//   // put your setup code here, to run once:
+//   Serial.begin(115200);
+//   WitInit(WIT_PROTOCOL_NORMAL, 0x50);
+//   WitSerialWriteRegister(SensorUartSend);
+//   WitRegisterCallBack(SensorDataUpdata);
+//   WitDelayMsRegister(Delayms);
+//   AutoScanSensor();
+
+//   // if (WitStartAccCali() == WIT_HAL_OK) {
+//   //   Serial.print("Start Calibri");
+//   //   delay(1000);
+//   //   Serial.print("Stop calibri");
+//   //   WitStopAccCali();
+//   // } else {
+//   //   Serial.print("Cannot start calibri acc\r\n");
+//   // }
+
+//   if (WitSetUartBaud(WIT_BAUD_9600) != WIT_HAL_OK)
+//     Serial.print("\r\nSet Baud Error\r\n");
+//   else {
+//     Serial1.begin(c_uiBaud[WIT_BAUD_9600]);
+//     Serial.print(" 9600 Baud rate modified successfully\r\n");
+//   }
+// }
+// void loop() {
+//   while (Serial1.available()) {
+//     WitSerialDataIn(Serial1.read());
+//   }
+//   if (s_cDataUpdate) {
+//     for (i = 0; i < 3; i++) {
+//       fAcc[i] = sReg[AX + i] / 32768.0f * 16.0f * 9.8f; // Unit: m/s^2
+//       fGyro[i] = sReg[GX + i] / 32768.0f * 2000.0f; // Unit: deg/s
+//       fAngle[i] = sReg[Roll + i] / 32768.0f * 180.0f; // Unit: deg
+//     }
+//     if (s_cDataUpdate & ACC_UPDATE) {
+//       Serial.print("acc:");
+//       Serial.print(fAcc[0], 3);
+//       Serial.print(" ");
+//       Serial.print(fAcc[1], 3);
+//       Serial.print(" ");
+//       Serial.print(fAcc[2], 3);
+//       Serial.print("\r\n");
+//       s_cDataUpdate &= ~ACC_UPDATE;
+//     }
+//     if (s_cDataUpdate & GYRO_UPDATE) {
+//       Serial.print("gyro:");
+//       Serial.print(fGyro[0], 1);
+//       Serial.print(" ");
+//       Serial.print(fGyro[1], 1);
+//       Serial.print(" ");
+//       Serial.print(fGyro[2], 1);
+//       Serial.print("\r\n");
+//       s_cDataUpdate &= ~GYRO_UPDATE;
+//     }
+//     if (s_cDataUpdate & ANGLE_UPDATE) {
+//       Serial.print("angle:");
+//       Serial.print(fAngle[0], 3);
+//       Serial.print(" ");
+//       Serial.print(fAngle[1], 3);
+//       Serial.print(" ");
+//       Serial.print(fAngle[2], 3);
+//       Serial.print("\r\n");
+//       s_cDataUpdate &= ~ANGLE_UPDATE;
+//     }
+//     if (s_cDataUpdate & MAG_UPDATE) {
+//       Serial.print("mag:");
+//       Serial.print(sReg[HX]);
+//       Serial.print(" ");
+//       Serial.print(sReg[HY]);
+//       Serial.print(" ");
+//       Serial.print(sReg[HZ]);
+//       Serial.print("\r\n");
+//       s_cDataUpdate &= ~MAG_UPDATE;
+//     }
+//     s_cDataUpdate = 0;
+//   }
+// }
+
+// static void SensorUartSend(uint8_t *p_data, uint32_t uiSize) {
+//   Serial1.write(p_data, uiSize);
+//   Serial1.flush();
+// }
+// static void Delayms(uint16_t ucMs) { delay(ucMs); }
+// static void SensorDataUpdata(uint32_t uiReg, uint32_t uiRegNum) {
+//   int i;
+//   for (i = 0; i < uiRegNum; i++) {
+//     switch (uiReg) {
+//     case AZ:
+//       s_cDataUpdate |= ACC_UPDATE;
+//       break;
+//     case GZ:
+//       s_cDataUpdate |= GYRO_UPDATE;
+//       break;
+//     case HZ:
+//       s_cDataUpdate |= MAG_UPDATE;
+//       break;
+//     case Yaw:
+//       s_cDataUpdate |= ANGLE_UPDATE;
+//       break;
+//     default:
+//       s_cDataUpdate |= READ_UPDATE;
+//       break;
+//     }
+//     uiReg++;
+//   }
+// }
+
+// static void AutoScanSensor(void) {
+//   int i, iRetry;
+
+//   for (i = 0; i < sizeof(c_uiBaud) / sizeof(c_uiBaud[0]); i++) {
+//     Serial1.begin(c_uiBaud[i]);
+//     Serial1.flush();
+//     iRetry = 2;
+//     s_cDataUpdate = 0;
+//     do {
+//       WitReadReg(AX, 3);
+//       delay(200);
+//       while (Serial1.available()) {
+//         WitSerialDataIn(Serial1.read());
+//       }
+//       if (s_cDataUpdate != 0) {
+//         Serial.print(c_uiBaud[i]);
+//         Serial.print(" baud find sensor\r\n\r\n");
+//         return;
+//       }
+//       iRetry--;
+//     } while (iRetry);
+//   }
+//   Serial.print("can not find sensor\r\n");
+//   Serial.print("please check your connection\r\n");
+// }

Test/Test.ino

@@ -1,50 +1,14 @@
-byte status = 0;
-byte layer = 0;
-
 void setup() {
-  Serial.begin(115200);
+  pinMode(A7, INPUT_PULLUP);
+  pinMode(2, INPUT_PULLUP);
   for (int i = 0; i < 8; i++) {
     pinMode(22 + i, OUTPUT);
+    digitalWrite(22 + i, HIGH);
   }
-  // pinMode(30, OUTPUT);
-  // pinMode(31, OUTPUT);
-
-  for (int i = 0; i < 8; i++) {
-    pinMode(30 + i, OUTPUT);
-  }
-
-  for (int i = 0; i < 8; i++) {
-    pinMode(38 + i, OUTPUT);
-  }
+  Serial.begin(115200);
 }
 
-int column = 0;
-
 void loop() {
-  if (Serial.available()) {
-    int num = Serial.read();
-    if (num >= '0' && num <= '7') {
-      num = num - '0';
-      status ^= 1 << num;
-
-      for (int i = 0; i < 8; i++) {
-        digitalWrite(22 + i, status >> i & 1);
-      }
-
-      Serial.println(status, BIN);
-    } else if (num >= 'a' && num <= 'h') {
-      num = num - 'a';
-      column = num;
-      Serial.print("LOAD");
-      Serial.print(num);
-      Serial.println(status, BIN);
-      digitalWrite(30 + column, HIGH);
-      digitalWrite(30 + column, LOW);
-    }
-    else if (num == 'z') {
-      digitalWrite(38 + layer, LOW);
-      layer = (layer + 1) % 8;
-      digitalWrite(38 + layer, HIGH);
-    }
-  }
+  Serial.println(digitalRead(2));
+  delay(100);
 }