Wireless Golf Swing Tracker

Merging my interest in technology and golf (and sports in general), in early 2012 I prototyped a small device that could be used to wirelessly track movement of the golf swing. Shortly after I made it, Swingbyte, which does the same thing and was of the same form factor, was announced and demonstrated at the PGA Show.

For the prototype, the goal was to build a small device that could be clipped to the golf club just below the handle. The device would transmit telemetry data wirelessly via bluetooth back to the PC (or phone, at some point), where it would be processed. Once processed, the PC application would render the path of the club through 3D space.

I didn’t make it to the 3D path rendering with the prototype (stopped working on it when life got in the way), but I was at the point of being able to capture individual axis graphs and calculate velocities from the data. Position was next, but I realized I was not as well versed in mathematics and needed to do more research (before life got in the way).

Still, it was a great learning experience, and I still have the device sitting in a box.

I ordered most of my parts off of Sparkfun, mainly because their breakout boards made prototyping easy.

Parts

  • Arduino Pro Mini 328
  • ITG3200/ADXL345 6DOF combo board
  • BlueSMiRF HID Bluetooth modem
  • 5V FTDI (mini-USB)
  • 110mAh Lithium Ion Battery
  • LiPo Charger mini-USB
  • SPDT Mini Power Switch

Code

This is the code for the Arduino sketch. I didn’t bother cleaning it up, so this is effectively a snapshot from when the project stopped.

#include <Wire.h>
#include "ADXL345.h"
#include "ITG3200.h"

ADXL345 accel;
ITG3200 gyro;

const int Sensitivity = 16;	// sensitivity value for accelerometer in g's
const int CalibrationCount = 100;
const float AccelerationMin = 0.05;	// minimum acceleration before we do anything
const int NoMovementThreshold = 3;	// max number of iterations without accelerator movement
const bool FullResolution = true;

#define IS_ZERO(x, threshold) ((x < threshold) && (x > -threshold))

bool bDeviceConnected = false;
unsigned long lastMicrosec = 0;

unsigned long prevTime = 0;
float deltaTime = 0;

char str[512];
	 int raw_values[9];
	 float rawf_values[6];
	 float quat[4];

float accelBias[3] = {0,0,0};
float yprBias[3] = {0,0,0};

#ifndef cbi
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#endif

#define twoKpDef  (2.0f * 0.5f) // 2 * proportional gain
#define twoKiDef  (2.0f * 0.1f) // 2 * integral gain

float exInt = 0.0f, eyInt = 0.0f, ezInt = 0.0;  // scaled integral error
volatile float twoKp = twoKpDef;      // 2 * proportional gain (Kp)
volatile float twoKi = twoKiDef;      // 2 * integral gain (Ki)
volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to auxiliary frame
volatile float integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f;
unsigned long lastUpdate = 0, now = 0; // sample period expressed in milliseconds
float sampleFreq = 1024.0f;//512.0f; // half the sample period expressed in seconds
int startLoopTime = 0;

float G_Dt=0.005;    // Integration time (DCM algorithm)  We will run the integration loop at 50Hz if possible

long timer=0;   //general purpuse timer
long timer_old;

float g_Quat[4] = {0,0,0,0};

extern unsigned long timer0_millis;

// this function replaces the arduino millis() funcion
unsigned long DIYmillis()
{
   unsigned long m;
   unsigned long m2;

   // timer0_millis could change inside timer0 interrupt and we don´t want to disable interrupts 
   // we can do two readings and compare.
   m = timer0_millis;
   m2 = timer0_millis;
   if (m!=m2)               // timer0_millis corrupted?
      m = timer0_millis;   // this should be fine...
   return m;
}

void DIYdelay(unsigned long ms)
{
   unsigned long start = DIYmillis();
   while (DIYmillis() - start <= ms)
      ;
}

float invSqrt(float number) {
  volatile long i;
  volatile float x, y;
  volatile const float f = 1.5F;

  x = number * 0.5F;
  y = number;
  i = * ( long * ) &y;
  i = 0x5f375a86 - ( i >> 1 );
  y = * ( float * ) &i;
  y = y * ( f - ( x * y * y ) );
  return y;
}

void GetRawValues(int *values)
{
	accel.readAccel(&values[0], &values[1], &values[2]);
	gyro.readGyroRaw(&values[3], &values[4], &values[5]);
}

void GetValues(float *values)
{
	ReadAccelerometer(values);
	gyro.readGyro(&values[3]);
}
/*
void GetRawAccelG(float *values)
{
	// to convert to G, we have to do this scaling. got this from 
	// https://answers.launchpad.net/freeimu/+question/178946
	float S = 1024.0/(float)(Sensitivity*2);//(16g)0.015625; // scale factor

	int accVal[3];
	accel.readAccel(&accVal[0], &accVal[1], &accVal[2]);

	// scale raw values
	values[0] = (float)(accVal[0]) * S;
	values[1] = (float)(accVal[1]) * S;
	values[2] = (float)(accVal[2]) * S;
}
*/
void ReadAccelerometer(float *values)
{
	// get data
	int accVal[3];
	accel.readAccel(&accVal[0], &accVal[1], &accVal[2]);

	int ival[3];

	// this website says since the data is in 2's complement
	// we just have to cast the data to the appropriate variable
	// http://mbed.org/forum/mbed/topic/1069/?page=1#comment-5173
	ival[0] = (int16_t)accVal[0];
	ival[1] = (int16_t)accVal[1];
	ival[2] = (int16_t)accVal[2];

	float S;
	if (FullResolution)
		S = 8192.0/(float)(Sensitivity*2);		// 13 bits
	else
		S = 1024.0/(float)(Sensitivity*2);		// 10 bits

	// divide values by sensitivity to get g's...
	values[0] = (float)ival[0] / S;
	values[1] = (float)ival[1] / S;
	values[2] = (float)ival[2] / S;
}

void CalibrateGyro()
{
	// calibrate the gyro to 0
	//Serial.print("Calibrating ITG3200...");
	gyro.zeroCalibrate(128, 5);
	delay(20);
	//Serial.println("done.");
}

void CalibrateAccelerometer(void) {
    
    //Take a number of readings and average them
    //to calculate any bias the accelerometer may have.

	// NOTE: ONLY "Z" appears to actually need any calibration to zero out. Strange.
	float accVal[3] = {0,0,0}; 
	int a_x = 0, a_y = 0, a_z = 0;
	float accum[3] = {0,0,0};

	//Serial.print("Calibrating ADXL345...");

	accelBias[0] = accelBias[1] = accelBias[2] = 0.0;

	// grab samples
	for (int i = 0; i < 200; i++) 
	{
		delay(1);		
		GetDynamicAcceleration(accVal, true);
		//ReadAccelerometer(accVal);
		for (int idx = 0; idx < 3; idx++)
			accum[idx] += accVal[idx];
	}

	// average samples
	for (int idx = 0; idx < 3; idx++)
		accelBias[idx] = accum[idx] / 200.0;

	//Serial.println("done.");
}

// Quaternion implementation of the 'DCM filter' [Mayhony et al].  Incorporates the magnetic distortion
// compensation algorithms from Sebastian Madgwick filter which eliminates the need for a reference
// direction of flux (bx bz) to be predefined and limits the effect of magnetic distortions to yaw
// axis only.
//
// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
//
//=====================================================================================================
void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az)
{
  float recipNorm;
  float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
  float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f;
  float qa, qb, qc;

  // Auxiliary variables to avoid repeated arithmetic
  q0q0 = q0 * q0;
  q0q1 = q0 * q1;
  q0q2 = q0 * q2;
  q0q3 = q0 * q3;
  q1q1 = q1 * q1;
  q1q2 = q1 * q2;
  q1q3 = q1 * q3;
  q2q2 = q2 * q2;
  q2q3 = q2 * q3;
  q3q3 = q3 * q3;

 // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
  if((ax != 0.0f) && (ay != 0.0f) && (az != 0.0f)) {
    float halfvx, halfvy, halfvz;
    
    // Normalise accelerometer measurement
    recipNorm = invSqrt(ax * ax + ay * ay + az * az);
    ax *= recipNorm;
    ay *= recipNorm;
    az *= recipNorm;
    
    // Estimated direction of gravity
    halfvx = q1q3 - q0q2;
    halfvy = q0q1 + q2q3;
    halfvz = q0q0 - 0.5f + q3q3;
  
    // Error is sum of cross product between estimated direction and measured direction of field vectors
    halfex += (ay * halfvz - az * halfvy);
    halfey += (az * halfvx - ax * halfvz);
    halfez += (ax * halfvy - ay * halfvx);
  }

  // Apply feedback only when valid data has been gathered from the accelerometer or magnetometer
  if(halfex != 0.0f && halfey != 0.0f && halfez != 0.0f) {
    // Compute and apply integral feedback if enabled
    if(twoKi > 0.0f) {
      integralFBx += twoKi * halfex * (1.0f / sampleFreq);  // integral error scaled by Ki
      integralFBy += twoKi * halfey * (1.0f / sampleFreq);
      integralFBz += twoKi * halfez * (1.0f / sampleFreq);
      gx += integralFBx;  // apply integral feedback
      gy += integralFBy;
      gz += integralFBz;
    }
    else {
      integralFBx = 0.0f; // prevent integral windup
      integralFBy = 0.0f;
      integralFBz = 0.0f;
    }

    // Apply proportional feedback
    gx += twoKp * halfex;
    gy += twoKp * halfey;
    gz += twoKp * halfez;
  }
  
  // Integrate rate of change of quaternion
  gx *= (0.5f * (1.0f / sampleFreq));   // pre-multiply common factors
  gy *= (0.5f * (1.0f / sampleFreq));
  gz *= (0.5f * (1.0f / sampleFreq));
  qa = q0;
  qb = q1;
  qc = q2;
  q0 += (-qb * gx - qc * gy - q3 * gz);
  q1 += (qa * gx + qc * gz - q3 * gy);
  q2 += (qa * gy - qb * gz + q3 * gx);
  q3 += (qa * gz + qb * gy - qc * gx);
  
  // Normalise quaternion
  recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
  q0 *= recipNorm;
  q1 *= recipNorm;
  q2 *= recipNorm;
  q3 *= recipNorm;
}

void getQ(float *q)
{
	float val[9];
	GetValues(val);

    // gyro values are expressed in deg/sec, the * M_PI/180 will convert it to radians/sec
	//AHRSupdate(val[3] * M_PI/180, val[4] * M_PI/180, val[5] * M_PI/180, val[0], val[1], val[2]);
	MadgwickAHRSupdateIMU(val[3] * M_PI/180, val[4] * M_PI/180, val[5] * M_PI/180, val[0], val[1], val[2]);

	q[0] = q0;
	q[1] = q1;
	q[2] = q2;
	q[3] = q3;
}

//---------------------------------------------------------------------------------------------------
// IMU algorithm update

void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
	float recipNorm;
	float beta = 0.1f;
	//float sampleFreq = 512.0f;
	float s0, s1, s2, s3;
	float qDot1, qDot2, qDot3, qDot4;
	float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;

	// Rate of change of quaternion from gyroscope
	qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
	qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
	qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
	qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);

	// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
	if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {

		// Normalise accelerometer measurement
		recipNorm = invSqrt(ax * ax + ay * ay + az * az);
		ax *= recipNorm;
		ay *= recipNorm;
		az *= recipNorm;   

		// Auxiliary variables to avoid repeated arithmetic
		_2q0 = 2.0f * q0;
		_2q1 = 2.0f * q1;
		_2q2 = 2.0f * q2;
		_2q3 = 2.0f * q3;
		_4q0 = 4.0f * q0;
		_4q1 = 4.0f * q1;
		_4q2 = 4.0f * q2;
		_8q1 = 8.0f * q1;
		_8q2 = 8.0f * q2;
		q0q0 = q0 * q0;
		q1q1 = q1 * q1;
		q2q2 = q2 * q2;
		q3q3 = q3 * q3;

		// Gradient decent algorithm corrective step
		s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
		s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
		s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
		s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
		recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
		s0 *= recipNorm;
		s1 *= recipNorm;
		s2 *= recipNorm;
		s3 *= recipNorm;

		// Apply feedback step
		qDot1 -= beta * s0;
		qDot2 -= beta * s1;
		qDot3 -= beta * s2;
		qDot4 -= beta * s3;
	}

	// Integrate rate of change of quaternion to yield quaternion
	q0 += qDot1 * (1.0f / sampleFreq);
	q1 += qDot2 * (1.0f / sampleFreq);
	q2 += qDot3 * (1.0f / sampleFreq);
	q3 += qDot4 * (1.0f / sampleFreq);

	// Normalise quaternion
	recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
	q0 *= recipNorm;
	q1 *= recipNorm;
	q2 *= recipNorm;
	q3 *= recipNorm;
}


// Returns the Euler angles in radians defined with the Aerospace sequence.
// See Sebastian O.H. Madwick report 
// "An efficient orientation filter for inertial and intertial/magnetic sensor arrays" Chapter 2 Quaternion representation
void getEuler(float * angles) {
  float q[4]; // quaternion
  getQ(q);
  angles[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1) * 180/M_PI; // psi
  angles[1] = -asin(2 * q[1] * q[3] + 2 * q[0] * q[2]) * 180/M_PI; // theta
  angles[2] = atan2(2 * q[2] * q[3] - 2 * q[0] * q[1], 2 * q[0] * q[0] + 2 * q[3] * q[3] - 1) * 180/M_PI; // phi
}

void getYawPitchRoll(float * ypr) {
  float q[4]; // quaternion
  float gx, gy, gz; // estimated gravity direction
  //getQ(q);

  q[0] = g_Quat[0];
  q[1] = g_Quat[1];
  q[2] = g_Quat[2];
  q[3] = g_Quat[3];
  
  gx = 2 * (q[1]*q[3] - q[0]*q[2]);
  gy = 2 * (q[0]*q[1] + q[2]*q[3]);
  gz = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
  
  ypr[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1) * 180/M_PI;
  ypr[1] = atan(gx / sqrt(gy*gy + gz*gz))  * 180/M_PI;
  ypr[2] = atan(gy / sqrt(gx*gx + gz*gz))  * 180/M_PI;
}

// dyanamic acceleration is acceleration without gravity included
void GetDynamicAcceleration(float *accelVec, bool cal)
{
	static float prevAccel[3] = {0,0,0};

	// get 64 samples of data
	float q[4];
	float qTotal[4] = {0,0,0,0};

	float accel[3];
	
	float aTotal[3] = {0,0,0};

	if (cal)
		getQ(q);
	else
	{
		q[0] = g_Quat[0];
		q[1] = g_Quat[1];
		q[2] = g_Quat[2];
		q[3] = g_Quat[3];
	}

	ReadAccelerometer(accelVec);

	float g[3] = {0,0,0};

	// calc expected direction of gravity
	g[0] = 2 * (q[1] * q[3] - q[0] * q[2]);
	g[1] = 2 * (q[0] * q[1] + q[2] * q[3]);
	g[2] = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];

	// adjust for bias, take gravity out of the equation
	// adjusting for gravity causes system to take time to stabilize..
	accelVec[0] -= g[0] + accelBias[0];
	accelVec[1] -= g[1] + accelBias[1];
	accelVec[2] -= g[2] + accelBias[2];	// for gravity

	//for (int idx = 0; idx < 3; idx++)
	//	if (IS_ZERO(accelVec[idx]-prevAccel[idx], AccelerationMin))
	//		accelVec[idx] = 0.0;

	prevAccel[0] = accelVec[0];
	prevAccel[1] = accelVec[1];
	prevAccel[2] = accelVec[2];
}
/*
void GetVelocity(float *accelVec, float *velocity, float deltaTime)
{
	static float prevVelocity[3] = {0,0,0};
	static float prevAccel[3] = {0,0,0};
	static int noAccelCount[3] = {0,0,0};

	for (int idx = 0; idx < 3; idx++)
	{
		if (IS_ZERO(accelVec[idx]-prevAccel[idx], AccelerationMin))
			noAccelCount[idx]++;
		else
			noAccelCount[idx] = 0;

		if (noAccelCount[idx] > NoMovementThreshold)
		{
			velocity[idx] = 0.0;
			noAccelCount[idx] = 0;
		}
		else
		{
			velocity[idx] = prevVelocity[idx] + (prevAccel[idx] + ((accelVec[idx] - prevAccel[idx])/2.0))*deltaTime;
		}

		// store new previous values for next iteration
		prevVelocity[idx] = velocity[idx];
		prevAccel[idx] = accelVec[idx];
	}
}

void GetPosition(float *velocity, float *position, float deltaTime)
{
	static float prevPos[3] = {0,0,0};
	static float prevVel[3] = {0,0,0};

	for (int idx = 0; idx < 3; idx++)
	{
		position[idx] = prevPos[idx] + (prevVel[idx] + ((velocity[idx] - prevVel[idx]) / 2.0))*deltaTime;
		position[idx] = position[idx];

		prevVel[idx] = velocity[idx];
		prevPos[idx] = position[idx];
	}
}

void PrintVector(float *v)
{
	Serial.print("(");
	Serial.print(v[0]);
	Serial.print(", ");
	Serial.print(v[1]);
	Serial.print(", ");
	Serial.print(v[2]);
	Serial.print(")");
}
*/
void SendVectorData(unsigned long tick, float *v, bool end)
{
	// vector has 3 components
	Serial.print(tick);
	Serial.print(",");
	Serial.print(v[0]);
	Serial.print(",");
	Serial.print(v[1]);
	Serial.print(",");
	Serial.print(v[2]);
	if (end)
		Serial.println("");
	else
		Serial.print(",");
}

void SendVectorData(float *v, bool end)
{
	// vector has 3 components
	Serial.print(v[0]);
	Serial.print(",");
	Serial.print(v[1]);
	Serial.print(",");
	Serial.print(v[2]);
	if (end)
		Serial.println("");
	else
		Serial.print(",");
}
/*
void SendVectorData(int *v)
{
	// vector has 3 components
	Serial.print(v[0]);
	Serial.print(",");
	Serial.print(v[1]);
	Serial.print(",");
	Serial.println(v[2]);
}
*/
bool GetDeviceConnection()
{
	bool connect = false;

	// C starts the data acquisition, S stops it
	int cmd = Serial.read();
	switch(cmd)
	{
	case 'C':
		connect = true; break;
	case 'S':
		connect = false; break;
	default:
		connect = connect; break;
	}

	return connect;
}

void setup(void){
  Serial.begin(115200);
  delay(10);
  Wire.begin();
  delay(10);
  Serial.print("Arduino init...");

  // disable internal pullups of ATMEGA which Wire enable by default
  //cbi(PORTC, 0);
  //cbi(PORTC, 1);

  //pinMode(7, OUTPUT);		// enable connection LED DO
  //digitalWrite(7, LOW);

  // init accelerometer
  accel.init(ADXL345_ADDR_ALT_LOW);
  accel.setRangeSetting(Sensitivity);
  accel.setFullResBit(FullResolution);
  accel.set_bw(ADXL345_BW_50);
  CalibrateAccelerometer();

 // accel.set_bw(ADXL345_BW_12);

  if (accel.status == ADXL345_OK)
	  Serial.print("ADXL345 OK...");
  else
  {
	  Serial.print("ADXL345 ERROR: ");
	  Serial.println(accel.error_code);
  }

  // init gyro
  gyro.init(ITG3200_ADDR_AD0_LOW);
  gyro.setFSRange(RANGE2000);
  gyro.setFilterBW(BW256_SR8);//BW042_SR1);
  CalibrateGyro();

  Serial.println("ITG3200 OK.");

  timer = millis();

  // we'll wait for the connection before sending data
  bDeviceConnected = false;

  delay(100);
}

void loop(void)
{
	unsigned long microsecs = micros();
	unsigned long timestamp = 0;

	now = micros();
	sampleFreq = 1.0 / ((now - lastUpdate) / 1000000.0);
	lastUpdate = now;

	//if ((DIYmillis() - timer) >= 5) // run at 50hz
	{
		 timer_old = timer;
        timer=DIYmillis();
        G_Dt = (timer-timer_old)/1000.0;    // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
        if(G_Dt > 1)
            G_Dt = 0;  //keeps dt from blowing up, goes to zero to keep gyros from departing

        getQ(g_Quat);

		if (!bDeviceConnected)
		{
			// temporary for testing purposes
			//bDeviceConnected= true;		
	
			int cmd = Serial.read();
			if ((cmd > 0) && (cmd == 'C'))
			{
				CalibrateAccelerometer();
				CalibrateGyro();
				bDeviceConnected = true;			
				
				//digitalWrite(7, HIGH);

				//Serial.println("R");

				// grab current orientation - orientation will be relative to this position from this point forward
				getYawPitchRoll(yprBias);
				lastUpdate = lastMicrosec = micros();
			}
		}
		else
		{
			timestamp = microsecs - lastMicrosec;

			//getQ(g_Quat);

			float ypr[3];
			getYawPitchRoll(ypr);
			//ypr[0]-=yprBias[0];
			//ypr[1]-=yprBias[1];
			//ypr[2]-=yprBias[2];
			SendVectorData(timestamp, ypr, false);
  
			float dynamicAccel[3];
			GetDynamicAcceleration(dynamicAccel, false);
			SendVectorData(dynamicAccel, true);

			int cmd = Serial.read();
			if ((cmd > 0) && (cmd == 'S'))
			{
				bDeviceConnected = false;
				//digitalWrite(7, LOW);
			}
		}



		// check for end connection
		//bDeviceConnected = GetDeviceConnection();

		//float velocity[3] = {0,0,0};
		//GetVelocity(dynamicAccel, velocity, deltaTime);
		//SendVectorData(velocity, false);

		//float position[3] = {0,0,0};
		//GetPosition(velocity, position, deltaTime);
		//SendVectorData(position, true);
	}

	//delay(5);
}