Kostka LED 4x4x4 z żyroskopem to interaktywny projekt, który wykorzystuje czujnik MPU6050 oraz diody LED do stworzenia efektownej kostki świetlnej, reagującej na zmiany orientacji.
1. Płytka Arduino UNO
2. 64 diody LED jednokolorowe
3. 2 rejestry przesuwne 74HC595
4. 4 rezystory 500 ohm
5. Drut do kostki
6. Alternatywnie zestaw ICSK059A
7. Czujnik MPU6050
Komputer
Drukarka 3D
Lutownica
Projekt składa się z fizycznej struktury w postaci kostki LED 4x4x4 oraz mikrokontrolera, który kontroluje jej zachowanie. Wykonanie projektu umożliwił zestaw ICSK059A, jednak można go zastąpić samodzielnie zlutowanymi diodami. Zestaw wykorzystuje 64 prostokątne diody LED, które są sterowane za pomocą Arduino. Płytka rozszerzeniowa używa dwóch układów rejestrów przesuwnych 74HC595, dzięki czemu przy użyciu ośmiu wejść/wyjść można sterować kostką świetlną 4x4x4. Zapotrzebowanie na zasoby sprzętowe jest mniejsze niż w przypadku innych płytek rozszerzeń kostek świetlnych 4x4x4 (które potrzebują 20 wejść/wyjść do sterowania).
Za pomocą czujnika żyroskopu (MPU6050), projekt monitoruje zmiany orientacji kostki w przestrzeni. Diody LED w kostce reagują na zmiany orientacji poprzez przesypywanie się światełek w odpowiednich kierunkach.
Opis kodu:
Obsługa czujnika MPU6050:
Kod zawarty w skrypcie odpowiedzialny jest za efektywną komunikację z czujnikiem MPU6050, wykorzystującą interfejs I2C. Inicjalizacja komunikacji odbywa się poprzez wywołanie funkcji Wire.begin() oraz konfiguracja czujnika za pomocą odpowiednich rejestrów. Następnie, w pętli głównej, odczytywane są dane z akcelerometru i żyroskopu, które są niezbędne do obliczenia kątów przechylenia, pochylenia i odchylenia. Algorytm opiera się na precyzyjnym przetwarzaniu danych surowych, a także zastosowaniu filtru komplementarnego, który integruje dane z obu czujników, zapewniając dokładne pomiary kątów.
Sterowanie diodami LED za pomocą rejestrów przesuwnych:
Kolejną istotną częścią kodu jest kontrola diod LED przy użyciu rejestrów przesuwnych. Funkcja turnOnLed jest odpowiedzialna za włączanie i wyłączanie diod na podstawie przetworzonych danych z czujnika MPU6050. Dzięki zastosowaniu rejestrów przesuwnych możliwe jest efektywne sterowanie wieloma diodami przy minimalnym zużyciu zasobów mikrokontrolera. Ponadto, diody LED są włączane lub wyłączane w zależności od kątów przechylenia i pochylenia, co pozwala na wizualizację zmian orientacji urządzenia w czasie rzeczywistym.
Obliczenia matematyczne dla przekształceń kątowych:
Kod zawiera zaawansowane obliczenia matematyczne, które umożliwiają dokładne przekształcenie danych z czujnika MPU6050 na kąty przechylenia, pochylenia i odchylenia. Funkcja angleMultiplication wykorzystuje przekształcenia macierzowe, aby uwzględnić kąty roll, pitch i yaw, co umożliwia precyzyjne śledzenie orientacji urządzenia w przestrzeni trójwymiarowej. Przekształcenia te są kluczowe dla wielu zastosowań, takich jak stabilizacja lotu dronów, nawigacja pojazdów autonomicznych czy kontrola wirtualnej rzeczywistości.
Sortowanie wektorów względem osi Z:
Ostatnim istotnym elementem kodu jest sortowanie wektorów na podstawie ich współrzędnej Z. Funkcja sortVectorsByZumożliwia efektywną wizualizację danych związanych z orientacją urządzenia, poprzez uporządkowanie wyświetlanych elementów na podstawie ich położenia w przestrzeni trójwymiarowej. Posortowane wektory są następnie wykorzystywane do sterowania diodami LED, co pozwala na dynamiczne odwzorowanie zmian orientacji urządzenia w czasie rzeczywistym.
Dzięki zastosowaniu zaawansowanych algorytmów przetwarzania danych oraz efektywnemu sterowaniu diodami LED, kod ten może znaleźć zastosowanie w wielu aplikacjach wymagających monitorowania orientacji i ruchu w przestrzeni trójwymiarowej.
#include <Wire.h> //for I2C communication
// ------------ Cube ---------------
#define _BV(a) (0x0001 << (a))
static unsigned int HC_595_Temp; //74HC595write_temp
// define pins and other variables
const int latchPin = 1;
const int clockPin = 0;
const int dataPin = 3;
const int LED_Pin16= 4;
const int LED_Pin17= 5;
const int LED_Pin18= 6;
const int LED_Pin19= 7;
// ------------- MPU6050 ---------------
const int MPU = 0x68; // MPU6050 I2C address
float AccX, AccY, AccZ;
float GyroX, GyroY, GyroZ;
float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;
float roll, pitch, yaw;
float elapsedTime, currentTime, previousTime;
float start[64][3];
int end[64][3];
int counter;
// setup the correct pins and initialize SPI library
void setup() {
// setup pins
pinMode(latchPin, OUTPUT);
pinMode(clockPin, OUTPUT);
pinMode(dataPin, OUTPUT);
pinMode( LED_Pin16, OUTPUT );
pinMode( LED_Pin17, OUTPUT );
pinMode( LED_Pin18, OUTPUT );
pinMode( LED_Pin19, OUTPUT );
digitalWrite(LED_Pin16,LOW);
digitalWrite(LED_Pin17,LOW);
digitalWrite(LED_Pin18,LOW);
digitalWrite(LED_Pin19,LOW);
//------------------ MPU6050 part
Wire.begin(); // Initialize comunication
Wire.beginTransmission(MPU); // Start communication with MPU6050 // MPU=0x68
Wire.write(0x6B); // Talk to the register 6B
Wire.write(0x00); // Make reset - place a 0 into the 6B register
Wire.endTransmission(true); //end the transmission
//Serial.begin(9600); //Causes error
// Configure Accelerometer Sensitivity - Full Scale Range (default +/- 2g)
Wire.beginTransmission(MPU);
Wire.write(0x1C); //Talk to the ACCEL_CONFIG register (1C hex)
Wire.write(0x10); //Set the register bits as 00010000 (+/- 8g full scale range)
Wire.endTransmission(true);
// Configure Gyro Sensitivity - Full Scale Range (default +/- 250deg/s)
Wire.beginTransmission(MPU);
Wire.write(0x1B); // Talk to the GYRO_CONFIG register (1B hex)
Wire.write(0x10); // Set the register bits as 00010000 (1000deg/s full scale)
Wire.endTransmission(true);
counter = 0;
delay(20);
}
void loop() {
// --------------- MPU6050 ------------------
// === Read acceleromter data === //
Wire.beginTransmission(MPU);
Wire.write(0x3B); // Start with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true); // Read 6 registers total, each axis value is stored in 2 registers
//For a range of +-2g, we need to divide the raw values by 16384, according to the datasheet
AccX = (Wire.read() << 8 | Wire.read()) / 16384.0; // X-axis value
AccY = (Wire.read() << 8 | Wire.read()) / 16384.0; // Y-axis value
AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0; // Z-axis value
// Calculating Roll and Pitch from the accelerometer data
accAngleX = (atan(AccY / sqrt(pow(AccX, 2) + pow(AccZ, 2))) * 180 / PI) - 0.77; // AccErrorX ~(0.58) See the calculate_IMU_error()custom function for more details
accAngleY = (atan(-1 * AccX / sqrt(pow(AccY, 2) + pow(AccZ, 2))) * 180 / PI) - 0.50; // AccErrorY ~(-1.58)
// === Read gyroscope data === //
previousTime = currentTime; // Previous time is stored before the actual time read
currentTime = millis(); // Current time actual time read
elapsedTime = (currentTime - previousTime) / 1000; // Divide by 1000 to get seconds
Wire.beginTransmission(MPU);
Wire.write(0x43); // Gyro data first register address 0x43
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true); // Read 4 registers total, each axis value is stored in 2 registers
// For a 250deg/s range we have to divide first the raw value by 131.0, according to the datasheet
GyroX = (Wire.read() << 8 | Wire.read()) / 131.0 + 0.37; // GyroErrorX ~(-0.56)
GyroY = (Wire.read() << 8 | Wire.read()) / 131.0 - 0.18; // GyroErrorY ~(2
GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;
GyroZ = GyroZ - 0.22; // GyroErrorZ ~ (-0.8)
// Currently the raw values are in degrees per seconds, deg/s, so we need to multiply by sendonds (s) to get the angle in degrees
gyroAngleX = gyroAngleX + GyroX * elapsedTime; // deg/s * s = deg
gyroAngleY = gyroAngleY + GyroY * elapsedTime;
yaw = yaw + GyroZ * elapsedTime;
// Complementary filter - combine acceleromter and gyro angle values
roll = 0.96 * gyroAngleX + 0.04 * accAngleX;
pitch = 0.96 * gyroAngleY + 0.04 * accAngleY;
//Serial.print(roll);
//Serial.println(" ");
if(counter % 25 == 0){
elementary(start, 64);
elementary_int(end, 64);
angleMultiplication();
sortVectorsByZ(start, end, 64);
}
for(int i = 0; i < 16; i++){
turnOnLed(end[i][0],end[i][1],end[i][2],HIGH);
turnOnLed(end[i][0],end[i][1],end[i][2],LOW);
}
if(counter>20000){
counter = 0;
}
counter += 1;
}
// turn on correct LED using 595's
void turnOnLed(int x, int y, int z, int charge) {
digitalWrite(latchPin, LOW);
int plane = x*10+y;
if(charge == LOW)
{
switch (plane)
{
case 11: HC_595_Temp &=~_BV(15); break;
case 12: HC_595_Temp &=~_BV(14); break;
case 13: HC_595_Temp &=~_BV(13); break;
case 14: HC_595_Temp &=~_BV(12); break;
case 21: HC_595_Temp &=~_BV(11); break;
case 22: HC_595_Temp &=~_BV(10); break;
case 23: HC_595_Temp &=~_BV(9); break;
case 24: HC_595_Temp &=~_BV(8); break;
case 31: HC_595_Temp &=~_BV(7); break;
case 32: HC_595_Temp &=~_BV(6); break;
case 33: HC_595_Temp &=~_BV(5); break;
case 34: HC_595_Temp &=~_BV(4); break;
case 41: HC_595_Temp &=~_BV(3); break;
case 42: HC_595_Temp &=~_BV(2); break;
case 43: HC_595_Temp &=~_BV(1); break;
case 44: HC_595_Temp &=~_BV(0); break;
}
/*digitalWrite(LED_Pin16,LOW);
digitalWrite(LED_Pin17,LOW);
digitalWrite(LED_Pin18,LOW);
digitalWrite(LED_Pin19,LOW);*/
}
else
{
switch (plane)
{
case 11: HC_595_Temp |=_BV(15); break;
case 12: HC_595_Temp |=_BV(14); break;
case 13: HC_595_Temp |=_BV(13); break;
case 14: HC_595_Temp |=_BV(12); break;
case 21: HC_595_Temp |=_BV(11); break;
case 22: HC_595_Temp |=_BV(10); break;
case 23: HC_595_Temp |=_BV(9); break;
case 24: HC_595_Temp |=_BV(8); break;
case 31: HC_595_Temp |=_BV(7); break;
case 32: HC_595_Temp |=_BV(6); break;
case 33: HC_595_Temp |=_BV(5); break;
case 34: HC_595_Temp |=_BV(4); break;
case 41: HC_595_Temp |=_BV(3); break;
case 42: HC_595_Temp |=_BV(2); break;
case 43: HC_595_Temp |=_BV(1); break;
case 44: HC_595_Temp |=_BV(0); break;
}
switch(z){
case 1:
digitalWrite(LED_Pin16,HIGH);
digitalWrite(LED_Pin17,HIGH);
digitalWrite(LED_Pin18,HIGH);
digitalWrite(LED_Pin19,LOW);
break;
case 2:
digitalWrite(LED_Pin16,HIGH);
digitalWrite(LED_Pin17,HIGH);
digitalWrite(LED_Pin18,LOW);
digitalWrite(LED_Pin19,HIGH);
break;
case 3:
digitalWrite(LED_Pin16,HIGH);
digitalWrite(LED_Pin17,LOW);
digitalWrite(LED_Pin18,HIGH);
digitalWrite(LED_Pin19,HIGH);
break;
case 4:
digitalWrite(LED_Pin16,LOW);
digitalWrite(LED_Pin17,HIGH);
digitalWrite(LED_Pin18,HIGH);
digitalWrite(LED_Pin19,HIGH);
break;
}
}
shiftOut(dataPin, clockPin, LSBFIRST, HC_595_Temp);
shiftOut(dataPin, clockPin, LSBFIRST, (HC_595_Temp >> 8));
digitalWrite(latchPin, HIGH);
}
void angleMultiplication(){
//RAZY WSZPOLCZYNNIKI
float RadPitch = DegToRad(roll*2.5);
float RadRoll = DegToRad(-pitch*3.2);
float RadYaw = DegToRad(-yaw*1.5);
float PitchRx[3][3]={
{1,0,0},
{0,cos(RadPitch),-sin(RadPitch)},
{0,sin(RadPitch),cos(RadPitch)}
};
float RollRy[3][3]={
{cos(RadRoll),0,sin(RadRoll)},
{0,1,0},
{-sin(RadRoll),0,cos(RadRoll)}
};
float YawRz[3][3]={
{cos(RadYaw),-sin(RadYaw),0},
{sin(RadYaw),cos(RadYaw),0},
{0,0,1}
};
float RxRy[3][3];
float RxRyRz[3][3];
MatrixMult(PitchRx, RollRy, RxRy);
MatrixMult(RxRy, YawRz, RxRyRz);
for(int m = 0; m<64; m++){
MatrixVectorMult(RxRyRz, start, m);
}
}
float DegToRad (float deg){
return deg*0.01745;
}
void MatrixMult(float A[][3], float B[][3], float C[][3]){
//C = AB
for(int i = 0; i < 3; i++){
for(int j = 0; j < 3; j++){
C[i][j] = 0;
for(int k = 0; k < 3; k++){
C[i][j] += A[i][k]*B[k][j];
}
}
}
}
void MatrixVectorMult(float A[][3], float (*b)[3], int m){
//c = Ab
float c[3];
for(int i = 0; i < 3; i++){
c[i] = 0;
for(int j = 0; j < 3; j++){
c[i] += b[m][j]*A[i][j];
}
b[m][i]=c[i]; //Nie jestem pewny czy to nie musi byc w oddzielnej petle
}
}
void sortVectorsByZ(float (*copyA)[3], int (*copyB)[3], int rows) {
for (int i = 0; i < rows - 1; i++) {
for (int j = 0; j < rows - i - 1; j++) {
if (copyA[j][2] > copyA[j + 1][2]) {
// Swap the vectors
for (int k = 0; k < 3; k++) {
float temp = copyA[j][k];
copyA[j][k] = copyA[j + 1][k];
copyA[j + 1][k] = temp;
temp = copyB[j][k];
copyB[j][k] = copyB[j + 1][k];
copyB[j + 1][k] = temp;
}
}
}
}
}
void printMatrix(int (*matrix)[3], int rows) {
for (int i = 0; i < rows; i++) {
for (int j = 0; j < 3; j++) {
Serial.print(matrix[i][j]);
Serial.print(" ");
}
Serial.println();
}
}
void elementary(float (*matrix)[3], int rows){
int n = 0;
for(int i = 1; i < 5; i++){
for(int j = 1; j < 5; j++){
for(int k = 1; k < 5; k++){
matrix[n][0] = i;
matrix[n][1] = j;
matrix[n][2] = k;
n ++;
}
}
}
}
void elementary_int(int (*matrix)[3], int rows){
int n = 0;
for(int i = 1; i < 5; i++){
for(int j = 1; j < 5; j++){
for(int k = 1; k < 5; k++){
matrix[n][0] = i;
matrix[n][1] = j;
matrix[n][2] = k;
n ++;
}
}
}
}https://medium.com/@tiemenwaterreus/driving-16-leds-using-only-three-pins-of-an-arduino-3a6b1f60bc2c
Building a 4x4x4 LED Cube. Part I: The hardware:
https://medium.com/@tiemenwaterreus/building-a-4x4x4-led-cube-part-i-the-hardware-8a7bca9fc043
Building a 4x4x4 LED Cube. Part II: The software:
https://medium.com/@tiemenwaterreus/building-a-4x4x4-led-cube-part-ii-the-software-813a5207bca8
How to Multiplex an LED Grid:
https://www.youtube.com/watch?v=MmCGDJ90Qt4&list=LL&index=11&t=131s
Arduino and MPU6050 Accelerometer and Gyroscope Tutorial:
https://howtomechatronics.com/tutorials/arduino/arduino-and-mpu6050-accelerometer-and-gyroscope-tutorial/