1338 lines
39 KiB
C++
1338 lines
39 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 JOYSTICK_VRX A1
|
|
#define JOYSTICK_VRY A0
|
|
#define JOYSTICK_SWITCH 2
|
|
|
|
#define ACC_UPDATE 0x01
|
|
#define GYRO_UPDATE 0x02
|
|
#define ANGLE_UPDATE 0x04
|
|
#define MAG_UPDATE 0x08
|
|
#define READ_UPDATE 0x80
|
|
|
|
#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;
|
|
float y;
|
|
float z;
|
|
};
|
|
|
|
class SensorReader {
|
|
private:
|
|
static const uint32_t c_uiBaud[8];
|
|
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) /
|
|
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.println(" baud find sensor");
|
|
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 Triple angle, acceleration, gyro;
|
|
|
|
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]);
|
|
if (DEBUG) {
|
|
Serial.println("9600 Baud rate modified successfully");
|
|
}
|
|
}
|
|
|
|
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_5HZ) != WIT_HAL_OK) {
|
|
Serial.print("Set report rate failed");
|
|
return false;
|
|
}
|
|
else {
|
|
if (DEBUG) {
|
|
Serial.println("Set report rate success");
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
static void read_data_from_sensor_interrupt() {
|
|
// Serial.println("Updated!");
|
|
while (Serial1.available()) {
|
|
WitSerialDataIn(Serial1.read());
|
|
}
|
|
if (SensorReader::s_cDataUpdate) {
|
|
if (SensorReader::s_cDataUpdate & ANGLE_UPDATE) {
|
|
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;
|
|
}
|
|
|
|
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;
|
|
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 {
|
|
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 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");
|
|
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));
|
|
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;
|
|
}
|
|
}
|
|
|
|
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 {
|
|
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 (TRACE) {
|
|
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 (TRACE) {
|
|
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 (TRACE) {
|
|
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 (TRACE) {
|
|
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 (TRACE) {
|
|
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();
|
|
}
|
|
};
|
|
|
|
enum Modes {
|
|
CalculateAndDraw,
|
|
Cube,
|
|
Rain,
|
|
Clock,
|
|
Words,
|
|
};
|
|
|
|
int Cube::layer_count = 0;
|
|
int Cube::brightness_count = 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, SensorReader::acceleration, SensorReader::gyro;
|
|
|
|
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 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;
|
|
|
|
void setup() {
|
|
for (int i = 22; i < 46; i++) {
|
|
pinMode(i, OUTPUT);
|
|
}
|
|
|
|
pinMode(JOYSTICK_SWITCH, INPUT_PULLUP);
|
|
pinMode(JOYSTICK_VRX, INPUT);
|
|
pinMode(JOYSTICK_VRY, INPUT);
|
|
|
|
Timer1.initialize();
|
|
Timer1.setPeriod(TIME_PER_LAYER_IN_US);
|
|
|
|
Serial.begin(9600);
|
|
|
|
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 = 6500 / 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);
|
|
|
|
// timer 5 is for read sensor data
|
|
TCCR5A = 0;
|
|
TCCR5B = 0;
|
|
TCNT5 = 0;
|
|
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;
|
|
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() {
|
|
if (can_change_mode) {
|
|
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;
|
|
}
|
|
}
|
|
|
|
ISR(TIMER4_COMPA_vect) {
|
|
Cube::do_blinking();
|
|
}
|
|
|
|
ISR(TIMER5_COMPA_vect) {
|
|
SensorReader::read_data_from_sensor_interrupt();
|
|
}
|
|
|
|
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 (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() {
|
|
return;
|
|
}
|
|
|
|
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;
|
|
need_reevaluate = true;
|
|
waiting_for_command = false;
|
|
}
|
|
else if (SensorReader::angle.x - x_ref < -40.0) {
|
|
if (DEBUG) {
|
|
Serial.println("b");
|
|
}
|
|
y_offset -= 1;
|
|
need_reevaluate = true;
|
|
waiting_for_command = false;
|
|
}
|
|
|
|
if (SensorReader::angle.y - y_ref > 40.0) {
|
|
if (DEBUG) {
|
|
Serial.println("c");
|
|
}
|
|
x_offset -= 1;
|
|
need_reevaluate = true;
|
|
waiting_for_command = false;
|
|
}
|
|
else if (SensorReader::angle.y - y_ref < -40.0) {
|
|
if (DEBUG) {
|
|
Serial.println("d");
|
|
}
|
|
x_offset += 1;
|
|
need_reevaluate = true;
|
|
waiting_for_command = false;
|
|
}
|
|
|
|
if (SensorReader::angle.z - z_ref > 40.0) {
|
|
if (DEBUG) {
|
|
Serial.println(SensorReader::angle.z);
|
|
Serial.println("e");
|
|
}
|
|
zoom /= 2.0;
|
|
need_reevaluate = true;
|
|
waiting_for_command = false;
|
|
}
|
|
else if (SensorReader::angle.z - z_ref < -40.0) {
|
|
if (DEBUG) {
|
|
Serial.println(SensorReader::angle.z);
|
|
Serial.println("f");
|
|
}
|
|
zoom *= 2.0;
|
|
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");
|
|
}
|
|
}
|
|
|
|
if (!need_reevaluate || input.length() == 0) {
|
|
return;
|
|
}
|
|
|
|
Serial.print("Expression to be evaluated: ");
|
|
Serial.println(input);
|
|
|
|
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++) {
|
|
symbol_x = (i + x_offset) * zoom;
|
|
symbol_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));
|
|
}
|
|
}
|
|
}
|
|
|
|
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;
|
|
}
|
|
}
|
|
|
|
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");
|
|
}
|
|
} |