This week I fixed up my IMU board from a few weeks ago and started to get it talking to the main board for my final project. Here's the IMU board in action:

I even got the documentation together enough to post an Instructable about the IMU.

I'm currently on my second revision of my main final project board. I made the first version during outputs week, but hit a snag when I realized my motors run off 6V (not 12V, I'm not sure how that idea got in my head). Here are the milling files:

The large, white areas show where the IMU and GPS will sit on top of the board.

I did a roughing pass with the 1/32" end mill to clear out a lot of material. Then I milled the rest of the traces with a 1/64" end mill.

I've designed this project so that many of the modules can be bought pre-made to make it a little easier to follow along. Here is the parts list:

PARTS LIST:

Motors:

(x1) 100uF electrolytic capacitor Digikey 493-2226-1-ND

(x4) Micro Metal Gearmotor with extended Motor Shaft Pololu 1595

(x2) Magnetic Encoder Pair Kit Pololu 2598

(x4) 2x3 pin header Mouser 649-95278-101A06LF

(x4) 6 pin crimp connector Mouser 649-71600-006LF

(x1) 6 conductor ribbon cable Amazon

(x8) 5 pin female 0.1" header Mouser 517-929870-01-05-RA

Motor Controllers:

(x4) DRV8838 motor driver / H-bridge Digikey 296-37539-1-ND

(x8) 0.1uF capacitor Digikey 1276-1017-1-ND

(x4) 10k resistor Digikey P10KECT-ND

(x8) 5 pin male 0.1" header Mouser 571-6404565

the items above can be replaced by (4x) brushed motor driver Pololu 2990

Power:

(x1) 3.3V voltage regulator Digikey LMS8117AMP-3.3/NOPBTR-ND

(x2) 10uF capacitor Digikey 587-1352-1-ND

(x1) 2.3mm power jack

(x1) 5V power supply

GPS:

(x1) EM-506 GPS Receiver Sparkfun GPS-1275

(x1) 6 pin connector Digikey 455-1792-1-ND

the items above can be replaced by the Sparkfun GPS Shield Sparkfun GPS-10710

AVR (or use an Arduino Uno):

(x1) Atmega328P Digikey ATMEGA328P-AU-ND

(x1) 10K resistor Digikey P10KECT-ND

(x1) tact switch Digikey SW262CT-ND

(x2) 0.1uF capacitor Digikey 1276-1017-1-ND

(x1) 10uF capacitor Digikey 587-1352-1-ND

(x1) 16Mhz crystal oscillator Digikey XC1282CT-ND

(x2) 0.22nF capacitor Digikey 1276-2759-1-ND

(x1) green LED (optional, but will help with debugging) Digikey 160-1404-1-ND

(x1) red LED (optional, but will help with debugging) Digikey 160-1405-1-ND

(x2) 1K resistor (optional, but will help with debugging) Digikey P1.0KECT-ND

Interface (not needed if you are using an Arduino):

(x1) 2x3 pin ISP header Mouser 649-95278-101A06LF

(x1) 6 pin male 0.1" header Mouser 571-3-644456-6

FTDI cable Digikey 768-1015-ND or board Sparkfun DEV-09873 (you may be able to use an Arduino as an FTDI connector) 3.3V or 5V is fine, but if you are making the IMU board too, you will need 3.3V.

ISP programmer (or use an Arduino or make your own)

IMU:

(x1) 9 Degrees of Freedom make your own or buy from Sparkfun SEN-10736

(x2) 6 pin female 0.1" header

Other:

(x35) 0 Ohm resistor (jumpers) Digikey P0.0ECT-ND

Milling:

(x1) 1/64" endmill Carbide Depot CU 129974 (for milling out the traces)

(x1) 1/32" endmill Carbide Depot CU 129985 (for roughing the traces and cutting out the board)

(x1) FR-1 machinable single sided PCB blank Inventables 24201-02

Unfortunately I ran into another snag regarding power for this version of the board. I was planning on running the 328P at 16MHz, but it turns out you cannot run it at that speed at 3.3V. I designed this board to have a 6V supply for the motors and a 3.3V supply for the microcontroller, the logic in the motor controllers, and the IMU. Then I realized that my motors are rated for 6V, but the manufacturer says they will probably be fine running off supplies from 3-9V. So another option could be to run the everything except the IMU off a 5V supply. I think the biggest risk is that the motors might run slightly slower than normal, but that should be ok. For now I'm using an 8Mhz resonator to clock the AVR chip. There are a lot of floating point calculations involved in my final project, so I was hoping to get above 8Mhz, but maybe that will be fine. I don't have a good sense of it yet.

I burned the Arduino bootloader on the board following my steps from inputs week.

Things were going well, but at some point my board got in a weird state and I was unable to upload code to it. I would have re-burned the bootloader to try to solve this, but I was at home and didn't have my fabISP handy. I will try again in the morning. Here is the code I'm working on, it allows the board to communicate with the IMU board and the GPS receiver using the TinyGPS library. I started some of this a while back, but my hardware setup has changed dramatically since then, so most of the lines that are interfacing with the electronics are new. The latest is up on github.

HardwareInputs.h:

 #ifndef HardwareInputs_h
 #define HardwareInputs_h
 #include "Arduino.h"
 #include "HardwareDelegate.h"
 #include 
 #include 
 #include 

 #define IMUBAUD 9600
 #define GPSBAUD  4800

 #define RXPIN 2
 #define TXPIN 3

 class HardwareInputs
  {
    public:

      HardwareInputs();//constructor method
      void init();//setup variables

      void setHardwareDelegate(HardwareDelegate *delegate);
      void checkIMU();
      void checkGPS();

      float yaw();
      float pitch();
      float roll();

    private:

      HardwareDelegate *_delegate;

      //print output
      boolean _printGPS;

      //gps variables
      TinyGPS _gps;
      void parseGPS(TinyGPS &gps);

      //imu
      boolean parseIMU();
      float _yaw;
      float _pitch;
      float _roll;
      float _angularTol;
      boolean checkNewValsAgainstTol(float newAngle, float oldangle);

  };

 #endif

HardwareInputs.cpp:

#include "HardwareInputs.h"

SoftwareSerial uart_gps(RXPIN, TXPIN);

HardwareInputs::HardwareInputs()
{
  _printGPS = false;
  _angularTol = 1.0;//angular noise tolerance
}

void HardwareInputs::init()
{
  uart_gps.begin(GPSBAUD);
  Serial.begin(IMUBAUD);

  // set IMU output parameters
  Serial.write("#ob"); // Turn on binary output
  Serial.write("#o1"); // Turn on continuous streaming output
  Serial.write("#oe0"); // Disable error message output
  Serial.flush();
  Serial.write("#s00"); // Request synch token

  //sync up to imu
  while(Serial.findUntil("#SYNCH00\r\n", "#SYNCH00\r\n")){
  }
  delay(1);
  while(_yaw == 0){
    this->parseIMU();//set initial pitch, yaw, roll vals
  }
}

void HardwareInputs::setHardwareDelegate(HardwareDelegate *delegate)
{
  _delegate = delegate;
}

void HardwareInputs::checkIMU()
{
  boolean shouldUpdateDelegate = this->parseIMU();
  if (shouldUpdateDelegate){
    _delegate->updateOrientation(_yaw, _pitch, _roll);
  }
}

boolean HardwareInputs::parseIMU(){
  // Read angles from serial port
  float oldYaw = _yaw;
  float oldPitch = _pitch;
  float oldRoll = _roll;
  while (Serial.available() >= 12) {
    _yaw = Serial.parseFloat();
    _pitch = Serial.parseFloat();
    _roll = Serial.parseFloat();
  }
  if (checkNewValsAgainstTol(_yaw, oldYaw)) return true;
  if (checkNewValsAgainstTol(_pitch, oldPitch)) return true;
  if (checkNewValsAgainstTol(_roll, oldRoll)) return true;
  return false;
}

float HardwareInputs::yaw(){
  return _yaw;
}

float HardwareInputs::pitch(){
  return _pitch;
}

float HardwareInputs::roll(){
  return _roll;
}

boolean HardwareInputs::checkNewValsAgainstTol(float newAngle, float oldAngle){
  if (abs(oldAngle-newAngle)>_angularTol){
    //double check that this wasn't a small deviation between -179 and +179
    if (360-abs(oldAngle)-abs(newAngle)<_angularTol){
      return false;
    }
    return true;
  }
  return false;
}

void HardwareInputs::checkGPS()
{
  boolean waitingForGPS = false;
  while (waitingForGPS){
    if(uart_gps.available()){
      int c = uart_gps.read();
      if(_gps.encode(c)) {
        waitingForGPS = false;
        this->parseGPS(_gps);
      }
    }
  }
}

void HardwareInputs::parseGPS(TinyGPS &gps)
{
  float latitude, longitude;
  int year;
  byte month, day, hour, minute, second, hundredths;
  // Then call this function
  gps.f_get_position(&latitude, &longitude);
  gps.crack_datetime(&year,&month,&day,&hour,&minute,&second,&hundredths);
  _delegate->updateGPSData(latitude, longitude, year, month, day, hour, minute, second);

  if (_printGPS){
    // You can now print variables latitude and longitude
    Serial.print("Lat/Long: ");
    Serial.print(latitude,5);
    Serial.print(", ");
    Serial.println(longitude,5);

    // Same goes for date and time
    int year;
    byte month, day, hour, minute, second, hundredths;
    gps.crack_datetime(&year,&month,&day,&hour,&minute,&second,&hundredths);
    // Print data and time
    Serial.print("Date: "); Serial.print(month, DEC); Serial.print("/");
    Serial.print(day, DEC); Serial.print("/"); Serial.print(year);
    Serial.print("  Time: "); Serial.print(hour, DEC); Serial.print(":");
    Serial.print(minute, DEC); Serial.print(":"); Serial.print(second, DEC);
    Serial.print("."); Serial.println(hundredths, DEC);
    //Since month, day, hour, minute, second, and hundr

    // Here you can print the altitude and course values directly since
    // there is only one value for the function
    Serial.print("Altitude (meters): "); Serial.println(gps.f_altitude());
    // Same goes for course
    Serial.print("Course (degrees): "); Serial.println(gps.f_course());
    // And same goes for speed
    Serial.print("Speed(kmph): "); Serial.println(gps.f_speed_kmph());
    //Serial.println();

    // Here you can print the number of satellites in view
    Serial.print("Satellites: ");
    Serial.println(gps.satellites());
  }
}

HardwareDelegate.h (a super class of my main Compass class that allows the hardware class to send messages out):

 #ifndef HardwareDelegate_h
 #define HardwareDelegate_h
 #include "Arduino.h"

 class HardwareDelegate
  {
    public:

      HardwareDelegate(){}//constructor method

      void virtual updateGPSData(float latitude, float longitude, int year, byte month, byte day, byte hour, byte minute, byte second){}
      void virtual updateOrientation(float yaw, float pitch, float roll){}

  };

 #endif

MainCompass.h:

 #ifndef CompassMain_h
 #define CompassMain_h
 #include "Arduino.h"
 #include "Motor.h"
 #include "HardwareInputs.h"

 //gear ratios
 #define _motorPulleyTeeth 50//pulley attached to motor
 #define _primaryPulleyTeeth 50//pulley attached to primary drive shaft
 #define _primaryOuterTeeth 97//outer primary gear
 #define _primaryInnerTeeth 87//inner primary gear
 #define _secondaryOuterTeeth 114//outer secondary gear
 #define _secondaryInnerTeeth 102//inner secondary gear
 #define _secondaryMainTeeth 78//secondary gear of inner differential system
 #define _tertiaryTeeth 43//tertiary gear

 class CompassMain: public MotorDelegate, public HardwareDelegate
  {
    public:

      CompassMain();//constructor method
      void init();//setup variables
      void timer1Routine();

      void threeAxisMotion(float threeAxisSpeeds[3], boolean directions[3]);//drive pirmary, secondary, and tertiary axes

//      void motorPositionDidChange(byte currentPosition);
//      void motorDidReachTargetPosition();

      void updateGPSData(float latitude, float longitude, int year, byte month, byte day, byte hour, byte minute, byte second);

      void mainLoop();

    private:

//      volatile boolean _shouldReleaseMotorTension;

      void setupTimer1();

      //current GPS position (make sure to use rad)
      float _longitude;
      float _sinLatitude;
      float _cosLatitude;

      //current time
      int _GMTYear;
      byte _GMTMonth;
      byte _GMTDay;
      byte _GMTHour;
      byte _GMTMinute;
      int _GMTSecond;
      int _timer1Counter;
      float _GWSiderealTime;//increment sidereal time and sync with GMT every so often (a sidereal "second" > regular second and some error will add up)
      int _lastSiderealSync;//time of last sidereal syc (in min)
      byte _lastGPSSync;//time of lst GPS sync (in hrs)
      int _lastCoordinateCalc;//time of last calculation of azimuth and altitude in min, based on current time (only changes about 0.25 degrees per minute)

      //these are the raw azimuth and altitude in horizontal coordinate system, not corrected for current orientation of compass
      float _calculatedAzimuth;
      float _calculatedAltitude;
      //target position, in the local coordinate space of the compass
      float _targetAzimuth;
      float _targetAltitude;
      //these are the Azimuth and altitude that the compass is currently pointing to, in the local coordinate space of the compass
      float _currentAzimuth;
      float _currentAltitude;

      //gear ratios
      float _primaryGearFactor, _secondaryOuterGearFactor, _secondaryInnerGearFactor, _tertiaryOuterGearFactor, _tertiaryInnerGearFactor;

      //horizontal coordinate calculations - these will not change very quicky, 0.25 degrees per minute, don't calculate these too often
      float getHourAngle(float GWSiderealTime, float longitude, float rightAscension);
      void syncGWSiderealTime(int year, byte month, byte day, byte hour, byte minute, byte second);
      int leapYearCorrection(int year, byte month, byte day);
      unsigned long getUT1DaysSinceJan2000(int year, byte month, byte day, byte hour, byte minute, byte second);//returns number *100,000
      void updateAzimuthAltitudeAndPlaneAngle();

      //local coordinate corrections (based on orintation of compass in 3D space) - apply these as often as possible for responsiveness
      float correctedAzimuth(float currentAzimuth, float heading);
      float correctedAltitude(float currentAzimuth, float currentAltitude, float pitch, float roll);

      //directions for turning to new target position
      void moveToNewPosition(float currentAzimuth, float currentAltitude, float targetAzimuth, float targetAltitude);

      //turn each rotational axis
      int * calculatePrimaryTurnMatrix(float turnSpeed, boolean turnDirection, int storage[4]);
      int * calculateSecondaryTurnMatrix(float turnSpeed, boolean turnDirection, int storage[4]);
      int * calculateTertiaryTurnMatrix(float turnSpeed, boolean turnDirection, int storage[4]);

      //motors
      Motor *_motors[4];
      void setMotorSpeeds(int motorSpeeds[4], boolean motorDirections[4]);

      //hardware
      HardwareInputs *_sensors;

      //helper functions
      int * getDirectionalSpeeds(byte motorSpeeds[4], boolean motorDirections[4], int storage[4]);
      int * getAbsoluteSpeeds(int directionalSpeeds[4], int storage[4]);
      boolean * getDirections(int directionalSpeeds[4], boolean storage[4]);
      float convertToRad(float angleDegrees);
  };

 #endif

MainCompass.cpp

#include "CompassMain.h"

#define clockRate 8//clock rate in Mhz

byte daysPerMonth[] = {31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31};

CompassMain::CompassMain()
{
  //calculate gear ratios
  _primaryGearFactor = float(_primaryPulleyTeeth)/float(_motorPulleyTeeth);
  _secondaryOuterGearFactor = float(_primaryGearFactor*_secondaryOuterTeeth)/float(_primaryOuterTeeth);
  _secondaryInnerGearFactor = float(_primaryGearFactor*_secondaryInnerTeeth)/float(_primaryInnerTeeth);
  _tertiaryOuterGearFactor = float(_secondaryOuterGearFactor*_tertiaryTeeth)/float(_secondaryMainTeeth);
  _tertiaryInnerGearFactor = float(_secondaryInnerGearFactor*_tertiaryTeeth)/float(_secondaryMainTeeth);
}

void CompassMain::init()
{
  //when device is turned on, needle should be pointing straight up with disk axis aligned with North (todo: or get from perm storage)

//  *(_motors) =  new Motor(12, 13, true);
//  *(_motors + 1) = new Motor(10, 11, false);
//  *(_motors + 2) = new Motor(8, 9, true);
//  *(_motors + 3) = new Motor(6, 7, false);

//  for (byte i=0;i<4;i++){
//    (*(_motors + i))->setMotorDelegate(this);
//  }

  _sensors = new HardwareInputs();
  _sensors->init();
  _sensors->setHardwareDelegate(this);
  _sensors->checkGPS();
  _sensors->checkGPS();//have to do this twice to ensure gps data has been set correctly initially

//  _shouldReleaseMotorTension = false;
  this->setupTimer1();

}


//---------------------------------------------------------------------
//--------------------TIMERS-------------------------------------------
//---------------------------------------------------------------------


void CompassMain::setupTimer1()
{
  //counting variables
  _timer1Counter = 0;

  cli();//stop interrupts

//  //set timer1 interrupt at 10kHz
  TCCR1A = 0;// set entire TCCR1A register to 0
  TCCR1B = 0;// same for TCCR1B
  TCNT1 = 0;//initialize counter value to 0;
  // set timer count for 10khz increments
  OCR1A = clockRate*25/2-1;// = (16*10^6) / (10000*8) - 1
  // turn on CTC mode
  TCCR1B |= (1 << WGM12);
  // Set CS11 bit for 8 prescaler
  TCCR1B |= (1 << CS11);
  // enable timer compare interrupt
  TIMSK1 |= (1 << OCIE1A);


  sei();//allow interrupts
}

void CompassMain::timer1Routine()
{
  _timer1Counter++;
  if (_timer1Counter >= 10000){
    _GMTSecond += 1;
    _GWSiderealTime += 1.0002785383;//sidereal second is slghtly longer
    _timer1Counter = 0;
  }

}

void CompassMain::mainLoop()
{
  if (_GMTHour != _lastGPSSync){//sync up with GPS each hr
    _sensors->checkGPS();
  }
  if (_GMTSecond >= _lastSiderealSync+60){//if at least a min has gone by
    this->syncGWSiderealTime(_GMTYear, _GMTMonth, _GMTDay, _GMTHour, _GMTMinute, _GMTSecond);//sync up sidereal time to correct for any error
  }
  if (_GMTSecond >= _lastCoordinateCalc+60){//if at least a min has gone by
    this->updateAzimuthAltitudeAndPlaneAngle();
  }
  _sensors->checkIMU();//continuously monitor imu

  //calculate new target position
  //drive mototrs
}

//---------------------------------------------------------------------
//-----------------GALACTIC TRANSFORMATIONS/CALCULATIONS---------------
//---------------------------------------------------------------------

float CompassMain::getHourAngle(float GWSiderealTime, float longitude, float rightAscension)
{
  return this->convertToRad(GWSiderealTime - longitude - rightAscension);
}

void CompassMain::syncGWSiderealTime(int year, byte month, byte day, byte hour, byte minute, byte second)
{
  unsigned long UT1DaysSince2000 = this->getUT1DaysSinceJan2000(year, month, day, hour, minute, second);
  int integerDays = UT1DaysSince2000/100000;
  unsigned long GWSiderealTime = 1869737.4558 + 0.06570982441908*UT1DaysSince2000 + (UT1DaysSince2000-(integerDays*100000))*24;//modified version of θG = 18.697374558 + 24.06570982441908 * D to deal with size limitations of unsigned long
  unsigned long amountToSubtract  = (GWSiderealTime/2400000)*2400000;//remove integer number of days
  _GWSiderealTime = (GWSiderealTime-amountToSubtract)/100000.0;
  _lastSiderealSync = _GMTSecond;
}

int CompassMain::leapYearCorrection(int year, byte month, byte day)
{
  int numLeapYearsCompleted = (year-2000)/4;//2000 was a leap year
  if ((year-2000)%4 != 0) return numLeapYearsCompleted;//if it's not a leap year, we're done
  if (month>2){//otherwise, see if we've passed feb 29 completely (if it is currently feb29, this time will get added as a fraction from the time)
    numLeapYearsCompleted ++;
  }
  return numLeapYearsCompleted;
}

unsigned long CompassMain::getUT1DaysSinceJan2000(int year, byte month, byte day, byte hour, byte minute, byte second)
{
  //as of now it's been about 5.3K days - I'm using an unsigned long here, = UT1Days*100,000 to increase precision and speed (Arduino has no double, only float)
  //this gives me room for up to 42K days, while still maintaining ~second precison (5 decimal places)
  unsigned long  numDaysSinceJan12000 = (year-2000)*36500000;
  numDaysSinceJan12000 -= 50000;//since we are starting at 12PM on Jan1, but all my times are based on 12AM = 0, subtract half a day (-0.5*100,000)
  numDaysSinceJan12000 += this->leapYearCorrection(year, month, day)*100000;
  for (byte i=0;igetHourAngle(currentSiderealTime, _longitude, 100);
  float sinHrAngle = sin(hourAngle);
  float cosHrAngle = cos(hourAngle);
  //make sure these constants are in rad
//  _calculatedAzimuth = atan2(sinHrAngle, (cosHrAngle*_sinLatitude-tan(δ)*_cosLatitude));
//  _calculatedAltitude = asin(_sinLatitude*sin(δ)+_cosLatitude*cos(δ)*cosHrAngle);

  _targetAzimuth = this->correctedAzimuth(_calculatedAzimuth, _sensors->yaw());
  _targetAltitude = this->correctedAltitude(_targetAzimuth, _calculatedAltitude, _sensors->pitch(), _sensors->roll());

  float poleHourAngle = this->getHourAngle(currentSiderealTime, _longitude, 100);
  float sinPoleHrAngle = sin(poleHourAngle);
  float cosPoleHrAngle = cos(poleHourAngle);
//  float currentPoleAzimuth = atan2(sinPoleHrAngle, (cosPoleHrAngle*sinLatitude-tan(δ)*cosLatitude));
//  float currentPoleAltitude = asin(sinLatitude*sin(δ)+cosLatitude*cos(δ)*cosPoleHrAngle);
  _lastCoordinateCalc = _GMTSecond;
}

//---------------------------------------------------------------------
//-----------------LOCAL TRANSFORMATIONS/CALCULATIONS------------------
//---------------------------------------------------------------------

float CompassMain::correctedAzimuth(float currentAzimuth, float heading)
{
  return currentAzimuth+heading;
}

float CompassMain::correctedAltitude(float currentAzimuth, float currentAltitude, float pitch, float roll)
{
  return currentAltitude;
}

//---------------------------------------------------------------------
//-----------------PITCH/ROLL/HEADING TO 3 ROTATIONAL AXES-------------
//---------------------------------------------------------------------


void CompassMain::moveToNewPosition(float currentAzimuth, float currentAltitude, float targetAzimuth, float targetAltitude)
{
  float diffAzimuth = targetAzimuth-currentAzimuth;
  float diffAltitude = targetAltitude-currentAltitude;
//  if (abs(diffAzimuth)>0.2 || abs(diffAltitude)>0.2){
//    this->moveToNewPosition(currentAzimuth, currentAltitude, targetAzimuth/2, targetAltitude/2;
//    return;
//  }
  //linearly move to target Azimuth and altitude

}


//---------------------------------------------------------------------
//-----------------3 AXES TRANSFORMATIONS/CALCULATIONS-----------------
//---------------------------------------------------------------------

int * CompassMain::calculatePrimaryTurnMatrix(float turnSpeed, boolean turnDirection, int storage[4])
{
//  byte adjustedSpeed = _primaryGearFactor*turnSpeed;
  byte adjustedSpeed = turnSpeed;
  byte motorSpeeds[] = {adjustedSpeed, adjustedSpeed, adjustedSpeed, adjustedSpeed};
  boolean motorDirections[] = {turnDirection, turnDirection, turnDirection, turnDirection};
  return this->getDirectionalSpeeds(motorSpeeds, motorDirections, storage);
}

int * CompassMain::calculateSecondaryTurnMatrix(float turnSpeed, boolean turnDirection, int storage[4])
{
//  byte adjustedOuterSpeed = _secondaryOuterGearFactor*turnSpeed;
//  byte adjustedInnerSpeed = _secondaryInnerGearFactor*turnSpeed;
  byte adjustedOuterSpeed = turnSpeed;
  byte adjustedInnerSpeed = turnSpeed;
  byte motorSpeeds[] = {adjustedOuterSpeed, adjustedInnerSpeed, adjustedInnerSpeed, adjustedOuterSpeed};
  boolean motorDirections[] = {turnDirection, !turnDirection, turnDirection, !turnDirection};
  return this->getDirectionalSpeeds(motorSpeeds, motorDirections, storage);
}

int * CompassMain::calculateTertiaryTurnMatrix(float turnSpeed, boolean turnDirection, int storage[4])
{
//  byte adjustedOuterSpeed = _tertiaryOuterGearFactor*turnSpeed;
//  byte adjustedInnerSpeed = _tertiaryInnerGearFactor*turnSpeed;
  byte adjustedOuterSpeed = turnSpeed;
  byte adjustedInnerSpeed = turnSpeed;
  byte motorSpeeds[] = {adjustedOuterSpeed, adjustedInnerSpeed, adjustedInnerSpeed, adjustedOuterSpeed};
  boolean motorDirections[] = {turnDirection, turnDirection, !turnDirection, !turnDirection};
  return this->getDirectionalSpeeds(motorSpeeds, motorDirections, storage);
}

void CompassMain::threeAxisMotion(float threeAxisSpeeds[3], boolean directions[3])
{
  //storage prevents arrays from being deallocated in this scope
  int storage1[4];
  int storage2[4];
  int storage3[4];

  int *primaryMat = this->calculatePrimaryTurnMatrix(threeAxisSpeeds[0], directions[0], storage1);
  int *secondaryMat = this->calculateSecondaryTurnMatrix(threeAxisSpeeds[1], directions[1], storage2);
  int *tertiaryMat = this->calculateTertiaryTurnMatrix(threeAxisSpeeds[2], directions[2], storage3);

  int combinedMatrix[4];
  for (byte i=0;i<4;i++){
    combinedMatrix[i] = *(primaryMat+i) + *(secondaryMat+i) + *(tertiaryMat+i);
  }

  //only constraint on motor movement is on primary rotation - avg position of outer gears must == avg position of inner gears
  //although I never specifically set the motor speeds to break this, little timing things during speed changes will add up over time and create a lot of tension in the mechanism - enough to unmesh gear teeth
//  _shouldReleaseMotorTension = true;
//  while (_shouldReleaseMotorTension){//don't send a new speed until this is resolved - handled in interrupt - should be over quickly
//  }

  //normalize speeds to 255
  int absSpeeds[4];
//  &(absSpeeds[4]) = this->getAbsoluteSpeeds(combinedMatrix, absSpeeds);

  boolean motorDirections[4];
  this->setMotorSpeeds(this->getAbsoluteSpeeds(combinedMatrix, absSpeeds), this->getDirections(combinedMatrix, motorDirections));

}

//---------------------------------------------------------------------
//--------------------MOTOR CONTROL------------------------------------
//---------------------------------------------------------------------

void CompassMain::setMotorSpeeds(int motorSpeeds[4], boolean motorDirections[4])
{
  for (byte i=0;i<4;i++){
    (*(_motors+i))->setMotorSpeed(motorSpeeds[i], motorDirections[i]);
  }
}

//---------------------------------------------------------------------
//--------------------MOTOR DELEGATE-----------------------------------
//---------------------------------------------------------------------

//void CompassMain::motorPositionDidChange(byte currentPosition)
//{
//}
//
//void CompassMain::motorDidReachTargetPosition()
//{
//}

//---------------------------------------------------------------------
//--------------------HARDWARE DELEGATE--------------------------------
//---------------------------------------------------------------------

void CompassMain::updateGPSData(float latitude, float longitude, int year, byte month, byte day, byte hour, byte minute, byte second)
{
  latitude = this->convertToRad(latitude);
  _sinLatitude = sin(latitude);
  _cosLatitude = cos(latitude);
  _longitude = this->convertToRad(longitude);
  _GMTYear = year;
  _GMTMonth = month;
  _GMTDay = day;
  _GMTHour = hour;
  _GMTMinute = minute;
  _GMTSecond = second;
  _lastGPSSync = hour;
  this->syncGWSiderealTime(year, month, day, hour, minute, second);//force sync sidereal time
  this->updateAzimuthAltitudeAndPlaneAngle();//calculate new azimuth and altitude
}

//---------------------------------------------------------------------
//--------------------Utilities----------------------------------------
//---------------------------------------------------------------------

int * CompassMain::getDirectionalSpeeds(byte motorSpeeds[4], boolean motorDirections[4], int storage[4])
{
  for (byte i=0;i<4;i++){
    storage[i] = motorSpeeds[i];
    if (!motorDirections[i]) storage[i] *= -1;
  }
  return storage;
}

int * CompassMain::getAbsoluteSpeeds(int directionalSpeeds[4], int storage[4])
{
  for (byte i=0;i<4;i++){
    int directionalSpeed = directionalSpeeds[i];
    storage[i] = abs(directionalSpeed);
  }
  return storage;
}

boolean * CompassMain::getDirections(int directionalSpeeds[4], boolean storage[4])
{
  for (byte i=0;i<4;i++){
    storage[i] = true;
    if (directionalSpeeds[i]<0) storage[i] = false;
  }
  return storage;
}

float CompassMain::convertToRad(float angleDegrees)
{
  return angleDegrees*PI/180;
}