Skip to content

Instantly share code, notes, and snippets.

@EDISON-SCIENCE-CORNER

EDISON-SCIENCE-CORNER/sand.ino

Created April 8, 2019 09:33
Show Gist options
  • Save EDISON-SCIENCE-CORNER/73473a4c97b6499929e87c85c4bb2dff to your computer and use it in GitHub Desktop.
Save EDISON-SCIENCE-CORNER/73473a4c97b6499929e87c85c4bb2dff to your computer and use it in GitHub Desktop.
Download ZIP
Raw
sand.ino
#include <Wire.h>
#include "LedControl.h"
LedControl lc=LedControl(12,10,11,4); // Pins: DIN,CLK,CS, # of Display connected
struct point
{
int y, x;
int aX, aY;
};
#define INIT_VALUE 32000
#define NUMBEROFSAND 50
point points[NUMBEROFSAND];
char pointsCounter = 1;
//IMU adress
#define MPU 0x68
//Used convertion ratio
#define A_R 16384.0
#define G_R 131.0
//Rad to degree convertion ratio 180/PI
#define RAD_A_DEG = 57.295779
//MPU-6050 gives 16 bits integrer values
//Start values
int16_t AcX, AcY, AcZ, GyX, GyY, GyZ;
//Start values
float Acc[2]; //16 bits values of the acceleration
float Gy[2]; //16 bits values of the gyro
float Angle[2]; //16 bits values of the angle
void setup()
{
randomSeed(analogRead(0));
Wire.begin();
Wire.beginTransmission(MPU);
Wire.write(0x6B); //i2c adress
Wire.write(0);
Wire.endTransmission(true);
Serial.begin(9600);
lc.shutdown(0,false); // Wake up displays
lc.shutdown(1,false);
lc.shutdown(2,false);
lc.shutdown(3,false);
lc.setIntensity(1,0);
lc.clearDisplay(0); // Clear Displays
lc.clearDisplay(1);
lc.clearDisplay(2);
lc.clearDisplay(3);
for(char i = 0; i < NUMBEROFSAND; i++)
{
points[i].x = INIT_VALUE;
points[i].y = INIT_VALUE;
}
points[0].x = 10;
points[0].y = 4;
points[0].aX = 0;
points[0].aY = 0;
}
void loop()
{
static long loopCounter = 0;
//We read the acceleration values from the module
Wire.beginTransmission(MPU);
Wire.write(0x3B); //we ask for 0x3B register - correspond to AcX
Wire.endTransmission(false);
Wire.requestFrom(MPU,6,true); //From 0x3B, we ask for 6 register
AcX=Wire.read()<<8|Wire.read(); //Each values ahs 2 bytes
AcY=Wire.read()<<8|Wire.read();
AcZ=Wire.read()<<8|Wire.read();
//We apply mathematics in order to calculate the acceleration
//This are the applied formulas
Acc[1] = atan(-1*(AcX/A_R)/sqrt(pow((AcY/A_R),2) + pow((AcZ/A_R),2)))*RAD_TO_DEG;
Acc[0] = atan((AcY/A_R)/sqrt(pow((AcX/A_R),2) + pow((AcZ/A_R),2)))*RAD_TO_DEG;
//We read data from the gyto register
Wire.beginTransmission(MPU);
Wire.write(0x43);
Wire.endTransmission(false);
Wire.requestFrom(MPU,4,true); //In this case we only ask for 4 register
GyX=Wire.read()<<8|Wire.read();
GyY=Wire.read()<<8|Wire.read();
//We calculate the angle
Gy[0] = GyX/G_R;
Gy[1] = GyY/G_R;
//We apply the complementary filter
Angle[0] = 0.98 *(Angle[0]+Gy[0]*0.010) + 0.02*Acc[0];
Angle[1] = 0.98 *(Angle[1]+Gy[1]*0.010) + 0.02*Acc[1];
//Printe the values using serial. Open serial monitor to see the values.
//Uncoment the values you want to serial print
//Serial.print("Y angle: "); Serial.print(Angle[1]);
//2//Serial.print(" "); Serial.print("X angle: "); Serial.print(Angle[0]);
Serial.print(" "); Serial.print("X acceleration: "); Serial.print(AcY);
//Serial.print(" "); Serial.print("Y acceleration: "); Serial.print(AcX);
//5//Serial.print(" "); Serial.print("Gyro X: "); Serial.print(GyX);
//6//Serial.print(" "); Serial.print("Gyro Y: "); Serial.print(GyY);
Serial.print("\n"); //jump a line
movePoints();
drawPoints();
flushMatrix();
delay(50); //Use a delay if the serial is printing too fast
clearMatrix();
if(pointsCounter < NUMBEROFSAND && loopCounter % 2)
{
points[pointsCounter].x = 10;
points[pointsCounter].y = 4;
points[pointsCounter].aX = 0;
points[pointsCounter].aY = 0;
if(pointsCounter < NUMBEROFSAND )pointsCounter++;
}
loopCounter++;
}
void drawPoints()
{
for(char i=0; i < NUMBEROFSAND; i++)
{
if(points[i].x != INIT_VALUE)
{
drawPoint(points[i].x, points[i].y);
}
}
}
char getPointMostLeftTopFromArray(point *points, char start, char sizeOfArray)
{
char index;
char x = 32;
for(char i = start; i < sizeOfArray; i++)
{
if(points[i].x == INIT_VALUE) continue;
if(points[i].x < x)
{
x = points[i].x;
index = i;
}
}
for(char i = start; i < sizeOfArray; i++)
{
if(points[i].x == INIT_VALUE) continue;
if(points[index].x == points[i].x)
{
if(points[index].y > points[i].y)
{
index = i;
}
}
}
return index;
}
char getPointMostRightTopFromArray(point *points, char start, char sizeOfArray)
{
char index;
char x = 32;
char y = 0;
for(char i = start; i < sizeOfArray; i++)
{
if(points[i].x == INIT_VALUE) continue;
if(points[i].x < x)
{
x = points[i].x;
index = i;
}
}
for(char i = start; i < sizeOfArray; i++)
{
if(points[i].x == INIT_VALUE) continue;
if(points[index].x == points[i].x)
{
if(points[index].y < points[i].y)
{
index = i;
}
}
}
return index;
}
char getPointMostRightBottomFromArray(point *points, char start, char sizeOfArray)
{
char index;
char x = 0;
char y = 0;
for(char i = start; i < sizeOfArray; i++)
{
if(points[i].x == INIT_VALUE) continue;
if(points[i].x > x)
{
x = points[i].x;
index = i;
}
}
for(char i = start; i < sizeOfArray; i++)
{
if(points[i].x == INIT_VALUE) continue;
if(points[index].x == points[i].x)
{
if(points[index].y < points[i].y)
{
index = i;
}
}
}
return index;
}
char getPointMostLeftBottomFromArray(point *points, char start, char sizeOfArray)
{
char index;
char x = 0;
char y = 0;
for(char i = start; i < sizeOfArray; i++)
{
if(points[i].x == INIT_VALUE) continue;
if(points[i].x > x)
{
x = points[i].x;
index = i;
}
}
for(char i = start; i < sizeOfArray; i++)
{
if(points[i].x == INIT_VALUE) continue;
if(points[index].x == points[i].x)
{
if(points[index].y > points[i].y)
{
index = i;
}
}
}
return index;
}
bool collision( point *pointsArray, point point)
{
//Serial.print(point.y);Serial.print(" ");Serial.print(pointsArray[0].y);Serial.print(pointsArray[1].y);Serial.print(pointsArray[2].y); Serial.print("\n");
for(char i = 0; i < NUMBEROFSAND; i++)
{
if(point.x == pointsArray[i].x && point.y == pointsArray[i].y)
{
return true;
}
}
return false;
}
void movePoints()
{
point tempPoint;
if(AcY < 5000 && AcY > -5000)
{
// sort points array
for(char i = 0; i < NUMBEROFSAND-1; i++)
{
if(points[i].x == INIT_VALUE) continue;
char minIndex = getPointMostRightTopFromArray(points, i, NUMBEROFSAND);
if(i != minIndex)
{
tempPoint.x = points[i].x;
tempPoint.y = points[i].y;
points[i].x = points[minIndex].x;
points[i].y = points[minIndex].y;
points[minIndex].x = tempPoint.x;
points[minIndex].y = tempPoint.y;
}
}
for(char i = 0; i < NUMBEROFSAND-1; i++)
{
points[i].aX = 1;
points[i].aY = 0;
}
}
else if(AcY < -5000 && AcY > -10000)
{
// sort points array
for(char i = 0; i < NUMBEROFSAND-1; i++)
{
if(points[i].x == INIT_VALUE) continue;
char minIndex = getPointMostRightTopFromArray(points, i, NUMBEROFSAND);
if(i != minIndex)
{
tempPoint.x = points[i].x;
tempPoint.y = points[i].y;
points[i].x = points[minIndex].x;
points[i].y = points[minIndex].y;
points[minIndex].x = tempPoint.x;
points[minIndex].y = tempPoint.y;
}
}
for(char i = 0; i < NUMBEROFSAND-1; i++)
{
points[i].aX = 1;
points[i].aY = 1;
}
}
else if(AcY > 5000 && AcY < 10000)
{
// sort points array
for(char i = 0; i < NUMBEROFSAND-1; i++)
{
if(points[i].x == INIT_VALUE) continue;
char minIndex = getPointMostRightTopFromArray(points, i, NUMBEROFSAND);
if(i != minIndex)
{
tempPoint.x = points[i].x;
tempPoint.y = points[i].y;
points[i].x = points[minIndex].x;
points[i].y = points[minIndex].y;
points[minIndex].x = tempPoint.x;
points[minIndex].y = tempPoint.y;
}
}
for(char i = 0; i < NUMBEROFSAND-1; i++)
{
points[i].aX = 1;
points[i].aY = -1;
}
}
else if(AcY < -5000 )
{
// sort points array
for(char i = 0; i < NUMBEROFSAND-1; i++)
{
if(points[i].x == INIT_VALUE) continue;
char minIndex = getPointMostLeftBottomFromArray(points, i, NUMBEROFSAND);
if(i != minIndex)
{
tempPoint.x = points[i].x;
tempPoint.y = points[i].y;
points[i].x = points[minIndex].x;
points[i].y = points[minIndex].y;
points[minIndex].x = tempPoint.x;
points[minIndex].y = tempPoint.y;
}
}
for(char i = 0; i < NUMBEROFSAND-1; i++)
{
points[i].aX = 0;
points[i].aY = 1;
}
}
else if(AcY > 5000 )
{
// sort points array
for(char i = 0; i < NUMBEROFSAND-1; i++)
{
if(points[i].x == INIT_VALUE) continue;
char minIndex = getPointMostRightBottomFromArray(points, i, NUMBEROFSAND);
if(i != minIndex)
{
tempPoint.x = points[i].x;
tempPoint.y = points[i].y;
points[i].x = points[minIndex].x;
points[i].y = points[minIndex].y;
points[minIndex].x = tempPoint.x;
points[minIndex].y = tempPoint.y;
}
}
for(char i = 0; i < NUMBEROFSAND-1; i++)
{
points[i].aX = 0;
points[i].aY = -1;
}
}
for(char i = 0; i < NUMBEROFSAND; i++)
{
if(points[i].x == INIT_VALUE)continue;
movePoint(&points[i]);
}
}
void movePoint(point *p)
{
point newPoint;
if(p->aX == 1 && p->aY == 0)
{
if((p->x+1) < 32)
{
newPoint.x = p->x+1;
}
else return;
newPoint.y = p->y;
if(!collision(points, newPoint))
{
p->x = newPoint.x;
return;
}
else
{
if((p->y+1) < 8)
{
newPoint.y = p->y+1;
if(!collision(points, newPoint))
{
p->y = newPoint.y;
}
}
if((p->y-1) >= 0)newPoint.y = p->y-1;
else return;
if(!collision(points, newPoint))
{
if(random(2) == 0)
{
p->y = newPoint.y;
}
}
}
}
else if(p->aX == 1 && p->aY == 1)
{
if((p->x+1) < 32 && (p->y-1) >= 0)
{
newPoint.x = p->x+1;
newPoint.y = p->y;
if(collision(points, newPoint))
{
newPoint.y = p->y-1;
if(!collision(points, newPoint))
{
p->x = newPoint.x;
p->y = newPoint.y;
return;
}
}
}
if((p->x+1) < 32)
{
newPoint.x = p->x+1;
if(p->y+1 < 8)
{
newPoint.y = p->y+1;
if(!collision(points, newPoint))
{
p->y = newPoint.y;
p->x = newPoint.x;
return;
}
}
else
{
newPoint.y = p->y;
if(!collision(points, newPoint))
{
p->x = newPoint.x;
return;
}
}
}
if((p->y+1) < 8)
{
newPoint.y = p->y+1;
newPoint.x = p->x;
if(!collision(points, newPoint))
{
p->y = newPoint.y;
return;
}
}
}
else if(p->aX == 1 && p->aY == -1)
{
if((p->x+1) < 32 && (p->y+1) < 8)
{
newPoint.x = p->x+1;
newPoint.y = p->y;
if(collision(points, newPoint))
{
newPoint.y = p->y+1;
if(!collision(points, newPoint))
{
p->x = newPoint.x;
p->y = newPoint.y;
return;
}
}
}
if((p->x+1) < 32)
{
newPoint.x = p->x+1;
if(p->y-1 >= 0)
{
newPoint.y = p->y-1;
if(!collision(points, newPoint))
{
p->y = newPoint.y;
p->x = newPoint.x;
return;
}
}
else
{
newPoint.y = p->y;
if(!collision(points, newPoint))
{
p->x = newPoint.x;
return;
}
}
}
if((p->y-1) >= 0)
{
newPoint.y = p->y-1;
newPoint.x = p->x;
if(!collision(points, newPoint))
{
p->y = newPoint.y;
return;
}
}
}
else if(p->aX == 0 && p->aY == 1)
{
if((p->x-1) > 10 && (p->y+1) < 8)
{
newPoint.x = p->x-1;
newPoint.y = p->y+1;
if(!collision(points, newPoint))
{
p->y = newPoint.y;
p->x = newPoint.x;
return;
}
}
if((p->y+1) < 8)
{
newPoint.y = p->y+1;
newPoint.x = p->x;
if(!collision(points, newPoint))
{
p->y = newPoint.y;
return;
}
}
if((p->x+1) < 32)
{
newPoint.x = p->x+1;
newPoint.y = p->y;
if(!collision(points, newPoint))
{
p->x = newPoint.x;
return;
}
}
}
else if(p->aX == 0 && p->aY == -1)
{
if((p->x-1) > 10 && (p->y-1) >= 0)
{
newPoint.x = p->x-1;
newPoint.y = p->y-1;
if(!collision(points, newPoint))
{
p->y = newPoint.y;
p->x = newPoint.x;
return;
}
}
if((p->y-1) >= 0)
{
newPoint.y = p->y-1;
newPoint.x = p->x;
if(!collision(points, newPoint))
{
p->y = newPoint.y;
return;
}
}
if((p->x+1) < 32)
{
newPoint.x = p->x+1;
newPoint.y = p->y;
if(!collision(points, newPoint))
{
p->x = newPoint.x;
return;
}
}
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment