// Full orientation sensing using NXP/Madgwick/Mahony and a range of 9-DoF
// sensor sets.
// You *must* perform a magnetic calibration before this code will work.
//
// To view this data, use the Arduino Serial Monitor to watch the
// scrolling angles, or run the OrientationVisualiser example in Processing.
// Based on https://github.com/ /NXPMotionSense with adjustments
// to Adafruit Sensor interface and SensorLab
#include <Adafruit_SensorLab.h>
#include <Adafruit_Sensor_Calibration.h>
#include <Adafruit_AHRS.h>
#include "Adafruit_TinyUSB.h"
#include <Adafruit_CircuitPlayground.h>
#include <Adafruit_Circuit_Playground.h>
uint8_t const desc_hid_report[] = {
TUD_HID_REPORT_DESC_KEYBOARD()
};
Adafruit_USBD_HID usb_hid(desc_hid_report, sizeof(desc_hid_report), HID_ITF_PROTOCOL_KEYBOARD, 2, false);
// pick your filter! slower == better quality output
Adafruit_NXPSensorFusion filter; // slowest
//Adafruit_Madgwick filter; // faster than NXP
//Adafruit_Mahony filter; // fastest
#define FILTER_UPDATE_RATE_HZ 100
uint32_t timestamp;
Adafruit_SensorLab lab;
Adafruit_Sensor *accelerometer, *gyroscope, *magnetometer;
#if defined(ADAFRUIT_SENSOR_CALIBRATION_USE_EEPROM)
Adafruit_Sensor_Calibration_EEPROM cal;
#else
Adafruit_Sensor_Calibration_SDFat cal;
#endif
const int debounce = 100;
const int thresh = 500;
void setup() {
//keyboard
CircuitPlayground.begin();
usb_hid.begin();
Serial.begin(115200);
while (!Serial) yield();
Serial.println(F("Sensor Lab - IMU AHRS!"));
lab.begin();
if (!cal.begin()) {
Serial.println("Failed to initialize calibration helper");
} else if (!cal.loadCalibration()) {
Serial.println("No calibration loaded/found");
}
Serial.println("Looking for a magnetometer");
magnetometer = lab.getMagnetometer();
if (!magnetometer) {
Serial.println(F("Could not find a magnetometer!"));
while (1) yield();
}
Serial.println("Looking for a gyroscope");
gyroscope = lab.getGyroscope();
if (!gyroscope) {
Serial.println(F("Could not find a gyroscope!"));
while (1) yield();
}
Serial.println("Looking for a accelerometer");
accelerometer = lab.getAccelerometer();
if (!accelerometer) {
Serial.println(F("Could not find a accelerometer!"));
while (1) yield();
}
accelerometer->printSensorDetails();
gyroscope->printSensorDetails();
magnetometer->printSensorDetails();
filter.begin(FILTER_UPDATE_RATE_HZ);
timestamp = millis();
Wire.setClock(400000); // 400KHz
}
void loop() {
uint8_t const report_id = 0;
uint8_t const modifier = 0;
uint8_t keycode[6] = { 0 };
float roll, pitch, heading;
float gx, gy, gz;
if ((millis() - timestamp) < (1000 / FILTER_UPDATE_RATE_HZ)) {
return;
}
timestamp = millis();
// Read the motion sensors
sensors_event_t accel, gyro, mag;
accelerometer->getEvent(&accel);
gyroscope->getEvent(&gyro);
magnetometer->getEvent(&mag);
//Serial.print("I2C took "); Serial.print(millis()-timestamp); Serial.println(" ms");
cal.calibrate(mag);
cal.calibrate(accel);
cal.calibrate(gyro);
// Gyroscope needs to be converted from Rad/s to Degree/s
// the rest are not unit-important
gx = gyro.gyro.x * SENSORS_RADS_TO_DPS;
gy = gyro.gyro.y * SENSORS_RADS_TO_DPS;
gz = gyro.gyro.z * SENSORS_RADS_TO_DPS;
// Update the SensorFusion filter
filter.update(gx, gy, gz,
accel.acceleration.x, accel.acceleration.y, accel.acceleration.z,
mag.magnetic.x, mag.magnetic.y, mag.magnetic.z);
//Serial.print("Update took "); Serial.print(millis()-timestamp); Serial.println(" ms");
/*Serial.print("Raw: ");
Serial.print(accel.acceleration.x, 4);
Serial.print(", ");
Serial.print(accel.acceleration.y, 4);
Serial.print(", ");
Serial.print(accel.acceleration.z, 4);
Serial.print(", ");
Serial.print(gx, 4);
Serial.print(", ");
Serial.print(gy, 4);
Serial.print(", ");
Serial.print(gz, 4);
Serial.print(", ");
Serial.print(mag.magnetic.x, 4);
Serial.print(", ");
Serial.print(mag.magnetic.y, 4);
Serial.print(", ");
Serial.print(mag.magnetic.z, 4);
Serial.println("");*/
// print the heading, pitch and roll
roll = filter.getRoll();
pitch = filter.getPitch();
heading = filter.getYaw();
/* Serial.print("Orientation: ");
Serial.print(heading);
Serial.print(" ");
Serial.print(pitch);
Serial.print(" ");
Serial.println(roll);
Serial.print("Took ");
Serial.print(millis() - timestamp);
Serial.println(" ms");*/
if (roll > 0 && digitalRead(CPLAY_SLIDESWITCHPIN)) //move left
{
keycode[0] = HID_KEY_A;
usb_hid.keyboardReport(report_id, modifier, keycode);
delay(debounce);
usb_hid.keyboardRelease(0);
Serial.print("Roll: (should be greater than 0): ");
Serial.print(roll);
Serial.println("");
}
delay(debounce);
if (roll < 0 && digitalRead(CPLAY_SLIDESWITCHPIN)) //move right
{
keycode[0] = HID_KEY_D;
usb_hid.keyboardReport(report_id, modifier, keycode);
delay(debounce);
usb_hid.keyboardRelease(0);
Serial.print("Roll: (should be less than 0): ");
Serial.print(roll);
Serial.println("");
}
delay(debounce);
if (pitch < 0 && digitalRead(CPLAY_SLIDESWITCHPIN)) //raise stance up
{
keycode[0] = HID_KEY_W;
usb_hid.keyboardReport(report_id, modifier, keycode);
delay(debounce);
usb_hid.keyboardRelease(0);
Serial.print("Pitch: Should be < 0): ");
Serial.print(pitch);
Serial.println("");
}
delay(debounce);
if (pitch > 0 && pitch < 30 && digitalRead(CPLAY_SLIDESWITCHPIN)) //lower stance down
{
keycode[0] = HID_KEY_S;
usb_hid.keyboardReport(report_id, modifier, keycode);
delay(debounce);
usb_hid.keyboardRelease(0);
Serial.print("Pitch: Should be between 0 and 30): ");
Serial.print(pitch);
Serial.println("");
}
delay(debounce);
if (accel.acceleration.z > 1 && digitalRead(CPLAY_SLIDESWITCHPIN)) // thrust
{
keycode[0] = HID_KEY_F;
usb_hid.keyboardReport(report_id, modifier, keycode);
delay(10);
usb_hid.keyboardRelease(0);
Serial.print("Accel: Should be greater than 10 ): ");
Serial.print(accel.acceleration.z, 4);
Serial.println("");
}
delay(debounce);
}
No comments:
Post a Comment
Note: Only a member of this blog may post a comment.