更改函数实现顺序。

8By8/8By8.ino

@@ -10,6 +10,10 @@
 #define CLOCK_START_PIN 30
 #define SW_START_PIN 38
 
+#define JOYSTICK_VRX A1
+#define JOYSTICK_VRY A0
+#define JOYSTICK_SWITCH 2
+
 #define ACC_UPDATE 0x01
 #define GYRO_UPDATE 0x02
 #define ANGLE_UPDATE 0x04
@@ -28,7 +32,6 @@ class SensorReader {
 private:
     static const uint32_t c_uiBaud[8];
     static volatile byte s_cDataUpdate;
-    static Triple angle;
 
     static void AutoScanSensor() {
         int iRetry;
@@ -91,6 +94,8 @@ private:
     }
 
 public:
+    static Triple angle, acceleration;
+
     static bool init() {
         WitInit(WIT_PROTOCOL_NORMAL, 0x50);
         WitSerialWriteRegister(SensorReader::SensorUartSend);
@@ -104,27 +109,29 @@ public:
         }
         else {
             Serial1.begin(SensorReader::c_uiBaud[WIT_BAUD_9600]);
-            Serial.println("9600 Baud rate modified successfully");
+            // Serial.println("9600 Baud rate modified successfully");
         }
 
-        if (WitSetContent(RSW_ANGLE) != WIT_HAL_OK) {
-            Serial.print("\r\nSet Content Angle Error\r\n");
+        if (WitSetContent(RSW_ANGLE | RSW_ACC) != WIT_HAL_OK) {
+            Serial.println("Set send content: angle, acc Error");
             return false;
         }
-        else {
-            Serial.println("Set Content Angle success");
+
+        if (WitSetOutputRate(RRATE_2HZ) != WIT_HAL_OK) {
+            Serial.print("Set report rate failed");
+            return false;
         }
+
         return true;
     }
 
     static void read_data_from_sensor_interrupt() {
+        // Serial.println("Updated!");
         while (Serial1.available()) {
-            WitSerialDataIn(Serial.read());
+            WitSerialDataIn(Serial1.read());
         }
-
         if (SensorReader::s_cDataUpdate) {
             if (SensorReader::s_cDataUpdate & ANGLE_UPDATE) {
-                Serial.println("Updated!");
                 SensorReader::angle.x = sReg[Roll] / 32768.0f * 180.0f;
                 SensorReader::angle.y = sReg[Roll + 1] / 32768.0f * 180.0f;
                 SensorReader::angle.z =
@@ -134,11 +141,16 @@ public:
 
                 SensorReader::s_cDataUpdate = 0;
             }
-        }
-    }
 
-    static Triple get_angle() {
-        return SensorReader::angle;
+            if (SensorReader::s_cDataUpdate & ACC_UPDATE) {
+                SensorReader::acceleration.x =
+                    sReg[AX] / 32768.0f * 16.0f * 9.8f;
+                SensorReader::acceleration.y =
+                    sReg[AX + 1] / 32768.0f * 16.0f * 9.8f;
+                SensorReader::acceleration.z =
+                    sReg[AX + 2] / 32768.0f * 16.0f * 9.8f;
+            }
+        }
     }
 };
 
@@ -146,10 +158,50 @@ class Cube {
 private:
     static int layer_count;
     static int brightness_count;
+    static bool blinking_LED_status;
 
 public:
     // Every int represents a row of 8 LED status.
-    static int LED_status[8][8];
+    static uint16_t LED_status[8][8];
+    static uint16_t LED_brightness[8][8];
+    static uint8_t LED_blinking_status[8][8];
+
+    static void set_blinking(int x, int y, int z) {
+        // 1 = enable blinking
+        if (x >= 8 || x < 0 || y >= 8 || y < 0 || z >= 8 || z < 0) {
+            return;
+        }
+        LED_blinking_status[z][x] = LED_blinking_status[z][x] | (1 << y);
+    }
+
+    static void unset_blinking(int x, int y, int z) {
+        if (x >= 8 || x < 0 || y >= 8 || y < 0 || z >= 8 || z < 0) {
+            return;
+        }
+        LED_blinking_status[z][x] = LED_blinking_status[z][x] & (~1 << y);
+
+        LED_status[z][x] = (LED_status[z][x] & (~(3 << (y * 2)))) |
+                           (LED_brightness[z][x] & (3 << (y * 2)));
+    }
+
+    static void do_blinking() {
+        blinking_LED_status ^= 1;
+        for (int i = 0; i < 8; i++) {
+            for (int j = 0; j < 8; j++) {
+                for (int k = 0; k < 8; k++) {
+                    if (LED_blinking_status[i][j] >> k & 1) {
+                        if (blinking_LED_status) {
+                            LED_status[i][j] = LED_brightness[i][j];
+                        }
+                        else {
+                            LED_status[i][j] =
+                                LED_status[i][j] & (~(3 << (k * 2)));
+                        }
+                    }
+                }
+            }
+        }
+    }
 
     static void display() {
         // Serial.println("Here");
@@ -183,19 +235,54 @@ public:
         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));
     }
 
     static int get_status(int x, int y, int z) {
-        return LED_status[z][x] >> (y * 2) & 3;
+        return LED_brightness[z][x] >> (y * 2) & 3;
     }
 
     static void clear() {
         for (int i = 0; i < 8; i++) {
             for (int j = 0; j < 8; j++) {
+                LED_brightness[i][j] = 0;
+                LED_blinking_status[i][j] = 0;
                 LED_status[i][j] = 0;
             }
         }
     }
+
+    static void draw_line(int x, int y, int z, int length, int direction,
+                          int brightness) {
+        if (x >= 8 || x < 0 || y >= 8 || y < 0 || z >= 8 || z < 0) {
+            return;
+        }
+        if (direction >= 3 || direction < 0 || length <= 0 || brightness >= 4 ||
+            brightness < 0) {
+            return;
+        }
+        // 0: x
+        // 1: y
+        // 2: z
+        switch (direction) {
+        case 0:
+            for (int i = 0; i < length; i++) {
+                set_status(x + i, y, z, brightness);
+            }
+            break;
+        case 1:
+            for (int i = 0; i < length; i++) {
+                set_status(x, y + i, z, brightness);
+            }
+            break;
+        case 2:
+            for (int i = 0; i < length; i++) {
+                set_status(x, y, z + i, brightness);
+            }
+            break;
+        }
+    }
 };
 
 enum Operators {
@@ -555,24 +642,34 @@ public:
 
 int Cube::layer_count = 0;
 int Cube::brightness_count = 0;
-int Cube::LED_status[8][8] = {0};
+uint16_t Cube::LED_status[8][8] = {0};
+uint16_t Cube::LED_brightness[8][8] = {0};
+uint8_t Cube::LED_blinking_status[8][8] = {0};
+bool Cube::blinking_LED_status = false;
 
 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;
+Triple SensorReader::angle, SensorReader::acceleration;
 
 double x, 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;
 
 void setup() {
     for (int i = 22; i < 46; i++) {
         pinMode(i, OUTPUT);
     }
 
+    pinMode(JOYSTICK_SWITCH, INPUT);
+    pinMode(JOYSTICK_VRX, INPUT);
+    pinMode(JOYSTICK_VRY, INPUT);
+
     Timer1.initialize();
     Timer1.setPeriod(TIME_PER_LAYER_IN_US);
 
@@ -585,74 +682,160 @@ void setup() {
     }
 
     Timer1.attachInterrupt(Cube::display);
+
+    cli();
+    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)
+    // 15625 = 1 sec
+    // turn on CTC mode
+    TCCR4B |= (1 << WGM12);
+    // Set CS12 and CS10 bits for 1024 prescaler
+    TCCR4B |= (1 << CS12) | (1 << CS10);
+    // enable timer compare interrupt
+    TIMSK4 |= (1 << OCIE4A);
+
+    TCCR5A = 0;
+    TCCR5B = 0;
+    TCNT5 = 0;
+    OCR5A = 10000 / 1;
+    TCCR5B |= (1 << WGM12);
+    TCCR5B |= (1 << CS12) | (1 << CS10);
+    TIMSK5 |= (1 << OCIE5A);
+    sei();
+
+    // Wait for z_ref to have value;
+    delay(500);
+    z_ref = SensorReader::angle.z;
 }
 
 void loop() {
-    Triple angle;
+    Serial.print("Zref: ");
+    Serial.println(z_ref);
 
-    angle = SensorReader::get_angle();
-    Serial.print("x: ");
-    Serial.println(angle.x);
-    Serial.print("y: ");
-    Serial.println(angle.y);
-    Serial.print("z: ");
-    Serial.println(angle.z);
+    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;
+            }
 
-    delay(500);
+            if (input_char == 0x3) {
+                break;
+            }
 
-    // String input = "";
-    // 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;
-    //         }
+            if (input_char == 0x4) {
+                input = "";
+                Serial.println("Clear input string");
+            }
+        }
+        if (SensorReader::angle.x > 40.0) {
+            Serial.println("a");
+            y_offset += 1;
+            break;
+        }
+        else if (SensorReader::angle.x < -40.0) {
+            Serial.println("b");
+            y_offset -= 1;
+            break;
+        }
 
-    //         if (input_char == 0x3) {
-    //             break;
-    //         }
-    //     }
-    // }
-    // Serial.print("Read: ");
-    // Serial.println(input);
-    // Calculator calculator(input);
-    // int x_offset, y_offset;
+        if (SensorReader::angle.y > 40.0) {
+            Serial.println("c");
+            x_offset -= 1;
+            break;
+        }
+        else if (SensorReader::angle.y < -40.0) {
+            Serial.println("d");
+            x_offset += 1;
+            break;
+        }
 
-    // for (x_offset = 0; x_offset < 4; x_offset++) {
-    //     for (y_offset = 0; y_offset < 4; y_offset++) {
-    //         Cube::clear();
+        if (SensorReader::angle.z - z_ref > 40.0) {
+            Serial.println("e");
+            zoom /= 2.0;
+            break;
+        }
+        else if (SensorReader::angle.z - z_ref < -40.0) {
+            Serial.println(SensorReader::angle.z);
+            Serial.println("f");
+            zoom *= 2.0;
+            break;
+        }
 
-    //         for (int i = 0; i < 8; i++) {
-    //             for (int j = 0; j < 8; j++) {
-    //                 x = i - x_offset;
-    //                 y = j - y_offset;
-    //                 double result = calculator.evaluate();
-    //                 int z;
-    //                 if (!(isinf(result) || isnan(result))) {
-    //                     z = round(result);
-    //                     Cube::set_status(i, j, z, 3);
-    //                 }
-    //                 else {
-    //                     z = 0;
-    //                 }
-    //                 // Serial.print(x);
-    //                 // Serial.print(" ");
-    //                 // Serial.print(y);
-    //                 // Serial.print(" ");
-    //                 // Serial.println(z);
-    //             }
-    //         }
-    //         delay(1000);
-    //     }
-    // }
+        if (abs(SensorReader::acceleration.x) +
+                abs(SensorReader::acceleration.y) +
+                abs(SensorReader::acceleration.z) >
+            120.0) {
+            x_offset = 0;
+            y_offset = 0;
+            z_offset = 0;
+            zoom = 1;
+        }
+    }
 
-    // // int z = round(calculator.evaluate());
-    // // Serial.println(z);
+    Serial.print("Read: ");
+    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);
+    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;
+            double result = calculator.evaluate();
+            int z;
+            if (!(isinf(result) || isnan(result))) {
+                z = round((result + z_offset) * zoom);
+                Cube::set_status(i, j, z, 3);
+            }
+            else {
+                z = 0;
+            }
+
+            // Serial.print(x);
+            // Serial.print(" ");
+            // Serial.print(y);
+            // Serial.print(" ");
+            // Serial.println(z);
+
+            // Display origin point
+            Cube::set_status(round(-x_offset), round(-y_offset),
+                             round(-z_offset), 3);
+            Cube::set_blinking(round(-x_offset), round(-y_offset),
+                               round(-z_offset));
+        }
+    }
+
+    // delay(1000);
+
+    // int z = round(calculator.evaluate());
+    // Serial.println(z);
     // Serial.print("Here");
+}
+
+ISR(TIMER4_COMPA_vect) {
+    Cube::do_blinking();
+}
+
+ISR(TIMER5_COMPA_vect) {
+    SensorReader::read_data_from_sensor_interrupt();
 }