first commit
This commit is contained in:
Sophie Schiller
commit
be8e71507b
8 changed files with 329 additions and 0 deletions
4
.gitignore
vendored
Normal file
4
.gitignore
vendored
Normal file
|
|
@ -0,0 +1,4 @@
|
|||
.pio
|
||||
.clang_complete
|
||||
.gcc-flags.json
|
||||
.ccls
|
||||
21
Makefile
Normal file
21
Makefile
Normal file
|
|
@ -0,0 +1,21 @@
|
|||
# Uncomment lines below if you have problems with $PATH
|
||||
#SHELL := /bin/bash
|
||||
#PATH := /usr/local/bin:$(PATH)
|
||||
|
||||
all:
|
||||
pio -f -c vim run
|
||||
|
||||
upload:
|
||||
pio -f -c vim run --target upload
|
||||
|
||||
clean:
|
||||
pio -f -c vim run --target clean
|
||||
|
||||
program:
|
||||
pio -f -c vim run --target program
|
||||
|
||||
uploadfs:
|
||||
pio -f -c vim run --target uploadfs
|
||||
|
||||
update:
|
||||
pio -f -c vim update
|
||||
0
README.md
Normal file
0
README.md
Normal file
39
include/README
Normal file
39
include/README
Normal file
|
|
@ -0,0 +1,39 @@
|
|||
|
||||
This directory is intended for project header files.
|
||||
|
||||
A header file is a file containing C declarations and macro definitions
|
||||
to be shared between several project source files. You request the use of a
|
||||
header file in your project source file (C, C++, etc) located in `src` folder
|
||||
by including it, with the C preprocessing directive `#include'.
|
||||
|
||||
```src/main.c
|
||||
|
||||
#include "header.h"
|
||||
|
||||
int main (void)
|
||||
{
|
||||
...
|
||||
}
|
||||
```
|
||||
|
||||
Including a header file produces the same results as copying the header file
|
||||
into each source file that needs it. Such copying would be time-consuming
|
||||
and error-prone. With a header file, the related declarations appear
|
||||
in only one place. If they need to be changed, they can be changed in one
|
||||
place, and programs that include the header file will automatically use the
|
||||
new version when next recompiled. The header file eliminates the labor of
|
||||
finding and changing all the copies as well as the risk that a failure to
|
||||
find one copy will result in inconsistencies within a program.
|
||||
|
||||
In C, the usual convention is to give header files names that end with `.h'.
|
||||
It is most portable to use only letters, digits, dashes, and underscores in
|
||||
header file names, and at most one dot.
|
||||
|
||||
Read more about using header files in official GCC documentation:
|
||||
|
||||
* Include Syntax
|
||||
* Include Operation
|
||||
* Once-Only Headers
|
||||
* Computed Includes
|
||||
|
||||
https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html
|
||||
46
lib/README
Normal file
46
lib/README
Normal file
|
|
@ -0,0 +1,46 @@
|
|||
|
||||
This directory is intended for project specific (private) libraries.
|
||||
PlatformIO will compile them to static libraries and link into executable file.
|
||||
|
||||
The source code of each library should be placed in a an own separate directory
|
||||
("lib/your_library_name/[here are source files]").
|
||||
|
||||
For example, see a structure of the following two libraries `Foo` and `Bar`:
|
||||
|
||||
|--lib
|
||||
| |
|
||||
| |--Bar
|
||||
| | |--docs
|
||||
| | |--examples
|
||||
| | |--src
|
||||
| | |- Bar.c
|
||||
| | |- Bar.h
|
||||
| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html
|
||||
| |
|
||||
| |--Foo
|
||||
| | |- Foo.c
|
||||
| | |- Foo.h
|
||||
| |
|
||||
| |- README --> THIS FILE
|
||||
|
|
||||
|- platformio.ini
|
||||
|--src
|
||||
|- main.c
|
||||
|
||||
and a contents of `src/main.c`:
|
||||
```
|
||||
#include <Foo.h>
|
||||
#include <Bar.h>
|
||||
|
||||
int main (void)
|
||||
{
|
||||
...
|
||||
}
|
||||
|
||||
```
|
||||
|
||||
PlatformIO Library Dependency Finder will find automatically dependent
|
||||
libraries scanning project source files.
|
||||
|
||||
More information about PlatformIO Library Dependency Finder
|
||||
- https://docs.platformio.org/page/librarymanager/ldf.html
|
||||
15
platformio.ini
Normal file
15
platformio.ini
Normal file
|
|
@ -0,0 +1,15 @@
|
|||
; PlatformIO Project Configuration File
|
||||
;
|
||||
; Build options: build flags, source filter
|
||||
; Upload options: custom upload port, speed and extra flags
|
||||
; Library options: dependencies, extra library storages
|
||||
; Advanced options: extra scripting
|
||||
;
|
||||
; Please visit documentation for the other options and examples
|
||||
; https://docs.platformio.org/page/projectconf.html
|
||||
|
||||
[env:uno]
|
||||
platform = atmelavr
|
||||
board = uno
|
||||
framework = arduino
|
||||
lib_deps = fastled/FastLED@^3.4.0
|
||||
193
src/main.ino
Normal file
193
src/main.ino
Normal file
|
|
@ -0,0 +1,193 @@
|
|||
/*
|
||||
Arduino and MPU6050 Accelerometer and Gyroscope Sensor Tutorial
|
||||
by Dejan, https://howtomechatronics.com
|
||||
*/
|
||||
|
||||
#include <Wire.h>
|
||||
#include <FastLED.h>
|
||||
|
||||
#define LED_PIN 7
|
||||
#define NUM_LEDS 20
|
||||
|
||||
const int MPU = 0x68; // MPU6050 I2C address
|
||||
float AccX, AccY, AccZ;
|
||||
float GyroX, GyroY, GyroZ;
|
||||
float accAngleX, accAngleY, accCombined, gyroAngleX, gyroAngleY, gyroAngleZ;
|
||||
float roll, pitch, yaw;
|
||||
float AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ;
|
||||
float elapsedTime, currentTime, previousTime;
|
||||
int c = 0;
|
||||
boolean debug = true;
|
||||
|
||||
CRGB leds[NUM_LEDS];
|
||||
|
||||
void setup() {
|
||||
Serial.begin(19200);
|
||||
//setup LEDs
|
||||
FastLED.addLeds<WS2812, LED_PIN, GRB>(leds, NUM_LEDS);
|
||||
|
||||
//setup IMU
|
||||
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
|
||||
|
||||
// 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);
|
||||
*/
|
||||
delay(20);
|
||||
|
||||
// Call this function if you need to get the IMU error values for your module
|
||||
//calculate_IMU_error();
|
||||
delay(200);
|
||||
|
||||
}
|
||||
|
||||
void calculateOrientationData() {
|
||||
// === 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.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
|
||||
if (debug) {
|
||||
Serial.print("Acceleration: ");
|
||||
Serial.print(accCombined, 3);
|
||||
Serial.print("\t");
|
||||
Serial.print("Orientation: ");
|
||||
Serial.print("\t");
|
||||
Serial.print(roll);
|
||||
Serial.print("\t");
|
||||
Serial.print(pitch);
|
||||
Serial.print("\t");
|
||||
Serial.println(yaw);
|
||||
}
|
||||
}
|
||||
|
||||
void enableLEDsOnAcceleration(){
|
||||
int ledcutoff = int(accCombined * NUM_LEDS / 2);
|
||||
if(debug) {
|
||||
Serial.print("[");
|
||||
}
|
||||
for (int i = 0; i <= ledcutoff; i++) {
|
||||
leds[i] = CRGB(64, 0, 0);
|
||||
if (debug) {
|
||||
Serial.print("|");
|
||||
}
|
||||
}
|
||||
for (int i = ledcutoff; i <= NUM_LEDS ; i++) {
|
||||
leds[i] = CRGB(0, 64, 0);
|
||||
if (debug) {
|
||||
Serial.print("-");
|
||||
}
|
||||
}
|
||||
if(debug) {
|
||||
Serial.println("]");
|
||||
}
|
||||
FastLED.show();
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// === Read acceleromter data === //
|
||||
calculateOrientationData();
|
||||
enableLEDsOnAcceleration();
|
||||
delay(20);
|
||||
}
|
||||
|
||||
|
||||
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);
|
||||
}
|
||||
11
test/README
Normal file
11
test/README
Normal file
|
|
@ -0,0 +1,11 @@
|
|||
|
||||
This directory is intended for PlatformIO Unit Testing and project tests.
|
||||
|
||||
Unit Testing is a software testing method by which individual units of
|
||||
source code, sets of one or more MCU program modules together with associated
|
||||
control data, usage procedures, and operating procedures, are tested to
|
||||
determine whether they are fit for use. Unit testing finds problems early
|
||||
in the development cycle.
|
||||
|
||||
More information about PlatformIO Unit Testing:
|
||||
- https://docs.platformio.org/page/plus/unit-testing.html
|
||||
Loading…
Reference in a new issue