658 lines
19 KiB
C++
658 lines
19 KiB
C++
#include "ListE.h"
|
|
#include "TimerOne.h"
|
|
#include <string.h>
|
|
|
|
#include "REG.h"
|
|
#include "wit_c_sdk.h"
|
|
|
|
#define TIME_PER_LAYER_IN_US 800
|
|
#define BUS_START_PIN 22
|
|
#define CLOCK_START_PIN 30
|
|
#define SW_START_PIN 38
|
|
|
|
#define ACC_UPDATE 0x01
|
|
#define GYRO_UPDATE 0x02
|
|
#define ANGLE_UPDATE 0x04
|
|
#define MAG_UPDATE 0x08
|
|
#define READ_UPDATE 0x80
|
|
|
|
#define DEBUG false
|
|
|
|
struct Triple {
|
|
float x;
|
|
float y;
|
|
float z;
|
|
};
|
|
|
|
class SensorReader {
|
|
private:
|
|
static const uint32_t c_uiBaud[8];
|
|
static volatile byte s_cDataUpdate;
|
|
static Triple angle;
|
|
|
|
static void AutoScanSensor() {
|
|
int iRetry;
|
|
|
|
for (int i = 0; i < sizeof(SensorReader::c_uiBaud) /
|
|
sizeof(SensorReader::c_uiBaud[0]);
|
|
i++) {
|
|
Serial1.begin(SensorReader::c_uiBaud[i]);
|
|
Serial1.flush();
|
|
iRetry = 2;
|
|
SensorReader::s_cDataUpdate = 0;
|
|
do {
|
|
WitReadReg(AX, 3);
|
|
delay(200);
|
|
while (Serial1.available()) {
|
|
WitSerialDataIn(Serial1.read());
|
|
}
|
|
if (SensorReader::s_cDataUpdate != 0) {
|
|
Serial.print(SensorReader::c_uiBaud[i]);
|
|
Serial.print(" baud find sensor\r\n\r\n");
|
|
return;
|
|
}
|
|
iRetry--;
|
|
} while (iRetry);
|
|
}
|
|
Serial.println("Can not find sensor, please check your connection");
|
|
}
|
|
|
|
static void SensorUartSend(uint8_t *p_data, uint32_t uiSize) {
|
|
Serial1.write(p_data, uiSize);
|
|
Serial1.flush();
|
|
}
|
|
|
|
static void SensorDataUpdata(uint32_t uiReg, uint32_t uiRegNum) {
|
|
int i;
|
|
for (i = 0; i < uiRegNum; i++) {
|
|
switch (uiReg) {
|
|
case AZ:
|
|
SensorReader::s_cDataUpdate |= ACC_UPDATE;
|
|
break;
|
|
case GZ:
|
|
SensorReader::s_cDataUpdate |= GYRO_UPDATE;
|
|
break;
|
|
case HZ:
|
|
SensorReader::s_cDataUpdate |= MAG_UPDATE;
|
|
break;
|
|
case Yaw:
|
|
SensorReader::s_cDataUpdate |= ANGLE_UPDATE;
|
|
break;
|
|
default:
|
|
SensorReader::s_cDataUpdate |= READ_UPDATE;
|
|
break;
|
|
}
|
|
uiReg++;
|
|
}
|
|
}
|
|
|
|
static void Delayms(uint16_t ucMs) {
|
|
delay(ucMs);
|
|
}
|
|
|
|
public:
|
|
static bool init() {
|
|
WitInit(WIT_PROTOCOL_NORMAL, 0x50);
|
|
WitSerialWriteRegister(SensorReader::SensorUartSend);
|
|
WitRegisterCallBack(SensorReader::SensorDataUpdata);
|
|
WitDelayMsRegister(SensorReader::Delayms);
|
|
SensorReader::AutoScanSensor();
|
|
|
|
if (WitSetUartBaud(WIT_BAUD_9600) != WIT_HAL_OK) {
|
|
Serial.println("Set Baud Error");
|
|
return false;
|
|
}
|
|
else {
|
|
Serial1.begin(SensorReader::c_uiBaud[WIT_BAUD_9600]);
|
|
Serial.println("9600 Baud rate modified successfully");
|
|
}
|
|
|
|
if (WitSetContent(RSW_ANGLE) != WIT_HAL_OK) {
|
|
Serial.print("\r\nSet Content Angle Error\r\n");
|
|
return false;
|
|
}
|
|
else {
|
|
Serial.println("Set Content Angle success");
|
|
}
|
|
return true;
|
|
}
|
|
|
|
static void read_data_from_sensor_interrupt() {
|
|
while (Serial1.available()) {
|
|
WitSerialDataIn(Serial.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 =
|
|
sReg[Roll + 2] / 32768.0f * 180.0f; // Unit: deg
|
|
|
|
SensorReader::s_cDataUpdate &= ~ANGLE_UPDATE;
|
|
|
|
SensorReader::s_cDataUpdate = 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
static Triple get_angle() {
|
|
return SensorReader::angle;
|
|
}
|
|
};
|
|
|
|
class Cube {
|
|
private:
|
|
static int layer_count;
|
|
static int brightness_count;
|
|
|
|
public:
|
|
// Every int represents a row of 8 LED status.
|
|
static int LED_status[8][8];
|
|
|
|
static void display() {
|
|
// Serial.println("Here");
|
|
if (brightness_count >= 2) {
|
|
digitalWrite(SW_START_PIN + layer_count, LOW);
|
|
layer_count = (layer_count + 1) % 8;
|
|
}
|
|
|
|
brightness_count = (brightness_count + 1) % 3;
|
|
|
|
for (int i = 0; i < 8; i++) {
|
|
for (int j = 0; j < 8; j++) {
|
|
// In LED_status:
|
|
// 0 = off
|
|
// 4 = brightest
|
|
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);
|
|
}
|
|
|
|
digitalWrite(SW_START_PIN + layer_count, HIGH);
|
|
}
|
|
|
|
static void set_status(int x, int y, int z, int brightness) {
|
|
if (x >= 8 || x < 0 || y >= 8 || y < 0 || z >= 8 || z < 0) {
|
|
return;
|
|
}
|
|
brightness %= 4;
|
|
LED_status[z][x] =
|
|
(LED_status[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;
|
|
}
|
|
|
|
static void clear() {
|
|
for (int i = 0; i < 8; i++) {
|
|
for (int j = 0; j < 8; j++) {
|
|
LED_status[i][j] = 0;
|
|
}
|
|
}
|
|
}
|
|
};
|
|
|
|
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};
|
|
|
|
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;
|
|
|
|
double x, y;
|
|
|
|
const String allowed_chars = "0123456789+-*/^()xy";
|
|
|
|
void setup() {
|
|
for (int i = 22; i < 46; i++) {
|
|
pinMode(i, OUTPUT);
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
void loop() {
|
|
Triple angle;
|
|
|
|
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);
|
|
|
|
delay(500);
|
|
|
|
// 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 == 0x3) {
|
|
// break;
|
|
// }
|
|
// }
|
|
// }
|
|
// Serial.print("Read: ");
|
|
// Serial.println(input);
|
|
// Calculator calculator(input);
|
|
// int x_offset, y_offset;
|
|
|
|
// for (x_offset = 0; x_offset < 4; x_offset++) {
|
|
// for (y_offset = 0; y_offset < 4; y_offset++) {
|
|
// Cube::clear();
|
|
|
|
// 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);
|
|
// }
|
|
// }
|
|
|
|
// // int z = round(calculator.evaluate());
|
|
// // Serial.println(z);
|
|
// Serial.print("Here");
|
|
} |