#include "twi.h"
#include <SPI.h>
#include <SD.h>
#define CS_PIN 10
const byte MPU_ADDRESS = 0x69; // I2C address of gyro.
#define CTRL_REG1 0x20
#define CTRL_REG2 0x21
#define CTRL_REG3 0x22
#define CTRL_REG4 0x23
int x, y, z;
const byte LED_PIN = 4;
const byte BUTTON_PIN = 5;
int16_t Denoise(int16_t value)
{
return value / 114;
}
struct GyroData
{
//const uint32_t reg = 0x28; // For other gyro.
const uint32_t reg = 0x43; // For MPU-6050.
const byte reg_size = 1;
volatile byte buffer[6];
int16_t GyroX() const { return buffer[0] << 8 | buffer[1]; } // Double check the byte order for your gyro.
int16_t GyroY() const { return buffer[2] << 8 | buffer[2]; }
int16_t GyroZ() const { return buffer[4] << 8 | buffer[3]; }
};
GyroData data;
File myFile;
void setup()
{
//PFUpdateStateMachine(); // Should generate a compile error.
pinMode(LED_PIN, OUTPUT);
pinMode(BUTTON_PIN, INPUT_PULLUP);
twi.Enable();
twi.SetFrequency(400000u);
Serial.begin(115200);
Serial.print("Initializing SD card...");
if (!SD.begin(CS_PIN))
{
Serial.println("initialization failed!");
while (1);
}
Serial.println("initialization done.");
/**
Write(MPU_ADDRESS, CTRL_REG1, 0x1F); // Turn on all axes, disable power down
Write(MPU_ADDRESS, CTRL_REG3, 0x08); // Enable control ready signal
Write(MPU_ADDRESS, CTRL_REG4, 0x80); // Set scale (500 deg/sec)
delay(100); // Wait to synchronize
/**/
//
// Perform full reset as per MPU-6000/MPU-6050 Register Map and Descriptions, Section 4.28, pages 40 to 41.
//
// PWR_MGMT_1 register: DEVICE_RESET = 1
Serial.println("Resetting MPU...");
twi.Write(MPU_ADDRESS, 0x6B, 1, 0b10000000, TWI::Modes::Wait);
delay(100); // Wait for reset to complete.
Serial.println("Done.");
// SIGNAL_PATH_RESET register: GYRO_RESET = 1, ACCEL_RESET = 1, TEMP_RESET = 1.
Serial.println("Resetting MPU gyro, accel and temp...");
twi.Write(MPU_ADDRESS, 0x68, 1, 0b00000111, TWI::Modes::Wait);
delay(100); // Wait for reset to complete.
Serial.println("Done.");
// Disable SLEEP mode because the reset re-enables it. Section 3, PWR_MGMT_1 register, page 8.
// PWR_MGMT_1 register: SLEEP = 0
Serial.println("Resetting MPU sleep mode...");
twi.Write(MPU_ADDRESS, 0x6B, 1, 0b00000000, TWI::Modes::Wait);
Serial.println("Done.");
myFile = SD.open("test.txt", FILE_WRITE);
}
void loop()
{
uint16_t timestamp = millis();
if (!digitalRead(BUTTON_PIN))
{
Serial.println("Closing file.");
myFile.close();
while (1);
}
//
// TASK 1: Read the gyro every interval.
//
const uint16_t READ_GYRO_INTERVAL = 10;
static uint16_t read_gyro_timestamp = timestamp;
static bool receiving = false;
if (timestamp - read_gyro_timestamp >= READ_GYRO_INTERVAL)
{
if (!receiving)
{
//Serial.println("\nBurst-read of 6 bytes (using non-blocking ISR).");
receiving = twi.Read(MPU_ADDRESS, data.reg, data.reg_size, data.buffer, sizeof(data.buffer));
}
}
if (receiving && twi.GetState() == TWI::States::Ready)
{
receiving = false;
if (twi.IsSuccess())
{
Serial.print(timestamp);
//Serial.print(timestamp - read_gyro_timestamp); // Delta time.
read_gyro_timestamp += READ_GYRO_INTERVAL;
x = Denoise(data.GyroX());
y = Denoise(data.GyroY());
z = Denoise(data.GyroZ());
Serial.print("\tRaw X:"); Serial.print(x);
Serial.print(" Raw Y:"); Serial.print(y);
Serial.print(" Raw Z:"); Serial.println(z);
myFile.print(timestamp);
myFile.print(" Raw X:"); myFile.print(x);
myFile.print(" Raw Y:"); myFile.print(y);
myFile.print(" Raw Z:"); myFile.println(z);
}
}
//
// TASK 2: Blink the external LED.
//
const uint16_t LED_BLINK_INTERVAL = 500;
static uint16_t led_blink_timestamp = timestamp;
static bool led_state = false;
if (timestamp - led_blink_timestamp >= LED_BLINK_INTERVAL)
{
led_state = !led_state;
digitalWrite(LED_PIN, led_state);
led_blink_timestamp += LED_BLINK_INTERVAL;
}
}