search

michaelcheng1991 / Golf-Training-Aid-

Using sensors to create a golfer-friendly teaching device
0 stars 0 forks source link

your code #26

Open michaelcheng1991 opened 8 years ago

michaelcheng1991 commented 8 years ago

//this is the arduino 3 IMU output code

include //add arduino SPI library;

//list of all register's binary addresses;

byte ACT_THS = 0B00000100; byte ACT_DUR = 0B00000101; byte INT_GEN_CFG_XL = 0B00000110; byte INT_GEN_THS_X_XL = 0B00000111; byte INT_GEN_THS_Y_XL = 0B00001000; byte INT_GEN_THS_Z_XL = 0B00001001; byte INT_GEN_DUR_XL = 0B00001010; byte REFERENCE_G = 0B00001011; byte INT1_CTRL = 0B00001100; byte INT2_CTRL = 0B00001101;

byte WHO_AM_I = 0B00001111; byte CTRL_REG1_G = 0B00010000; byte CTRL_REG2_G = 0B00010001; byte CTRL_REG3_G = 0B00010010; byte ORIENT_CFG_G = 0B00010011; byte INT_GEN_SRC_G = 0B00010100; byte OUT_TEMP_L = 0B00010101; byte OUT_TEMP_H = 0B00010110; byte STATUS_REG0 = 0B00010111; byte OUT_X_L_G = 0B00011000; byte OUT_X_H_G = 0B00011001; byte OUT_Y_L_G = 0B00011010; byte OUT_Y_H_G = 0B00011011; byte OUT_Z_L_G = 0B00011100; byte OUT_Z_H_G = 0B00011101; byte CTRL_REG4 = 0B00011110; byte CTRL_REG5_XL = 0B00011111; byte CTRL_REG6_XL = 0B00100000; byte CTRL_REG7_XL = 0B00100001; byte CTRL_REG8 = 0B00100010; byte CTRL_REG9 = 0B00100011; byte CTRL_REG10 = 0B00100100;

byte INT_GEN_SRC_XL = 0B00100110; byte STATUS_REG = 0B00100111; byte OUT_X_L_XL = 0B00101000; byte OUT_X_H_XL = 0B00101001; byte OUT_Y_L_XL = 0B00101010; byte OUT_Y_H_XL = 0B00101011; byte OUT_Z_L_XL = 0B00101100; byte OUT_Z_H_XL = 0B00101101; byte FIFO_CTRL = 0B00101110; byte FIFO_SRC = 0B00101111; byte INT_GEN_CFG_G = 0B00110000; byte INT_GEN_THS_XH_G = 0B00110001; byte INT_GEN_THS_XL_G = 0B00110010; byte INT_GEN_THS_YH_G = 0B00110011; byte INT_GEN_THS_YL_G = 0B00110100; byte INT_GEN_THS_ZH_G = 0B00110101; byte INT_GEN_THS_ZL_G = 0B00110110; byte INT_GEN_DUR_G = 0B00110111;

byte OFFSET_X_REG_L_M = 0B00000101; byte OFFSET_X_REG_H_M = 0B00000110; byte OFFSET_Y_REG_L_M = 0B00000111; byte OFFSET_Y_REG_H_M = 0B00001000; byte OFFSET_Z_REG_L_M = 0B00001001; byte OFFSET_Z_REG_H_M = 0B00001010;

//byte WHO_AM_I_M = 0B00001111;

byte CTRL_REG1_M = 0B00100000; byte CTRL_REG2_M = 0B00100001; byte CTRL_REG3_M = 0B00100010; byte CTRL_REG4_M = 0B00100011; byte CTRL_REG5_M = 0B00100100;

byte STATUS_REG_M = 0B00100111; byte OUT_X_L_M = 0B00101000; byte OUT_X_H_M = 0B00101001; byte OUT_Y_L_M = 0B00101010; byte OUT_Y_H_M = 0B00101011; byte OUT_Z_L_M = 0B00101100; byte OUT_Z_H_M = 0B00101101;

byte INT_CFG_M = 0B00110000; byte INT_SRC_M = 0B00110001; byte INT_THS_L_M = 0B00110010; byte INT_THS_H_M = 0B00110011; byte Read = 0B10000000; byte Write = 0B00000000;

//chip select pin on arduino; const int CS_AG = 7; const int CS_M = 8; const int CS_AG_1 = 9; const int CS_M_1 = 10; const int CS_AG_2 = 11; const int CS_M_2 = 12;

void setup() {
Serial.begin(115200); //start the SPI library; SPI.begin(); SPI.setClockDivider(1); //starts with 16MHz //https://www.arduino.cc/en/Reference/SPISettings

//initalize the chip select pins; pinMode(CS_AG, OUTPUT); pinMode(CS_M, OUTPUT); pinMode(CS_AG_1,OUTPUT); pinMode(CS_M_1,OUTPUT); pinMode(CS_AG_2,OUTPUT); pinMode(CS_M_2,OUTPUT);

digitalWrite(CS_AG, HIGH); digitalWrite(CS_M,HIGH); digitalWrite(CS_AG_1,HIGH); digitalWrite(CS_M_1,HIGH); digitalWrite(CS_AG_2,HIGH); digitalWrite(CS_M_2,HIGH);

//Make sure you know odr is changed WriteRegister_AG(CTRL_REG4,0B00111000); //enable gyro axis WriteRegister_AG(CTRL_REG5_XL,0B00111000); //enable accelerometer WriteRegister_AG(CTRL_REG1_G,0B01000000); //gyro/accel odr and bw gonna change 001 to 010 WriteRegister_M(CTRL_REG3_M,0B00000000); //enable mag continuous

WriteRegister_AG_1(CTRL_REG4,0B00111000); //enable gyro axis WriteRegister_AG_1(CTRL_REG5_XL,0B00111000); //enable accelerometer WriteRegister_AG_1(CTRL_REG1_G,0B01000000); //gyro/accel odr and bw gonna change 001 to 010 WriteRegister_M_1(CTRL_REG3_M,0B00000000); //enable mag continuous

WriteRegister_AG_2(CTRL_REG4,0B00111000); //enable gyro axis WriteRegister_AG_2(CTRL_REG5_XL,0B00111000); //enable accelerometer WriteRegister_AG_2(CTRL_REG1_G,0B00100000); //gyro/accel odr and bw WriteRegister_M_2(CTRL_REG3_M,0B00000000); //enable mag continuous

delay(200);

}

void loop() { //this is to show how long a loop takes unsigned long millisStart = millis();

//alloff(); //NOTE: ALL OFF IS COMMENTED OUT

//This section is for IMU 1 CS: 9,10 //byte WHOAMI = ReadRegister_AG(WHO_AM_I);

//this section is for IMU 2 CS : 7,8 //byte WHOAMI_1 = ReadRegister_AG_1(WHO_AM_I);

//this section is for IMU 3 CS : 11,12 //byte WHOAMI_2 = ReadRegister_AG_2(WHO_AM_I);

//gyro byte X_L = ReadRegister_AG(OUT_X_L_G); byte X_H = ReadRegister_AG(OUT_X_H_G); byte Y_L = ReadRegister_AG(OUT_Y_L_G); byte Y_H = ReadRegister_AG(OUT_Y_H_G); byte Z_L = ReadRegister_AG(OUT_Z_L_G); byte Z_H = ReadRegister_AG(OUT_Z_H_G);

int X_AXIs = X_H <<8 | X_L; int Y_AXIs = Y_H <<8 | Y_L; int Z_AXIs = Z_H <<8 | Z_L;

//accelerameter byte X_L_A = ReadRegister_AG(OUT_X_L_XL); byte X_H_A = ReadRegister_AG(OUT_X_H_XL); byte Y_L_A = ReadRegister_AG(OUT_Y_L_XL); byte Y_H_A = ReadRegister_AG(OUT_Y_H_XL); byte Z_L_A = ReadRegister_AG(OUT_Z_L_XL); byte Z_H_A = ReadRegister_AG(OUT_Z_H_XL);

int X_AXIS_A = X_H_A <<8 | X_L_A; int Y_AXIS_A = Y_H_A <<8 | Y_L_A; int Z_AXIS_A = Z_H_A <<8 | Z_L_A;

//.000061+.022 may need recalibration float x = X_AXIS_A_0.000061+.022; float y = Y_AXIS_A_0.000061+.0017; float z = Z_AXIS_A_0.000061-.022; float pitch = atan(x/sqrt((y_y)+(zz)))(180/PI); float roll = atan(y/sqrt((x_x)+(zz)))(180/PI); float yaw = atan(z/sqrt((x_x)+(yy)))(180/PI);

//magnetometer byte X_L_M = ReadRegister_M(OUT_X_L_M); byte X_H_M = ReadRegister_M(OUT_X_H_M); byte Y_L_M = ReadRegister_M(OUT_Y_L_M); byte Y_H_M = ReadRegister_M(OUT_Y_H_M); byte Z_L_M = ReadRegister_M(OUT_Z_L_M); byte Z_H_M = ReadRegister_M(OUT_Z_H_M);

int X_AXIS_M = X_H_M <<8 | X_L_M; int Y_AXIS_M = Y_H_M <<8 | Y_L_M; int Z_AXIS_M = Z_H_M <<8 | Z_L_M;

//Serial.println("Port 1");

//Serial.print("X-Axis: "); //.00875 is from the datasheet and my settings when i writeregister Serial.print(0.00875_(XAXIs+110), DEC); Serial.print("\t"); //Serial.println(" dps"); //Serial.print("Y-Axis: ");
Serial.print(0.00875(Y_AXIs+190), DEC); Serial.print("\t"); //Serial.println(" dps"); //Serial.print("Z-Axis: ");
Serial.print(0.00875*(Z_AXIs+115), DEC); Serial.print("\t"); //Serial.println(" dps");

//Serial.print("X-Axis_A: "); Serial.print(X_AXIS_A_0.000061, DEC); // .000061 comes from the datasheet and my settings for the accelerometer. Serial.print("\t"); //Serial.println(" G"); //Serial.print("Y-Axis_A: "); // output will be in g's. Serial.print(Y_AXIS_A_0.000061, DEC); Serial.print("\t"); //Serial.println(" G"); //Serial.print("Z-Axis_A: "); Serial.print(Z_AXIS_A*0.000061, DEC); Serial.print("\t"); //Serial.println(" G");

//Serial.print("X-Axis_M: "); Serial.print(X_AXIS_M_0.00014, DEC); // 0.00014 comes from the datasheet and my settings for the magnetometer Serial.print("\t"); //Serial.println(" Gauss"); //Serial.print("Y-Axis_M: "); // output will be in gauss Serial.print(Y_AXIS_M_0.00014, DEC); Serial.print("\t"); //Serial.println(" Gauss"); //Serial.print("Z-Axis_M: "); Serial.print(Z_AXIS_M*0.00014, DEC); Serial.print("\t"); //Serial.println(" Gauss");

/ Serial.println("IMU1"); Serial.print("roll:"); Serial.println(roll); Serial.print("pitch:"); Serial.println(pitch); Serial.print("yaw:"); Serial.println(yaw); /

//IMU2 //gyro byte X_L_1 = ReadRegister_AG_1(OUT_X_L_G); byte X_H_1 = ReadRegister_AG_1(OUT_X_H_G); byte Y_L_1 = ReadRegister_AG_1(OUT_Y_L_G); byte Y_H_1 = ReadRegister_AG_1(OUT_Y_H_G); byte Z_L_1 = ReadRegister_AG_1(OUT_Z_L_G); byte Z_H_1 = ReadRegister_AG_1(OUT_Z_H_G);

int X_AXIs_1 = X_H_1 <<8 | X_L_1; int Y_AXIs_1 = Y_H_1 <<8 | Y_L_1; int Z_AXIs_1 = Z_H_1 <<8 | Z_L_1;

//acelerometer byte X_L_A_1 = ReadRegister_AG_1(OUT_X_L_XL); byte X_H_A_1 = ReadRegister_AG_1(OUT_X_H_XL); byte Y_L_A_1 = ReadRegister_AG_1(OUT_Y_L_XL); byte Y_H_A_1 = ReadRegister_AG_1(OUT_Y_H_XL); byte Z_L_A_1 = ReadRegister_AG_1(OUT_Z_L_XL); byte Z_H_A_1 = ReadRegister_AG_1(OUT_Z_H_XL);

int X_AXIS_A_1 = X_H_A_1 <<8 | X_L_A_1; int Y_AXIS_A_1 = Y_H_A_1 <<8 | Y_L_A_1; int Z_AXIS_A_1 = Z_H_A_1 <<8 | Z_L_A_1;

//.000061+.022 may need recalibration float x_1 = X_AXIS_A_1_0.000061+.022; float y_1 = Y_AXIS_A_1_0.000061+.0017; float z_1 = Z_AXIS_A_1_0.000061-.022; float pitch_1 = atan(x_1/sqrt((y_1_y_1)+(z_1_z1)))(180/PI); float roll_1 = atan(y_1/sqrt((x_1_x_1)+(z_1_z1)))(180/PI); float yaw_1 = atan(z_1/sqrt((x_1_x_1)+(y_1_y1)))(180/PI);

//magnetometer byte X_L_M_1 = ReadRegister_M_1(OUT_X_L_M); byte X_H_M_1 = ReadRegister_M_1(OUT_X_H_M); byte Y_L_M_1 = ReadRegister_M_1(OUT_Y_L_M); byte Y_H_M_1 = ReadRegister_M_1(OUT_Y_H_M); byte Z_L_M_1 = ReadRegister_M_1(OUT_Z_L_M); byte Z_H_M_1 = ReadRegister_M_1(OUT_Z_H_M);

int X_AXIS_M_1 = X_H_M_1 <<8 | X_L_M_1; int Y_AXIS_M_1 = Y_H_M_1 <<8 | Y_L_M_1; int Z_AXIS_M_1 = Z_H_M_1 <<8 | Z_L_M_1;

//Serial.println("Port 2"); //Serial.print("X-Axis: "); //.00875 is from the datasheet and my settings when i writeregister Serial.print(0.00875_(X_AXIs1+110), DEC); Serial.print("\t"); //Serial.println(" dps"); //Serial.print("Y-Axis: ");
Serial.print(0.00875(Y_AXIs_1+190), DEC); Serial.print("\t"); //Serial.println(" dps"); //Serial.print("Z-Axis: ");
Serial.print(0.00875*(Z_AXIs_1+115), DEC); Serial.print("\t"); //Serial.println(" dps");

//Serial.print("X-Axis_A: "); Serial.print(X_AXIS_A_1_0.000061, DEC); // .000061 comes from the datasheet and my settings for the accelerometer. Serial.print("\t"); //Serial.println(" G"); //Serial.print("Y-Axis_A: "); // output will be in g's. Serial.print(Y_AXIS_A_1_0.000061, DEC); Serial.print("\t"); //Serial.println(" G"); //Serial.print("Z-Axis_A: "); Serial.print(Z_AXIS_A_1*0.000061, DEC); Serial.print("\t"); //Serial.println(" G");

//Serial.print("X-Axis_M: "); Serial.print(X_AXIS_M_1_0.00014, DEC); // 0.00014 comes from the datasheet and my settings for the magnetometer Serial.print("\t"); //Serial.println(" Gauss"); //Serial.print("Y-Axis_M: "); // output will be in gauss Serial.print(Y_AXIS_M_1_0.00014, DEC); Serial.print("\t"); //Serial.println(" Gauss"); //Serial.print("Z-Axis_M: "); Serial.print(Z_AXIS_M_1*0.00014, DEC); Serial.print("\t"); //Serial.println(" Gauss");

/ Serial.println("IMU2"); Serial.print("roll:"); Serial.println(roll_1); Serial.print("pitch:"); Serial.println(pitch_1); Serial.print("yaw:"); Serial.println(yaw_1); /

//IMU 3 //gyro byte X_L_2 = ReadRegister_AG_2(OUT_X_L_G); byte X_H_2 = ReadRegister_AG_2(OUT_X_H_G); byte Y_L_2 = ReadRegister_AG_2(OUT_Y_L_G); byte Y_H_2 = ReadRegister_AG_2(OUT_Y_H_G); byte Z_L_2 = ReadRegister_AG_2(OUT_Z_L_G); byte Z_H_2 = ReadRegister_AG_2(OUT_Z_H_G);

int X_AXIs_2 = X_H_2 <<8 | X_L_2; int Y_AXIs_2 = Y_H_2 <<8 | Y_L_2; int Z_AXIs_2 = Z_H_2 <<8 | Z_L_2;

//accelerameter byte X_L_A_2 = ReadRegister_AG_2(OUT_X_L_XL); byte X_H_A_2 = ReadRegister_AG_2(OUT_X_H_XL); byte Y_L_A_2 = ReadRegister_AG_2(OUT_Y_L_XL); byte Y_H_A_2 = ReadRegister_AG_2(OUT_Y_H_XL); byte Z_L_A_2 = ReadRegister_AG_2(OUT_Z_L_XL); byte Z_H_A_2 = ReadRegister_AG_2(OUT_Z_H_XL);

int X_AXIS_A_2 = X_H_A_2 <<8 | X_L_A_2; int Y_AXIS_A_2 = Y_H_A_2 <<8 | Y_L_A_2; int Z_AXIS_A_2 = Z_H_A_2 <<8 | Z_L_A_2;

//.000061+.022 may need recalibration float x_2 = X_AXIS_A_2_0.000061+.022; float y_2 = Y_AXIS_A_2_0.000061+.0017; float z_2 = Z_AXIS_A_2_0.000061-.022; float pitch_2 = atan(x_2/sqrt((y_2_y_2)+(z_2_z2)))(180/PI); float roll_2 = atan(y_2/sqrt((x_2_x_2)+(z_2_z2)))(180/PI); float yaw_2 = atan(z_2/sqrt((x_2_x_2)+(y_2_y2)))(180/PI);

//magnetometer byte X_L_M_2 = ReadRegister_M_2(OUT_X_L_M); byte X_H_M_2 = ReadRegister_M_2(OUT_X_H_M); byte Y_L_M_2 = ReadRegister_M_2(OUT_Y_L_M); byte Y_H_M_2 = ReadRegister_M_2(OUT_Y_H_M); byte Z_L_M_2 = ReadRegister_M_2(OUT_Z_L_M); byte Z_H_M_2 = ReadRegister_M_2(OUT_Z_H_M);

int X_AXIS_M_2 = X_H_M_2 <<8 | X_L_M_2; int Y_AXIS_M_2 = Y_H_M_2 <<8 | Y_L_M_2; int Z_AXIS_M_2 = Z_H_M_2 <<8 | Z_L_M_2;

//Serial.println("Port 3"); //Serial.print("X-Axis: "); //.00875 is from the datasheet and my settings when i writeregister Serial.print(0.00875_(X_AXIs2+110), DEC); Serial.print("\t"); //Serial.println(" dps"); //Serial.print("Y-Axis: ");
Serial.print(0.00875(Y_AXIs_2+190), DEC); Serial.print("\t"); //Serial.println(" dps"); //Serial.print("Z-Axis: ");
Serial.print(0.00875*(Z_AXIs_2+115), DEC); Serial.print("\t"); //Serial.println(" dps");

//Serial.println(" G"); //Serial.print("X-Axis_A: "); Serial.print(X_AXIS_A_2_0.000061, DEC); // .000061 comes from the datasheet and my settings for the accelerometer. Serial.print("\t"); //Serial.println(" G"); //Serial.print("Y-Axis_A: "); // output will be in g's. Serial.print(Y_AXIS_A_2_0.000061, DEC); Serial.print("\t"); //Serial.println(" G"); //Serial.print("Z-Axis_A: "); Serial.print(Z_AXIS_A_2*0.000061, DEC); Serial.print("\t");

//Serial.print("X-Axis_M: "); Serial.print(X_AXIS_M_2_0.00014, DEC); // 0.00014 comes from the datasheet and my settings for the magnetometer Serial.print("\t"); //Serial.println(" Gauss"); //Serial.print("Y-Axis_M: "); // output will be in gauss Serial.print(Y_AXIS_M_2_0.00014, DEC); Serial.print("\t"); //Serial.println(" Gauss"); //Serial.print("Z-Axis_M: "); Serial.print(Z_AXIS_M_2*0.00014, DEC); Serial.println(); //Serial.println(" Gauss");

/ Serial.println("IMU3"); Serial.print("roll:"); Serial.println(roll_2); Serial.print("pitch:"); Serial.println(pitch_2); Serial.print("yaw:"); Serial.println(yaw_2); /

//delay(50); }

void alloff() { WriteRegister_AG(CS_AG,255); WriteRegister_M(CS_M,255); WriteRegister_AG_1(CS_AG_1,255); WriteRegister_M_1(CS_M_1,255); }

byte ReadRegister_M(byte Address){ byte result = 0; digitalWrite(CS_M, LOW); SPI.transfer(Read | Address); result = SPI.transfer(0x00); digitalWrite(CS_M, HIGH); return(result);
}

byte ReadRegister_AG(byte Address){ byte result = 0; digitalWrite(CS_AG, LOW); SPI.transfer(Read | Address); result = SPI.transfer(0x00); //Serial.print(Address, BIN); //Serial.print(" : "); //Serial.println(result, BIN); digitalWrite(CS_AG, HIGH); return(result);
}

void WriteRegister_M(byte Address, byte Value){ digitalWrite(CS_M, LOW); SPI.transfer(Write | Address); SPI.transfer(Value); digitalWrite(CS_M, HIGH);
}

void WriteRegister_AG(byte Address, byte Value){ digitalWrite(CS_AG, LOW); SPI.transfer(Write | Address); SPI.transfer(Value); digitalWrite(CS_AG, HIGH);
}

byte ReadRegister_M_1(byte Address){ byte result = 0; digitalWrite(CS_M_1, LOW); SPI.transfer(Read | Address); result = SPI.transfer(0x00); digitalWrite(CS_M_1, HIGH); return(result);
}

byte ReadRegister_AG_1(byte Address){ byte result = 0; digitalWrite(CS_AG_1, LOW); SPI.transfer(Read | Address); result = SPI.transfer(0x00); //Serial.print(Address, BIN); //Serial.print(" : "); //Serial.println(result, BIN); digitalWrite(CS_AG_1, HIGH); return(result);
}

void WriteRegister_M_1(byte Address, byte Value){ digitalWrite(CS_M_1, LOW); SPI.transfer(Write | Address); SPI.transfer(Value); digitalWrite(CS_M_1, HIGH);
}

void WriteRegister_AG_1(byte Address, byte Value){ digitalWrite(CS_AG_1, LOW); SPI.transfer(Write | Address); SPI.transfer(Value); digitalWrite(CS_AG_1, HIGH);
}

byte ReadRegister_M_2(byte Address){ byte result = 0; digitalWrite(CS_M_2, LOW); SPI.transfer(Read | Address); result = SPI.transfer(0x00); digitalWrite(CS_M_2, HIGH); return(result);
}

byte ReadRegister_AG_2(byte Address){ byte result = 0; digitalWrite(CS_AG_2, LOW); SPI.transfer(Read | Address); result = SPI.transfer(0x00); //Serial.print(Address, BIN); //Serial.print(" : "); //Serial.println(result, BIN); digitalWrite(CS_AG_2, HIGH); return(result);
}

void WriteRegister_M_2(byte Address, byte Value){ digitalWrite(CS_M_2, LOW); SPI.transfer(Write | Address); SPI.transfer(Value); digitalWrite(CS_M_2, HIGH);
}

void WriteRegister_AG_2(byte Address, byte Value){ digitalWrite(CS_AG_2, LOW); SPI.transfer(Write | Address); SPI.transfer(Value); digitalWrite(CS_AG_2, HIGH);
}

michaelcheng1991 commented 8 years ago

%this is the MatLab receiver code

%%this script attempt to integrate Gait tracking and arduino data

%1. inport Data from IMU (5000 sets of data) %2. use script.m

addpath('Quaternions'); addpath('ximu_matlab_library');

%initialize serial port %fclose(instrfindall);

delete (instrfindall); s = serial ('/dev/tty.usbmodem1411'); set(s,'BaudRate', 115200); %set(s,'DataBits',8); %set(s,'StopBits',1); fopen(s); s.ReadAsyncMode='continuous';

s.BytesAvailable

%Start asynchronous reading readasync(s); while(s.BytesAvailable <=0) % sometimes program terminates here

end

s.BytesAvailable

numofData = 1000; mData = zeros(numofData,27); %preallocating array ?? mData{1000,27}

%pause so that arduino could catch up %pause(.5); tic; for i = 1: numofData sSerialData=fscanf(s);
t=strsplit(sSerialData,'\t'); for j= 1:27 mData(i,j) = str2double(t(j)); end end toc;

stopasync(s); fclose(s);

%export data to excel filename = 'swingdatanum1.csv'; csvwrite(filename,mData);

% Import data samplePeriod = 1/256; %original is 1/256 %xIMUdata = xIMUdataClass(filePath, 'InertialMagneticSampleRate', 1/samplePeriod); time = 1:numofData; gyrX = mData(:,1); gyrY = mData(:,2); gyrZ = mData(:,3); accX = mData(:,4); accY = mData(:,5); accZ = mData(:,6); gyrX_1 = mData(:,10); gyrY_1 = mData(:,11); gyrZ_1 = mData(:,12); accX_1 = mData(:,13); accY_1 = mData(:,14); accZ_1 = mData(:,15); gyrX_2 = mData(:,19); gyrY_2 = mData(:,20); gyrZ_2 = mData(:,21); accX_2 = mData(:,22); accY_2 = mData(:,23); accZ_2 = mData(:,24); %%clear('xIMUdata');

% ------------------------------------------------------------------------- % Manually frame data

% startTime = 0; these two were comment out initially % stopTime = 10; % % indexSel = find(sign(time-startTime)+1, 1) : find(sign(time-stopTime)+1, 1); %find(x,n) returns the first n indices corresponding to the nonzero elements as X % time = time(indexSel); % gyrX = gyrX(indexSel, :); %%indexSel= 1~513 in columns 1 2 3 4 5 6....513 % gyrY = gyrY(indexSel, :); % comes out to be rows of data % gyrZ = gyrZ(indexSel, :); % accX = accX(indexSel, :); % accY = accY(indexSel, :); % accZ = accZ(indexSel, :);

% ------------------------------------------------------------------------- % Detect stationary periods

% Compute accelerometer magnitude acc_mag = sqrt(accX._accX + accY._accY + accZ.*accZ);

% HP filter accelerometer data filtCutOff = 0.001; [b, a] = butter(1, (2*filtCutOff)/(1/samplePeriod), 'high'); acc_magFilt = filtfilt(b, a, acc_mag);

% Compute absolute value acc_magFilt = abs(acc_magFilt);

% LP filter accelerometer data filtCutOff = 5; [b, a] = butter(1, (2*filtCutOff)/(1/samplePeriod), 'low'); acc_magFilt = filtfilt(b, a, acc_magFilt);

% Threshold detection stationary = acc_magFilt < 0.05;

% ------------------------------------------------------------------------- % Plot data raw sensor data and stationary periods

%figure('Position', [9 39 900 600], 'Number', 'off', 'Name', 'Sensor Data'); figure

% this section just skecth raw data, ax(1) = subplot(2,1,1); hold on; plot(time, gyrX, 'r'); plot(time, gyrY, 'g'); plot(time, gyrZ, 'b'); title('Gyroscope'); xlabel('Time (s)'); ylabel('Angular velocity (^\circ/s)'); legend('X', 'Y', 'Z'); hold off; ax(2) = subplot(2,1,2); hold on; plot(time, accX, 'r'); plot(time, accY, 'g'); plot(time, accZ, 'b'); plot(time, acc_magFilt, ':k'); plot(time, stationary, 'k', 'LineWidth', 2); title('Accelerometer'); xlabel('Time (s)'); ylabel('Acceleration (g)'); legend('X', 'Y', 'Z', 'Filtered', 'Stationary'); hold off; linkaxes(ax,'x');

% ------------------------------------------------------------------------- % Compute orientation %An attitude and heading reference system (AHRS) consists of %sensors on three axes that provide attitude information for aircraft, %including heading, pitch and yaw. quat = zeros(length(time), 4); AHRSalgorithm = AHRS('SamplePeriod', 1/256, 'Kp', 1, 'KpInit', 1);

% Initial convergence initPeriod = 2; indexSel = 1 : find(sign(time-(time(1)+initPeriod))+1, 1); for i = 1:1000 AHRSalgorithm.UpdateIMU([0 0 0], [mean(accX(indexSel)) mean(accY(indexSel)) mean(accZ(indexSel))]); end

% For all data for t = 1:length(time) if(stationary(t)) AHRSalgorithm.Kp = 0.5; else AHRSalgorithm.Kp = 0; end AHRSalgorithm.UpdateIMU(deg2rad([gyrX(t) gyrY(t) gyrZ(t)]), [accX(t) accY(t) accZ(t)]); quat(t,:) = AHRSalgorithm.Quaternion; end

% ------------------------------------------------------------------------- % Compute translational accelerations

% Rotate body accelerations to Earth frame acc = quaternRotate([accX accY accZ], quaternConj(quat));

% % Remove gravity from measurements % acc = acc - [zeros(length(time), 2) ones(length(time), 1)]; % unnecessary due to velocity integral drift compensation

% Convert acceleration measurements to m/s/s acc = acc * 9.81;

% Plot translational accelerations %figure('Position', [9 39 900 300], 'Number', 'off', 'Name', 'Accelerations'); hold on; plot(time, acc(:,1), 'r'); plot(time, acc(:,2), 'g'); plot(time, acc(:,3), 'b'); title('Acceleration'); xlabel('Time (s)'); ylabel('Acceleration (m/s/s)'); legend('X', 'Y', 'Z'); hold off;

% ------------------------------------------------------------------------- % Compute translational velocities

acc(:,3) = acc(:,3) - 9.81;

% Integrate acceleration to yield velocity vel = zeros(size(acc)); for t = 2:length(vel) vel(t,:) = vel(t-1,:) + acc(t,:) * samplePeriod; if(stationary(t) == 1) vel(t,:) = [0 0 0]; % force zero velocity when foot stationary end end

% Compute integral drift during non-stationary periods velDrift = zeros(size(vel)); stationaryStart = find([0; diff(stationary)] == -1); stationaryEnd = find([0; diff(stationary)] == 1); for i = 1:numel(stationaryEnd) driftRate = vel(stationaryEnd(i)-1, :) / (stationaryEnd(i) - stationaryStart(i)); enum = 1:(stationaryEnd(i) - stationaryStart(i)); drift = [enum'_driftRate(1) enum'_driftRate(2) enum'*driftRate(3)]; velDrift(stationaryStart(i):stationaryEnd(i)-1, :) = drift; end

% Remove integral drift vel = vel - velDrift;

% Plot translational velocity %figure('Position', [9 39 900 300], 'Number', 'off', 'Name', 'Velocity'); figure hold on; plot(time, vel(:,1), 'r'); plot(time, vel(:,2), 'g'); plot(time, vel(:,3), 'b'); title('Velocity'); xlabel('Time (s)'); ylabel('Velocity (m/s)'); legend('X', 'Y', 'Z'); hold off;

% ------------------------------------------------------------------------- % Compute translational position

% Integrate velocity to yield position pos = zeros(size(vel)); for t = 2:length(pos) pos(t,:) = pos(t-1,:) + vel(t,:) * samplePeriod; % integrate velocity to yield position end

% Plot translational position %figure('Position', [9 39 900 600], 'Number', 'off', 'Name', 'Position'); figure hold on; plot(time, pos(:,1), 'r'); plot(time, pos(:,2), 'g'); plot(time, pos(:,3), 'b'); title('Position'); xlabel('Time (s)'); ylabel('Position (m)'); legend('X', 'Y', 'Z'); hold off;

% ------------------------------------------------------------------------- % Plot 3D foot trajectory

% % Remove stationary periods from data to plot % posPlot = pos(find(~stationary), :); % quatPlot = quat(find(~stationary), :); posPlot = pos; quatPlot = quat;

% Extend final sample to delay end of animation extraTime = 20; onesVector = ones(extraTime_(1/samplePeriod), 1); posPlot = [posPlot; [posPlot(end, 1)_onesVector, posPlot(end, 2)_onesVector, posPlot(end, 3)_onesVector]]; quatPlot = [quatPlot; [quatPlot(end, 1)_onesVector, quatPlot(end, 2)_onesVector, quatPlot(end, 3)_onesVector, quatPlot(end, 4)_onesVector]];

% Create 6 DOF animation SamplePlotFreq = 4; Spin = 120; SixDofAnimation(posPlot, quatern2rotMat(quatPlot), ... 'SamplePlotFreq', SamplePlotFreq, 'Trail', 'All', ... 'Position', [9 39 1280 768], 'View', [(100:(Spin/(length(posPlot)-1)):(100+Spin))', 10*ones(length(posPlot), 1)], ... 'AxisLength', 0.1, 'ShowArrowHead', false, ... 'Xlabel', 'X (m)', 'Ylabel', 'Y (m)', 'Zlabel', 'Z (m)', 'ShowLegend', false, ... 'CreateAVI', false, 'AVIfileNameEnum', false, 'AVIfps', ((1/samplePeriod) / SamplePlotFreq));

michaelcheng1991 commented 8 years ago

%MatLab KVest result code

%%this script attempt to integrate Gait tracking and arduino data

%1. inport Data from IMU (5000 sets of data) %2. use script.m

addpath('Quaternions'); addpath('ximu_matlab_library');

delete (instrfindall); s = serial ('/dev/tty.usbmodem1411'); set(s,'BaudRate', 115200); fopen(s); s.ReadAsyncMode='continuous';

s.BytesAvailable

%Start asynchronous reading readasync(s); while(s.BytesAvailable <=0) % sometimes program terminates here

end

s.BytesAvailable

numofData = 100; mData = zeros(numofData,27);

tic; for i = 1: numofData sSerialData=fscanf(s);
t=strsplit(sSerialData,'\t'); for j= 1:27 mData(i,j) = str2double(t(j)); end end toc;

stopasync(s); fclose(s);

%export data to excel filename = 'swingdatanum1.csv'; csvwrite(filename,mData);

% Import data samplePeriod = 1/256; %original is 1/256 %xIMUdata = xIMUdataClass(filePath, 'InertialMagneticSampleRate', 1/samplePeriod); time = 1:numofData; gyrX = mData(:,1); gyrY = mData(:,2); gyrZ = mData(:,3); accX = mData(:,4); accY = mData(:,5); accZ = mData(:,6); gyrX_1 = mData(:,10); gyrY_1 = mData(:,11); gyrZ_1 = mData(:,12); accX_1 = mData(:,13); accY_1 = mData(:,14); accZ_1 = mData(:,15); gyrX_2 = mData(:,19); gyrY_2 = mData(:,20); gyrZ_2 = mData(:,21); accX_2 = mData(:,22); accY_2 = mData(:,23); accZ_2 = mData(:,24); %%clear('xIMUdata');

figure hold on plot(time,gyrX); plot(time,gyrX_1); plot(time,gyrX_2); plot(time,gyrY); plot(time,gyrY_1); plot(time,gyrY_2); plot(time,gyrZ); plot(time,gyrZ_1); plot(time,gyrZ_2); hold off