/*
3D Rotatey-Cube is placed under the MIT license
Copyleft (c+) 2016 tobozzo
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
This project is heavily inspired from the work of GoblinJuicer https://www.reddit.com/user/GoblinJuicer
who developed and implemented the idea.
Started from this sub https://www.reddit.com/r/arduino/comments/3vmw1k/ive_been_playing_with_a_gyroscope_and_an_lcd/
The code is mainly a remix to make it work with u8glib and mpu6050
Deps:
U8glib library grabbed from: https://github.com/olikraus/u8glib
I2Cdev library grabbed from: https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/I2Cdev
MPU650 library grabbed from: https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050
Pinout OLED and MPU (shared)
VCC => 5V
GND => GND
SCL => A5
SDA => A4
Pinout MPU
AD0 => GND
INT => D2
*/
#include "U8glib.h"
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
#define CLK 2
#define DT 3
#define SW 4
unsigned long lastButtonPress = 0;
int counter = 0;
int aState;
int aLastState;
bool scaleMode = true;
#define DEBUG true
int btnVarState = 0;
// Accel and gyro data
int16_t ax, ay, az, gx, gy, gz;
double MMPI = 1000*M_PI;
long int timeLast = -100, period = 1;
// Overall scale and perspective distance
double sZ = 4, scale = 16;
// screen center
uint8_t centerX = 64;
uint8_t centerY = 32;
// Initialize cube point arrays
double C1[] = { 1, 1, 1 };
double C2[] = { 1, 1, -1 };
double C3[] = { 1, -1, 1 };
double C4[] = { 1, -1, -1 };
double C5[] = { -1, 1, 1 };
double C6[] = { -1, 1, -1 };
double C7[] = { -1, -1, 1 };
double C8[] = { -1, -1, -1 };
// Initialize cube points coords
uint8_t P1[] = { 0, 0 };
uint8_t P2[] = { 0, 0 };
uint8_t P3[] = { 0, 0 };
uint8_t P4[] = { 0, 0 };
uint8_t P5[] = { 0, 0 };
uint8_t P6[] = { 0, 0 };
uint8_t P7[] = { 0, 0 };
uint8_t P8[] = { 0, 0 };
U8GLIB_SSD1306_128X64 u8g(U8G_I2C_OPT_DEV_0 | U8G_I2C_OPT_NO_ACK | U8G_I2C_OPT_FAST); // Fast I2C / TWI
MPU6050 mpu;
void setup() {
// assign default color value
// Initialize encoder pins
pinMode(CLK, INPUT);
pinMode(DT, INPUT);
pinMode(SW, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(CLK), readEncoder, FALLING);
if ( u8g.getMode() == U8G_MODE_R3G3B2 ) {
u8g.setColorIndex(255); // white
}
else if ( u8g.getMode() == U8G_MODE_GRAY2BIT ) {
u8g.setColorIndex(3); // max intensity
}
else if ( u8g.getMode() == U8G_MODE_BW ) {
u8g.setColorIndex(1); // pixel on
}
else if ( u8g.getMode() == U8G_MODE_HICOLOR ) {
u8g.setHiColorByRGB(255,255,255);
}
// join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
#if DEBUG == true
// initialize serial communication
Serial.begin(115200);
#endif
mpu.initialize();
if (mpu.dmpInitialize() == 0) {
// turn on the DMP, now that it's ready
mpu.setDMPEnabled(true);
// supply your own gyro offsets here, scaled for min sensitivity
mpu.setXGyroOffset(161);
mpu.setYGyroOffset(48);
mpu.setZGyroOffset(43);
mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
}
}
void readEncoder() {
int dtValue = digitalRead(DT);
if (dtValue == HIGH) {
counter++; // Clockwise
}
if (dtValue == LOW) {
counter--; // Counterclockwise
}
}
void loop() {
u8g.firstPage();
do {
cubeloop();
}
while( u8g.nextPage() );
}
void cubeloop() {
if (digitalRead(SW) == LOW) {
if (millis() - lastButtonPress > 50) {
Serial.println("Button pressed!");
counter = 0;
if(btnVarState < 3){
btnVarState ++;
}
else{
btnVarState = 0;
}
}
// Remember last button press event
lastButtonPress = millis();
}
switch (btnVarState) {
case 0: // your hand is on the sensor
scale = 16 + counter*0.5;
u8g.setFont(u8g_font_4x6);
u8g.drawStr(8,16,"Scale: ");
u8g.setPrintPos(8, 28);
u8g.print(scale);
break;
case 1: // your hand is close to the sensor
sZ = 4 + counter *0.5;
u8g.setFont(u8g_font_4x6);
u8g.drawStr(8,16,"Perspec: ");
u8g.setPrintPos(8, 28);
u8g.print(sZ);
break;
case 2: // your hand is a few inches from the sensor
centerX = 64 + counter;
u8g.setFont(u8g_font_4x6);
u8g.drawStr(8,16,"cX: ");
u8g.setPrintPos(8, 28);
u8g.print(centerX);
break;
case 3: // your hand is nowhere near the sensor
centerY = 32 + counter;
u8g.setFont(u8g_font_4x6);
u8g.drawStr(8,16,"cY: ");
u8g.setPrintPos(8, 28);
u8g.print(centerY);
break;
}
// switch(btnVarState) {
// case 0:
// scale = 16 + counter*0.5;
// u8g.setFont(u8g_font_4x6);
// u8g.drawStr(8,16,"Scale: ");
// u8g.setPrintPos(8, 28);
// u8g.print(scale);
// break;
// case 1:
// sZ = 4 + counter *0.5;
// u8g.setFont(u8g_font_4x6);
// u8g.drawStr(8,16,"Perspec: ");
// u8g.setPrintPos(8, 28);
// u8g.print(sZ);
// // statements
// break;
// case 2:
// // statements
// break;
// case 3:
// break;
// default:
// // default statements
// }
period = millis()- timeLast;
timeLast = millis();
// precalc
double MMPI_TIME = MMPI*period;
//Read gyro, apply calibration, ignore small values
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
// ignore low values (supply uour own values here, based on Serial console output)
if(abs(gx)<10){
gx = 0;
}
if(abs(gy)<30){
gy = 0;
}
if(abs(gz)<12){
gz = 0;
}
// scale angles down, rotate
vectRotXYZ((double)(gy/57.296)/MMPI_TIME, 1); // X
vectRotXYZ((double)(-gx/57.296)/MMPI_TIME, 2); // Y
vectRotXYZ((double)(gz/57.296)/MMPI_TIME, 3); // Z
#if DEBUG == true
Serial.print(scale);
Serial.print("\t");
Serial.print((uint8_t) gx);
Serial.print("\t");
Serial.print((uint8_t) gy);
Serial.print("\t");
Serial.println((uint8_t) gz);
#endif
// calculate each point coords
P1[0] = centerX + scale/(1+C1[2]/sZ)*C1[0]; P1[1] = centerY + scale/(1+C1[2]/sZ)*C1[1];
P2[0] = centerX + scale/(1+C2[2]/sZ)*C2[0]; P2[1] = centerY + scale/(1+C2[2]/sZ)*C2[1];
P3[0] = centerX + scale/(1+C3[2]/sZ)*C3[0]; P3[1] = centerY + scale/(1+C3[2]/sZ)*C3[1];
P4[0] = centerX + scale/(1+C4[2]/sZ)*C4[0]; P4[1] = centerY + scale/(1+C4[2]/sZ)*C4[1];
P5[0] = centerX + scale/(1+C5[2]/sZ)*C5[0]; P5[1] = centerY + scale/(1+C5[2]/sZ)*C5[1];
P6[0] = centerX + scale/(1+C6[2]/sZ)*C6[0]; P6[1] = centerY + scale/(1+C6[2]/sZ)*C6[1];
P7[0] = centerX + scale/(1+C7[2]/sZ)*C7[0]; P7[1] = centerY + scale/(1+C7[2]/sZ)*C7[1];
P8[0] = centerX + scale/(1+C8[2]/sZ)*C8[0]; P8[1] = centerY + scale/(1+C8[2]/sZ)*C8[1];
// draw each cube edge
u8g.drawLine(P1[0], P1[1], P2[0], P2[1]); //1-2
u8g.drawLine(P1[0], P1[1], P3[0], P3[1]); //1-3
u8g.drawLine(P1[0], P1[1], P5[0], P5[1]); //1-5
u8g.drawLine(P2[0], P2[1], P4[0], P4[1]); //2-4
u8g.drawLine(P2[0], P2[1], P6[0], P6[1]); //2-6
u8g.drawLine(P3[0], P3[1], P4[0], P4[1]); //3-4
u8g.drawLine(P3[0], P3[1], P7[0], P7[1]); //3-7
u8g.drawLine(P4[0], P4[1], P8[0], P8[1]); //4-8
u8g.drawLine(P5[0], P5[1], P6[0], P6[1]); //5-6
u8g.drawLine(P5[0], P5[1], P7[0], P7[1]); //5-7
u8g.drawLine(P6[0], P6[1], P8[0], P8[1]); //6-8
u8g.drawLine(P7[0], P7[1], P8[0], P8[1]); //7-8
}
void vectRotXYZ(double angle, int axe) {
int8_t m1; // coords polarity
uint8_t i1, i2; // coords index
switch(axe) {
case 1: // X
i1 = 1; // y
i2 = 2; // z
m1 = -1;
break;
case 2: // Y
i1 = 0; // x
i2 = 2; // z
m1 = 1;
break;
case 3: // Z
i1 = 0; // x
i2 = 1; // y
m1 = 1;
break;
}
double t1 = C1[i1];
double t2 = C1[i2];
C1[i1] = t1*cos(angle)+(m1*t2)*sin(angle);
C1[i2] = (-m1*t1)*sin(angle)+t2*cos(angle);
t1 = C2[i1];
t2 = C2[i2];
C2[i1] = t1*cos(angle)+(m1*t2)*sin(angle);
C2[i2] = (-m1*t1)*sin(angle)+t2*cos(angle);
t1 = C3[i1];
t2 = C3[i2];
C3[i1] = t1*cos(angle)+(m1*t2)*sin(angle);
C3[i2] = (-m1*t1)*sin(angle)+t2*cos(angle);
t1 = C4[i1];
t2 = C4[i2];
C4[i1] = t1*cos(angle)+(m1*t2)*sin(angle);
C4[i2] = (-m1*t1)*sin(angle)+t2*cos(angle);
t1 = C5[i1];
t2 = C5[i2];
C5[i1] = t1*cos(angle)+(m1*t2)*sin(angle);
C5[i2] = (-m1*t1)*sin(angle)+t2*cos(angle);
t1 = C6[i1];
t2 = C6[i2];
C6[i1] = t1*cos(angle)+(m1*t2)*sin(angle);
C6[i2] = (-m1*t1)*sin(angle)+t2*cos(angle);
t1 = C7[i1];
t2 = C7[i2];
C7[i1] = t1*cos(angle)+(m1*t2)*sin(angle);
C7[i2] = (-m1*t1)*sin(angle)+t2*cos(angle);
t1 = C8[i1];
t2 = C8[i2];
C8[i1] = t1*cos(angle)+(m1*t2)*sin(angle);
C8[i2] = (-m1*t1)*sin(angle)+t2*cos(angle);
}