acceleration graph and fire

This commit is contained in:
Sophie Schiller 2021-02-14 17:42:09 +01:00
parent be8e71507b
commit 13b4aa1d4b
2 changed files with 154 additions and 155 deletions

View file

@ -12,4 +12,6 @@
platform = atmelavr platform = atmelavr
board = uno board = uno
framework = arduino framework = arduino
lib_deps = fastled/FastLED@^3.4.0 lib_deps =
rfetick/MPU6050_light@^1.1.0
fastled/FastLED@^3.4.0

View file

@ -5,189 +5,186 @@
#include <Wire.h> #include <Wire.h>
#include <FastLED.h> #include <FastLED.h>
#include <MPU6050_light.h>
#define LED_PIN 7 #define LED_PIN 7
#define NUM_LEDS 20
const int MPU = 0x68; // MPU6050 I2C address const int mode = 2; // 1 for acceleration, 2 for fire
float AccX, AccY, AccZ; const int debug = 2;
float GyroX, GyroY, GyroZ; const int STRIPS = 2;
float accAngleX, accAngleY, accCombined, gyroAngleX, gyroAngleY, gyroAngleZ; const int NUM_LEDS = 15;
float roll, pitch, yaw; const int FRAMES_PER_SECOND = 30;
float AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ; const float range = 0.5; //accelleration range in g
float elapsedTime, currentTime, previousTime;
int c = 0;
boolean debug = true;
CRGB leds[NUM_LEDS]; const int BRIGHTNESS = 50;
const int COOLING = 80;
const int SPARKING = 50;
float accelerationHistory [STRIPS];
MPU6050 mpu(Wire);
CRGB leds[NUM_LEDS * STRIPS];
CRGBPalette16 gPal;
void setup() { void setup() {
Serial.begin(19200); Serial.begin(19200);
//setup LEDs //setup LEDs
FastLED.addLeds<WS2812, LED_PIN, GRB>(leds, NUM_LEDS); FastLED.addLeds<WS2812, LED_PIN, GRB>(leds, NUM_LEDS*STRIPS).setCorrection( TypicalLEDStrip );
FastLED.setBrightness( BRIGHTNESS );
//setup IMU gPal = CRGBPalette16( CRGB::Black, CRGB::Red, CRGB::Yellow, CRGB::White);
Wire.begin(); // Initialize comunication //gPal = CRGBPalette16( CRGB::Black, CRGB::Blue, CRGB::Aqua, CRGB::White);
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
// Configure Accelerometer Sensitivity - Full Scale Range (default +/- 2g) for (int i = 0; i<STRIPS; i++) {
/*Wire.beginTransmission(MPU); accelerationHistory[i] = 1;
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);
*/
delay(20);
// Call this function if you need to get the IMU error values for your module Wire.begin();
//calculate_IMU_error(); byte status = mpu.begin();
delay(200); Serial.print(F("MPU6050 status: "));
Serial.println(status);
while(status!=0){ } // stop everything if could not connect to MPU6050
Serial.println(F("Calculating offsets, do not move MPU6050"));
delay(1000);
mpu.calcOffsets(true,true); // gyro and accelero
Serial.println("Done!\n");
} }
void calculateOrientationData() { float calculateOrientationData() {
// === Read acceleromter data === // mpu.update();
Wire.beginTransmission(MPU); float accCombined = sqrt(pow(mpu.getAccX(), 2) + pow(mpu.getAccY(), 2) + pow(mpu.getAccZ(), 2));
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.50; // 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) + 1.98; // AccErrorY ~(-1.58)
accCombined = sqrt(pow(AccX, 2) + pow(AccY, 2) + pow(AccZ, 2));
// === 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
GyroX = (Wire.read() << 8 | Wire.read()) / 131.0; // For a 250deg/s range we have to divide first the raw value by 131.0, according to the datasheet
GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;
GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;
// Correct the outputs with the calculated error values
GyroX = GyroX + 1.42; // GyroErrorX ~(-0.56)
GyroY = GyroY - 0.51; // GyroErrorY ~(2)
GyroZ = GyroZ + 1.00; // 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;
// Print the values on the serial monitor // Print the values on the serial monitor
if (debug) { if (debug <= 1) {
Serial.print("Acceleration: "); Serial.print(F("TEMPERATURE: "));Serial.println(mpu.getTemp());
Serial.print(accCombined, 3); Serial.print(F("ACCELERO X: "));Serial.print(mpu.getAccX());
Serial.print("\tY: ");Serial.print(mpu.getAccY());
Serial.print("\tZ: ");Serial.println(mpu.getAccZ());
Serial.print(F("GYRO X: "));Serial.print(mpu.getGyroX());
Serial.print("\tY: ");Serial.print(mpu.getGyroY());
Serial.print("\tZ: ");Serial.println(mpu.getGyroZ());
Serial.print(F("ACC ANGLE X: "));Serial.print(mpu.getAccAngleX());
Serial.print("\tY: ");Serial.println(mpu.getAccAngleY());
Serial.print(F("ANGLE X: "));Serial.print(mpu.getAngleX());
Serial.print("\tY: ");Serial.print(mpu.getAngleY());
Serial.print("\tZ: ");Serial.println(mpu.getAngleZ());
}
if (debug <= 2) {
Serial.print("ACC COMBINED: ");
Serial.println(accCombined, 3);
}
return accCombined;
}
void drawAccelerationOnStrip(int strip, float acceleration){ // 0 1
if (debug <= 2) {
Serial.print("drawing strip ");
Serial.print(strip);
Serial.print("\t"); Serial.print("\t");
Serial.print("Orientation: "); Serial.print(acceleration, 4);
Serial.print("\t"); Serial.print("\t");
Serial.print(roll); }
int stripStart = NUM_LEDS * strip; // 10*0 0
if (debug <= 2) {
Serial.print(stripStart);
Serial.print("\t"); Serial.print("\t");
Serial.print(pitch); }
int ledcutoff = stripStart + int((NUM_LEDS/(2*range))*acceleration - (NUM_LEDS/2)); // 0+(10/(2*0,5)*1-10/2) 5
if (debug <= 2) {
Serial.print(ledcutoff);
Serial.print("\t"); Serial.print("\t");
Serial.println(yaw); }
int stripEnd = (NUM_LEDS * (strip + 1)) - 1; // (10*(0+1))-1 9
if (debug <= 2) {
Serial.println(stripEnd);
}
if ((ledcutoff < stripStart) || (ledcutoff > stripEnd)){
for (int i = stripStart; i <= stripEnd; i++) {
leds[i] = CRGB(0, 0, 64);
}
}
else {
for (int i = stripStart; i <= ledcutoff; i++) {
leds[i] = CRGB(64, 0, 0);
}
for (int i = ledcutoff; i <= stripEnd; i++) {
leds[i] = CRGB(0, 64, 0);
}
} }
} }
void enableLEDsOnAcceleration(){ void enableLEDsOnAcceleration(float accCombined){
int ledcutoff = int(accCombined * NUM_LEDS / 2); // draw all stored accelerations
if(debug) { for (int i = 0; i < STRIPS; i++){
Serial.print("["); drawAccelerationOnStrip(i, accelerationHistory[i]);
} }
for (int i = 0; i <= ledcutoff; i++) { //shift them to the front
leds[i] = CRGB(64, 0, 0); for (int i = 0; i < STRIPS-1; i++) {
if (debug) { accelerationHistory[i] = accelerationHistory[i+1];
Serial.print("|");
} }
//add new entry at the end
accelerationHistory[STRIPS-1] = accCombined;
}
void calculateFire(int strip){
// Array of temperature readings at each simulation cell
static byte heat[STRIPS][NUM_LEDS];
// Step 1. Cool down every cell a little
for( int i = 0; i < NUM_LEDS; i++) {
heat[strip][i] = qsub8( heat[strip][i], random8(0, ((COOLING * 10) / NUM_LEDS) + 2));
} }
for (int i = ledcutoff; i <= NUM_LEDS ; i++) {
leds[i] = CRGB(0, 64, 0); // Step 2. Heat from each cell drifts 'up' and diffuses a little
if (debug) { for( int k= NUM_LEDS - 1; k >= 2; k--) {
Serial.print("-"); heat[strip][k] = (heat[strip][k - 1] + heat[strip][k - 2] + heat[strip][k - 2] ) / 3;
} }
// Step 3. Randomly ignite new 'sparks' of heat near the bottom
if( random8() < SPARKING ) {
int y = random8(7);
heat[strip][y] = qadd8( heat[strip][y], random8(160,255) );
} }
if(debug) {
Serial.println("]"); // Step 4. Map from heat cells to LED colors
for( int j = 0; j < NUM_LEDS; j++) {
// Scale the heat value from 0-255 down to 0-240
// for best results with color palettes.
byte colorindex = scale8( heat[strip][j], 240);
CRGB color = ColorFromPalette( gPal, colorindex);
int pixelnumber = (strip * NUM_LEDS) + j;
leds[pixelnumber] = color;
}
}
void drawFire(){
for (int i = 0; i < STRIPS; i++){
calculateFire(i);
} }
FastLED.show();
} }
void loop() { void loop() {
if (mode == 1) {
// === Read acceleromter data === // // === Read acceleromter data === //
calculateOrientationData(); float accCombined = calculateOrientationData();
enableLEDsOnAcceleration(); enableLEDsOnAcceleration(accCombined);
delay(20); FastLED.show();
FastLED.delay(1000 / FRAMES_PER_SECOND);
}
else if (mode == 2) {
random16_add_entropy( random());
drawFire();
FastLED.show();
FastLED.delay(1000 / FRAMES_PER_SECOND);
}
} }
void calculate_IMU_error() {
// We can call this funtion in the setup section to calculate the accelerometer and gyro data error. From here we will get the error values used in the above equations printed on the Serial Monitor.
// Note that we should place the IMU flat in order to get the proper values, so that we then can the correct values
// Read accelerometer values 200 times
while (c < 200) {
Wire.beginTransmission(MPU);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true);
AccX = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
AccY = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
// Sum all readings
AccErrorX = AccErrorX + ((atan((AccY) / sqrt(pow((AccX), 2) + pow((AccZ), 2))) * 180 / PI));
AccErrorY = AccErrorY + ((atan(-1 * (AccX) / sqrt(pow((AccY), 2) + pow((AccZ), 2))) * 180 / PI));
c++;
}
//Divide the sum by 200 to get the error value
AccErrorX = AccErrorX / 200;
AccErrorY = AccErrorY / 200;
c = 0;
// Read gyro values 200 times
while (c < 200) {
Wire.beginTransmission(MPU);
Wire.write(0x43);
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true);
GyroX = Wire.read() << 8 | Wire.read();
GyroY = Wire.read() << 8 | Wire.read();
GyroZ = Wire.read() << 8 | Wire.read();
// Sum all readings
GyroErrorX = GyroErrorX + (GyroX / 131.0);
GyroErrorY = GyroErrorY + (GyroY / 131.0);
GyroErrorZ = GyroErrorZ + (GyroZ / 131.0);
c++;
}
//Divide the sum by 200 to get the error value
GyroErrorX = GyroErrorX / 200;
GyroErrorY = GyroErrorY / 200;
GyroErrorZ = GyroErrorZ / 200;
// Print the error values on the Serial Monitor
Serial.print("AccErrorX: ");
Serial.println(AccErrorX);
Serial.print("AccErrorY: ");
Serial.println(AccErrorY);
Serial.print("GyroErrorX: ");
Serial.println(GyroErrorX);
Serial.print("GyroErrorY: ");
Serial.println(GyroErrorY);
Serial.print("GyroErrorZ: ");
Serial.println(GyroErrorZ);
}