Merge manually.

This commit is contained in:
unlockable
2023-09-08 13:59:18 +08:00
parent 35eb89d5a9
commit 9c67837956

View File

@@ -159,11 +159,49 @@ private:
static int layer_count;
static int brightness_count;
static bool blinking_LED_status;
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");
@@ -179,7 +217,9 @@ public:
// In LED_status:
// 0 = off
// 4 = brightest
digitalWrite(BUS_START_PIN + j, ((LED_status[layer_count][i] >> (j * 2)) & 3) >= (3 - brightness_count));
digitalWrite(BUS_START_PIN + j,
((LED_status[layer_count][i] >> (j * 2)) & 3) >=
(3 - brightness_count));
}
digitalWrite(CLOCK_START_PIN + i, HIGH);
digitalWrite(CLOCK_START_PIN + i, LOW);
@@ -193,39 +233,609 @@ public:
return;
}
brightness %= 4;
LED_status[z][x] = (LED_status[z][x] & (~(3 << (y * 2)))) | (brightness << (y * 2));
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_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 {
plus,
minus,
multiply,
divide,
power,
left_parenthesis,
right_parenthesis,
};
enum CompareResult {
less,
equal,
greater,
};
class Symbol {
private:
bool is_symbol;
String name;
double value;
public:
static double *x_value_ptr, *y_value_ptr;
Symbol(String symbol_name) {
this->is_symbol = true;
this->name = symbol_name;
}
Symbol(double new_value) {
this->is_symbol = false;
this->value = new_value;
}
double get_value() const {
if (this->is_symbol) {
if (this->name == "x") {
return *x_value_ptr;
}
else if (this->name == "y") {
return *y_value_ptr;
}
}
return this->value;
}
};
class Operator {
private:
Operators name;
public:
Operator(const char op) {
if ('+' == op) {
this->name = plus;
}
else if ('-' == op) {
this->name = minus;
}
else if ('*' == op) {
this->name = multiply;
}
else if ('/' == op) {
this->name = divide;
}
else if ('^' == op) {
this->name = power;
}
else if ('(' == op) {
this->name = left_parenthesis;
}
else if (')' == op) {
this->name = right_parenthesis;
}
}
CompareResult compare(const Operator &other_operator) {
int self_priority, other_priority;
if (this->name == plus || this->name == minus) {
self_priority = 2;
}
else if (this->name == multiply || this->name == divide) {
self_priority = 4;
}
else if (this->name == power) {
self_priority = 6;
}
else if (this->name == left_parenthesis) {
self_priority = 8;
}
else if (this->name == right_parenthesis) {
self_priority = 1;
}
if (other_operator.name == plus || other_operator.name == minus) {
other_priority = 3;
}
else if (other_operator.name == multiply ||
other_operator.name == divide) {
other_priority = 5;
}
else if (other_operator.name == power) {
other_priority = 7;
}
else if (other_operator.name == left_parenthesis) {
other_priority = 1;
}
else if (other_operator.name == right_parenthesis) {
other_priority = 9;
}
if (self_priority < other_priority) {
return less;
}
else if (self_priority == other_priority) {
return equal;
}
else {
return greater;
}
}
Symbol calc(const Symbol first_pop, const Symbol second_pop) {
if (this->name == plus) {
return Symbol(first_pop.get_value() + second_pop.get_value());
}
else if (this->name == minus) {
return Symbol(second_pop.get_value() - first_pop.get_value());
}
else if (this->name == multiply) {
return Symbol(first_pop.get_value() * second_pop.get_value());
}
else if (this->name == divide) {
if (abs(first_pop.get_value()) <= 1e-5) {
return Symbol(atof("inf"));
}
return Symbol(second_pop.get_value() / first_pop.get_value());
}
else if (this->name == power) {
return Symbol(pow(second_pop.get_value(), first_pop.get_value()));
}
return 0.0;
}
bool operator==(Operators op) {
return this->name == op;
}
char display() {
switch (this->name) {
case plus:
return '+';
case minus:
return '-';
case multiply:
return '*';
case divide:
return '/';
case power:
return '^';
case left_parenthesis:
return '(';
case right_parenthesis:
return ')';
}
}
};
class Calculator {
private:
String expression;
List<Symbol> number_stack;
List<Operator> operator_stack;
int left_parenthesis_count;
void calculate() {
Operator operation = this->operator_stack.pop();
// this->operator_stack.pop();
Symbol number1 = this->number_stack.pop();
// this->number_stack.pop();
Symbol number2 = this->number_stack.pop();
// this->number_stack.pop();
this->number_stack.append(operation.calc(number1, number2));
}
public:
Calculator(const String expression) {
this->expression = expression + ')';
}
double evaluate() {
this->number_stack.clear();
this->operator_stack.clear();
this->operator_stack.append(Operator('('));
this->left_parenthesis_count = 1;
char operator_chars[] = "+-*/^()";
int operator_pos = -1;
while (true) {
if (DEBUG) {
Serial.println("A");
Serial.print("Operator pos: ");
Serial.println(operator_pos);
Serial.println("Number stack: ");
for (int i = 0; i < this->number_stack.length(); i++) {
Serial.println(this->number_stack[i].get_value());
}
Serial.println("Operator stack: ");
for (int i = 0; i < this->operator_stack.length(); i++) {
Serial.println(this->operator_stack[i].display());
}
}
bool found_next_operator = false;
int first_char_of_num = operator_pos + 1;
// operator_pos =
// this->expression.indexOf(operator_chars, operator_pos + 1);
operator_pos = this->expression.length();
for (int i = 0; i < sizeof(operator_chars); i++) {
int current_operator_pos = this->expression.indexOf(
operator_chars[i], first_char_of_num);
if (current_operator_pos < operator_pos &&
current_operator_pos >= 0) {
found_next_operator = true;
operator_pos = current_operator_pos;
}
}
if (!found_next_operator) {
break;
}
// Find the operator position
if (DEBUG) {
Serial.println("B");
Serial.print("Operator pos: ");
Serial.println(operator_pos);
Serial.println("Number stack: ");
for (int i = 0; i < this->number_stack.length(); i++) {
Serial.println(this->number_stack[i].get_value());
}
Serial.println("Operator stack: ");
for (int i = 0; i < this->operator_stack.length(); i++) {
Serial.println(this->operator_stack[i].display());
}
}
if (operator_pos != first_char_of_num) {
String number =
this->expression.substring(first_char_of_num, operator_pos);
if (number == "x" || number == "y") {
this->number_stack.append(Symbol(number));
}
else {
this->number_stack.append(Symbol(atof(number.c_str())));
}
}
else if (this->expression[operator_pos] == '-' &&
this->number_stack.length() ==
this->operator_stack.length() -
this->left_parenthesis_count) {
this->number_stack.append(0.0);
}
// Understand the number and push to stack
if (DEBUG) {
Serial.println("C");
Serial.print("Operator pos: ");
Serial.println(operator_pos);
Serial.println("Number stack: ");
for (int i = 0; i < this->number_stack.length(); i++) {
Serial.println(this->number_stack[i].get_value());
}
Serial.println("Operator stack: ");
for (int i = 0; i < this->operator_stack.length(); i++) {
Serial.println(this->operator_stack[i].display());
}
}
Operator current_operator(this->expression[operator_pos]);
if (current_operator == left_parenthesis) {
this->left_parenthesis_count += 1;
}
// Understand the operator
if (DEBUG) {
Serial.println("D");
Serial.print("Read operator: ");
Serial.println(current_operator.display());
Serial.print("Operator pos: ");
Serial.println(operator_pos);
Serial.println("Number stack: ");
for (int i = 0; i < this->number_stack.length(); i++) {
Serial.println(this->number_stack[i].get_value());
}
Serial.println("Operator stack: ");
for (int i = 0; i < this->operator_stack.length(); i++) {
Serial.println(this->operator_stack[i].display());
}
}
switch (current_operator.compare(this->operator_stack.peek())) {
case greater:
// Serial.println("Choose greater");
this->operator_stack.append(current_operator);
break;
case equal:
// Serial.println("Choose equal");
this->operator_stack.pop();
this->left_parenthesis_count -= 1;
break;
case less:
// Serial.println("Choose less");
if (this->number_stack.length() < 2) {
// Error
return atof("nan");
}
// Serial.println("Into Calc");
this->calculate();
// Serial.println("Out Calc");
// We only dealt with previous operators. Need to go through
// this operator again.
operator_pos--;
break;
}
if (DEBUG) {
Serial.println("E");
Serial.print("Operator pos: ");
Serial.println(operator_pos);
Serial.println("Number stack: ");
for (int i = 0; i < this->number_stack.length(); i++) {
Serial.println(this->number_stack[i].get_value());
}
Serial.println("Operator stack: ");
for (int i = 0; i < this->operator_stack.length(); i++) {
Serial.println(this->operator_stack[i].display());
}
}
}
if (this->left_parenthesis_count != 0) {
return 0.0;
}
return this->number_stack.peek().get_value();
}
};
int Cube::layer_count = 0;
int Cube::brightness_count = 0;
int Cube::LED_status[8][8] = {0};
int bright = 3;
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, 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);
Serial.begin(9600);
Symbol::x_value_ptr = &x;
Symbol::y_value_ptr = &y;
if (!SensorReader::init()) {
Serial.println("Initialize sensor failed.");
}
Timer1.attachInterrupt(Cube::display);
Serial.begin(115200);
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() {
Serial.print("Zref: ");
Serial.println(z_ref);
for (int i = 7; i >= 0; i--) {
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 == 0x3) {
break;
}
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 (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;
}
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;
}
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;
}
}
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++) {
for (int k = 0; k < 8; k++) {
Cube::set_status(i, j, k, bright);
delay(5);
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));
}
}
}
bright = (bright + 1) % 4;
// delay(1000);
// int z = round(calculator.evaluate());
// Serial.println(z);
// Serial.print("Here");
}
ISR(TIMER4_COMPA_vect) { Cube::do_blinking(); }
ISR(TIMER4_COMPA_vect) {
Cube::do_blinking();
}
ISR(TIMER5_COMPA_vect) {
SensorReader::read_data_from_sensor_interrupt();
}