diff --git a/trash.ino b/trash.ino new file mode 100644 index 0000000..0af470f --- /dev/null +++ b/trash.ino @@ -0,0 +1,361 @@ +#include +#include +#include +#include "I2Cdev.h" +// #include "MPU6050_6Axis_MotionApps20.h" +#include "MPU6050_6Axis_MotionApps612.h" + +DisplaySSD1306_128x64_I2C display(-1); +MPU6050 mpu; + +// Simulation constants +#define SIM_WIDTH 128 +#define SIM_HEIGHT 64 +#define NUM_PARTICLES 1000 +#define PARTICLE_RADIUS 2 +#define GRAVITY 1.0f +#define DAMPING 0.4f +#define PRESSURE_RADIUS 3.5f +#define PRESSURE_FORCE 0.9f +#define MAX_VEL 1.0f +#define IMU_ADDRESS 0x68 +#define OUTPUT_READABLE_YAWPITCHROLL +#define LED 2 + +// I2C device found at address 0x3C ! // OLED +// I2C device found at address 0x68 ! // IMU + +typedef float f32 __attribute__((aligned(4))); +struct Particle { + f32 x, y; + f32 vx, vy; +}; + + +inline float fast_sqrt(float x) { + union { float f; uint32_t i; } u; + u.f = x; + u.i = 0x5f375a86 - (u.i >> 1); + return u.f * (1.5f - 0.5f * x * u.f * u.f); +} + +Particle particles[NUM_PARTICLES]; +uint8_t canvasData[SIM_WIDTH*(SIM_HEIGHT/8)]; // because of 1bit display, not RGB +NanoCanvas1 canvas(SIM_WIDTH, SIM_HEIGHT, canvasData); + + +#define GRID_SIZE 16 +#define CELL_SIZE (SIM_WIDTH/GRID_SIZE) + +int const INTERRUPT_PIN = 18; +/*---MPU6050 Control/Status Variables---*/ +bool DMPReady = false; // Set true if DMP init was successful +uint8_t MPUIntStatus; // Holds actual interrupt status byte from MPU +uint8_t devStatus; // Return status after each device operation (0 = success, !0 = error) +uint16_t packetSize; // Expected DMP packet size (default is 42 bytes) +uint8_t FIFOBuffer[64]; // FIFO storage buffer +/*---Orientation/Motion Variables---*/ +Quaternion q; // [w, x, y, z] Quaternion container +VectorInt16 aa; // [x, y, z] Accel sensor measurements +VectorInt16 gy; // [x, y, z] Gyro sensor measurements +VectorInt16 aaReal; // [x, y, z] Gravity-free accel sensor measurements +VectorInt16 aaWorld; // [x, y, z] World-frame accel sensor measurements +VectorFloat gravity; // [x, y, z] Gravity vector +uint16_t fifoCount; +float euler[3]; // [psi, theta, phi] Euler angle container +float ypr[3]; // [yaw, pitch, roll] Yaw/Pitch/Roll container and gravity vector + +/*------Interrupt detection routine------*/ +volatile bool MPUInterrupt = true; // Indicates whether MPU6050 interrupt pin has gone high +void DMPDataReady() { + MPUInterrupt = true; +} + + + +struct GridCell { + uint8_t particles[10]; + uint8_t count; +}; + +GridCell grid[GRID_SIZE][GRID_SIZE]; + +bool blinkState; + +void buildSpatialGrid() { + memset(grid, 0, sizeof(grid)); + + for(int i=0; i= GRID_SIZE) continue; + if(gy+dy < 0 || gy+dy >= GRID_SIZE) continue; + + GridCell &cell = grid[gx+dx][gy+dy]; + for(int c=0; c 0.01f) { + const float dist = fast_sqrt(dist_sq) + 0.001f; + const float force = PRESSURE_FORCE * (1.0f - dist/PRESSURE_RADIUS); + + // Only apply horizontal forces to preserve gravity + particles[i].vx -= force * dx/dist; + particles[j].vx += force * dx/dist; + + // Reduce vertical force impact + particles[i].vy -= force * dy/dist * 0.3f; + particles[j].vy += force * dy/dist * 0.3f; + } + } + } + } + } + + // Gravity and movement + for(int i=0; i= SIM_WIDTH-1) { + particles[i].vx *= -DAMPING; + particles[i].x = constrain(particles[i].x, 1, SIM_WIDTH-2); + } + + // Y-axis + if(particles[i].y <= 0 || particles[i].y >= SIM_HEIGHT-1) { + particles[i].vy *= -DAMPING; + particles[i].y = constrain(particles[i].y, 1, SIM_HEIGHT-2); + } + } +} + +// Direct canvas buffer access (replace drawParticles) +void drawParticles() { + // Clear canvas by direct memory access + memset(canvasData, 0, sizeof(canvasData)); + // canvas.clear(); + for(int i=0; i(particles[i].x + 0.5f), 0, SIM_WIDTH-1); + int y = constrain(static_cast(particles[i].y + 0.5f), 0, SIM_HEIGHT-1); + + #if PARTICLE_RADIUS == 1 + canvas.putPixel(x, y); + #else + canvas.drawCircle(x,y,PARTICLE_RADIUS-1); + #endif + } + + display.drawCanvas(0,0,canvas); +} + +void reportIMU() { + if (!DMPReady) return; // Stop the program if DMP programming fails. + if (!MPUInterrupt) return; + MPUIntStatus = mpu.getIntStatus(); + /* Read a packet from FIFO */ + fifoCount = mpu.getFIFOCount(); + // check for overflow (this should never happen unless our code is too inefficient) + if ((MPUIntStatus & 0x10) || fifoCount == 1024) { + // reset so we can continue cleanly + mpu.resetFIFO(); + Serial.println(F("FIFO overflow!, giro descompensat!!")); + Serial.print("&"); + // otherwise, check for DMP data ready interrupt (this should happen frequently) + } + else if (MPUIntStatus & 0x02) { + if (mpu.dmpGetCurrentFIFOPacket(FIFOBuffer)) { // Get the Latest packet + #ifdef OUTPUT_READABLE_YAWPITCHROLL + /* Display Euler angles in degrees */ + mpu.dmpGetQuaternion(&q, FIFOBuffer); + mpu.dmpGetGravity(&gravity, &q); + mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); + Serial.print("ypr\t"); + Serial.print(ypr[0] * 180/M_PI); + Serial.print("\t"); + Serial.print(ypr[1] * 180/M_PI); + Serial.print("\t"); + Serial.println(ypr[2] * 180/M_PI); + #endif + + #ifdef OUTPUT_READABLE_QUATERNION + /* Display Quaternion values in easy matrix form: [w, x, y, z] */ + mpu.dmpGetQuaternion(&q, FIFOBuffer); + Serial.print("quat\t"); + Serial.print(q.w); + Serial.print("\t"); + Serial.print(q.x); + Serial.print("\t"); + Serial.print(q.y); + Serial.print("\t"); + Serial.println(q.z); + #endif + #ifdef OUTPUT_READABLE_WORLDACCEL + /* Display initial world-frame acceleration, adjusted to remove gravity + and rotated based on known orientation from Quaternion */ + mpu.dmpGetQuaternion(&q, FIFOBuffer); + mpu.dmpGetAccel(&aa, FIFOBuffer); + mpu.dmpGetGravity(&gravity, &q); + mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); + mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q); + Serial.print("aworld\t"); + Serial.print(aaWorld.x); + Serial.print("\t"); + Serial.print(aaWorld.y); + Serial.print("\t"); + Serial.println(aaWorld.z); + #endif + + /* Blink LED to indicate activity */ + blinkState = !blinkState; + digitalWrite(LED, blinkState); + } + } + MPUInterrupt = false; +} + + +void loop() { + static uint32_t last_frame = 0; + //lcd_delay(5000); + drawParticles(); + //delay(1000); + if(millis() - last_frame >= 33) { + last_frame = millis(); + applyPhysics(); + reportIMU(); + } else { + lcd_delay(millis() - last_frame); + } +}