Hello,
I found and downloaded the library for TimerOne and looked at the example codes it had, but am still not sure how I could implement it into the existing code I have. Also, I have added to the code since earlier as it still required data from two other sources to be averaged with the value I had from the MPU, so I will post the latest version, all I have so far this time as there could be other parts affecting it:
//included libraries
#include <Arduino.h>
#include <Adafruit_SHT31.h>
#include <I2Cdev.h>
#include <math.h>
#include <QMI8658C.h>
#include <Stepper.h>
#include <SR04.h>
#include <Servo.h>
#include <pitches.h>
#include <MFRC522.h>
#include <MPU6050.h>
#include <Keypad.h>
#include <LiquidCrystal.h>
#include <DS3231.h>
#include <dht_nonblocking.h>
#include "SevSeg.h"
#include <KalmanFilter.h> //library from Jarzebski's GitHub, same as with MPU library
#include <Wire.h>
// Thermistor parameters from TTC203 datasheet
#define RT0 20000 //thermistor resistance at 25°C, in Ω
#define B 4250 //beta coefficnent at 25°C
#define R 22000 //series resistor value in Ω
MPU6050 mpu;
SevSeg sevseg;
Adafruit_SHT31 sht31 = Adafruit_SHT31();
// Timers
const unsigned long mpuRepeatTime = 10; //interval of when MPU reads and writes gyro and accel data
const unsigned long tempRepeatTime = 1000; //interval of when MPU, Thermistor, and Sensiron SHT31-D read and write temperature data
unsigned long mpuPrevTime = 0;
unsigned long tempPrevTime = 0; //holds value of when temp was last read (in milliseconds)
float timeStep = 0.01; //needed to find yaw as doesn't use Kalman
//Kalman filter and MPU variables
KalmanFilter kalmanX(0.001, 0.003, 0.03);
KalmanFilter kalmanY(0.001, 0.003, 0.03);
float accPitch = 0;
float accRoll = 0;
float kalPitch = 0;
float kalRoll = 0;
float yaw = 0;
float RT, VR, ln, TX, T0, VRT; // Variables for thermistor calculations
float mpuTemp; //holds value of temperature according to the mpu
float t; //holds value of temperature from Adafruit Sensiron
float avgTemp; //average temperature value from the 3 sensors
int buzzerPin = 40; //assigning a pin to the buzzer
//Declarations for ultrasonic sensor
const int trigPin = 2;
const int echoPin = 3;
float duration, distance;
unsigned long previousSonicMillis = 0; // stores last time the ultrasonic was triggered
const long sonicInterval = 12; // interval at which to trigger a reset of sonic (milliseconds)
LiquidCrystal lcd(22, 23, 24, 25, 26, 27); //LCD Display Pin Assignment
//Servo Control Pin Assignment
int servoPin1 = 6;
Servo servoPitch;
int servoPin2 = 5;
Servo servoRoll;
int servoPin3 = 4;
Servo servoYaw;
float pos = 0;
void setup() {
// put your setup code here, to run once:
Serial.begin(115200);
// Initialize MPU6050
while (!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G)) {
Serial.println("Could not find a valid MPU6050 sensor, check wiring!");
delay(500);
}
if (! sht31.begin(0x44)) {
Serial.println("Couldn't find SHT31");
while (1) delay(1);
}
T0 = 25 + 273.15; // Convert T0 for thermistor from Celsius to Kelvin
pinMode(buzzerPin, OUTPUT);
//for ultrasonic sensor
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
mpu.calibrateGyro(); //calibrate the gyroscpoe while its at rest for a few seconds before beginning find and to read off data
mpu.setThreshold(3); //mpu threshold sensitivity (3 is the default value)
servoPitch.attach(servoPin1); // Attaches Pitch control servo to its specified pin
servoRoll.attach(servoPin2); // Attaches Roll control servo to its specified pin
servoYaw.attach(servoPin3); // Attaches Yaw control servo to its specified pin
//setting pins for LEDs
pinMode(47, OUTPUT);
pinMode(48, OUTPUT);
pinMode(49, OUTPUT);
pinMode(50, OUTPUT);
pinMode(51, OUTPUT);
pinMode(52, OUTPUT);
pinMode(53, OUTPUT);
//setting up 7 segment display
byte numDigits = 4;
byte digitPins[] = { 36, 37, 38, 39 };
byte segmentPins[] = { 35, 28, 29, 31, 32, 34, 33, 30 };
bool resistorsOnSegments = true;
byte hardwareConfig = COMMON_CATHODE;
sevseg.begin(hardwareConfig, numDigits, digitPins, segmentPins, resistorsOnSegments);
//Printing SID on LCD display
lcd.begin(16, 2); // Can now interact with the LCD, e.g.:
lcd.print("11162788");
delay(5000); // Waits 5 seconds
lcd.clear(); // Clear screen
}
void loop() {
unsigned long currentTime = millis();
//nested gyroscope-reading code within if statement
if (currentTime - mpuPrevTime >= mpuRepeatTime) {
// calculate and print Pitch, Roll, & Yaw Data data every 10ms
Vector acc = mpu.readNormalizeAccel(); //reading normalised values from the accelerometer
Vector gyr = mpu.readNormalizeGyro(); //reading normalised values from the gyroscope
// Calculate Pitch & Roll from accelerometer (in deg)
accPitch = (atan2(acc.XAxis, sqrt(acc.YAxis * acc.YAxis + acc.ZAxis * acc.ZAxis)) * 180.0) / M_PI;
accRoll = (atan2(acc.YAxis, acc.ZAxis) * 180.0) / M_PI;
// Apply Kalman filter
kalPitch = kalmanY.update(accPitch, gyr.YAxis);
kalRoll = kalmanX.update(accRoll, gyr.XAxis);
yaw = yaw + gyr.ZAxis * timeStep; //Calculate Yaw [cant be done with a Kalman Filter, would require a magnetometer]
// Output prints
Serial.print(" Pitch = ");
Serial.print(kalPitch);
Serial.print(" Roll = ");
Serial.print(kalRoll);
Serial.print(" Yaw = ");
Serial.println(yaw);
lcd.print("Pitch = ");
lcd.print(kalPitch, 2);
lcd.print(" ");
lcd.setCursor(0, 1); //moves LCD cursor to second line first row
lcd.print("Roll = ");
lcd.print(kalRoll, 2);
lcd.print(" ");
lcd.setCursor(0, 0); // moves LCD display cursor back to first position first line
//making different LEDs turn on or off indicate pitchhing level
if (kalPitch == 0) {
digitalWrite(47, LOW);
digitalWrite(48, LOW);
digitalWrite(49, LOW);
digitalWrite(50, HIGH);
digitalWrite(51, LOW);
digitalWrite(52, LOW);
digitalWrite(53, LOW);
} else if (kalPitch > 0 && kalPitch <= 8) {
digitalWrite(47, LOW);
digitalWrite(48, LOW);
digitalWrite(49, HIGH);
digitalWrite(50, LOW);
digitalWrite(51, LOW);
digitalWrite(52, LOW);
digitalWrite(53, LOW);
} else if (kalPitch < 0 && kalPitch >= -8) {
digitalWrite(47, LOW);
digitalWrite(48, LOW);
digitalWrite(49, LOW);
digitalWrite(50, LOW);
digitalWrite(51, HIGH);
digitalWrite(52, LOW);
digitalWrite(53, LOW);
} else if (kalPitch > 8 && kalPitch <= 16) {
digitalWrite(47, LOW);
digitalWrite(48, HIGH);
digitalWrite(49, LOW);
digitalWrite(50, LOW);
digitalWrite(51, LOW);
digitalWrite(52, LOW);
digitalWrite(53, LOW);
} else if (kalPitch > 16 && kalPitch <= 90) {
digitalWrite(47, HIGH);
digitalWrite(48, LOW);
digitalWrite(49, LOW);
digitalWrite(50, LOW);
digitalWrite(51, LOW);
digitalWrite(52, LOW);
digitalWrite(53, LOW);
} else if (kalPitch < -8 && kalPitch >= -16) {
digitalWrite(47, LOW);
digitalWrite(48, LOW);
digitalWrite(49, LOW);
digitalWrite(50, LOW);
digitalWrite(51, LOW);
digitalWrite(52, HIGH);
digitalWrite(53, LOW);
} else if (kalPitch < -16 && kalPitch >= -90) {
digitalWrite(47, LOW);
digitalWrite(48, LOW);
digitalWrite(49, LOW);
digitalWrite(50, LOW);
digitalWrite(51, LOW);
digitalWrite(52, LOW);
digitalWrite(53, HIGH);
}
mpuPrevTime = currentTime;
}
//nested temperature reading code within if statement
if (currentTime - tempPrevTime >= tempRepeatTime) {
//find temperature reading from gyroscope
mpuTemp = mpu.readTemperature();
Serial.print(" Temp = ");
Serial.print(mpuTemp);
Serial.print(" *C (from MPU)");
//find temp reading and displaying from thermistor
VRT = (5.00 / 1023.00) * analogRead(A0); // Read the voltage across the thermistor
VR = 5.00 - VRT; // Calculate the voltage across the resistor
RT = VRT / (VR / R); // Calculate resistance of the thermistor
ln = log(RT / RT0); // Calculate temperature from thermistor resistance
TX = (1 / ((ln / B) + (1 / T0))); //same description as line above
TX = TX - 273.15; // Convert to Celsius
Serial.print("Temperature: ");
// Display in Celsius
Serial.print(TX);
Serial.print(" *C (from Thermistor)");
// find temperature from sht31
t = sht31.readTemperature();
if (! isnan(t)) { // check if 'is not a number'
Serial.print("Temp = "); Serial.print(t); Serial.print(" *C (from Adafruit Sensiron)");
} else {
Serial.println("Failed to read temperature");
}
//gathering an average value for temperature
avgTemp = ((mpuTemp + t + TX)/ 3 );
Serial.print("Avg Temp = "); Serial.print(avgTemp); Serial.println(" *C");
//Output to Seven-Segment Display
sevseg.setNumberF(avgTemp, 2);
//Buzzer trigger if difference between any sensor for temperature is more than 5ºC
if ((t - TX >= 5) || (t - TX <= -5) || (t - mpuTemp >= 5) || (t - mpuTemp <= -5) || (TX - mpuTemp >= 5) || (TX - mpuTemp <= -5)) {
tone(buzzerPin, 1000, 2000);
} else {
noTone(buzzerPin);
}
tempPrevTime = currentTime;
}
sevseg.refreshDisplay();
}