Shoulder Harness

ShoulderHarness.ino

#include <Arduino_LSM9DS1.h>
#include "MadgwickAHRS.h"
#include "GY521.h"
#include "Wire.h"

Madgwick sensor_back;
GY521 sensor_left_shoulder(0x68);
GY521 sensor_right_shoulder(0x69);

float roll[] = {0, 0, 0};
float pitch[] = {0, 0, 0};
float yaw[] = {0, 0, 0};

const float sensorRate = 104.00;

void initSensor(GY521 *sensor, int sensorID) {

  while (sensor->wakeup() == false)
  {
    Serial.println("Failed to initialize IMU: " + sensorID);
    delay(500);
  }
  sensor->setAccelSensitivity(2);  // 8g
  sensor->setGyroSensitivity(1);   // 500 degrees/s

  sensor->setThrottle();

  sensor->axe = 0.574;
  sensor->aye = -0.002;
  sensor->aze = -1.043;
  sensor->gxe = 10.702;
  sensor->gye = -6.436;
  sensor->gze = -0.676;
}

void printResults() {
  Serial.print(roll[0]);
  Serial.print('\t');
  Serial.print(roll[1]);
  Serial.print('\t');
  Serial.print(roll[2]);
  Serial.print('\t');
  Serial.print(pitch[0]);
  Serial.print('\t');
  Serial.print(pitch[1]);
  Serial.print('\t');
  Serial.print(pitch[2]);
  Serial.print('\t');
  Serial.print(yaw[0]);
  Serial.print('\t');
  Serial.print(yaw[1]);
  Serial.print('\t');
  Serial.print(yaw[2]);
  Serial.println();
}

void setup() {
  Serial.begin(9600);

  Wire.begin();
  delay(100);

  initSensor(&sensor_left_shoulder, 1);
  initSensor(&sensor_right_shoulder, 2);

  sensor_back.begin(sensorRate);
}

void loop() {
  float xAcc, yAcc, zAcc;
  float xGyro, yGyro, zGyro;

  if (IMU.accelerationAvailable() && IMU.gyroscopeAvailable()) {
    IMU.readAcceleration(xAcc, yAcc, zAcc);
    IMU.readGyroscope(xGyro, yGyro, zGyro);
    sensor_back.updateIMU(xGyro, yGyro, zGyro, xAcc, yAcc, zAcc);
  }

  sensor_right_shoulder.read();
  sensor_left_shoulder.read();

  roll[0] = sensor_left_shoulder.getRoll();
  roll[1] = sensor_back.getRoll();
  roll[2] = sensor_right_shoulder.getRoll();

  pitch[0] = sensor_left_shoulder.getPitch();
  pitch[1] = sensor_back.getPitch();
  pitch[2] = sensor_right_shoulder.getPitch();

  yaw[0] = sensor_left_shoulder.getYaw();
  yaw[1] = sensor_back.getYaw();
  yaw[2] = sensor_right_shoulder.getYaw();

  printResults();
}