Code

Flow Chart

Part 1. Angle Calculations

//Calculating angles from master mpu 6050 read_mpu_6050_data(); //Subtract the offset values from the raw gyro values gyro_x -= gyro_x_cal; gyro_y -= gyro_y_cal; gyro_z -= gyro_z_cal; //Gyro angle calculations . Note 0.0003817 = 1 / (40Hz x 65.5) angle_pitch += gyro_x * 0.0003817; //Calculate the traveled pitch angle and add this to the angle_pitch variable angle_roll += gyro_y * 0.0003817; //Calculate the traveled roll angle and add this to the angle_roll variable //0.000006662 = 0.0003817 * (3.142(PI) / 180degr) The Arduino sin function is in radians angle_pitch += angle_roll * sin(gyro_z * 0.000006662); //If the IMU has yawed transfer the roll angle to the pitch angel angle_roll -= angle_pitch * sin(gyro_z * 0.000006662); //If the IMU has yawed transfer the pitch angle to the roll angel //Accelerometer angle calculations acc_total_vector = sqrt((acc_x*acc_x)+(acc_y*acc_y)+(acc_z*acc_z)); //Calculate the total accelerometer vector //57.296 = 1 / (3.142 / 180) The Arduino asin function is in radians angle_pitch_acc = asin((float)acc_y/acc_total_vector)* 57.296; //Calculate the pitch angle angle_roll_acc = asin((float)acc_x/acc_total_vector)* -57.296; //Calculate the roll angle angle_pitch_acc -= 0.0; //Accelerometer calibration value for pitch angle_roll_acc -= 0.0; //Accelerometer calibration value for roll

Part 2. Filtering

if(set_gyro_angles){ //If the IMU is already started angle_pitch = angle_pitch * 0.9996 + angle_pitch_acc * 0.0004; //Correct the drift of the gyro pitch angle with the accelerometer pitch angle angle_roll = angle_roll * 0.9996 + angle_roll_acc * 0.0004; //Correct the drift of the gyro roll angle with the accelerometer roll angle } else{ //At first start angle_pitch = angle_pitch_acc; //Set the gyro pitch angle equal to the accelerometer pitch angle angle_roll = angle_roll_acc; //Set the gyro roll angle equal to the accelerometer roll angle set_gyro_angles = true; //Set the IMU started flag } //Dampens the output angle_pitch_output = angle_pitch_output * 0.9 + angle_pitch * 0.1; //Take 90% of the output pitch value and add 10% of the raw pitch value angle_roll_output = angle_roll_output * 0.9 + angle_roll * 0.1; //Take 90% of the output roll value and add 10% of the raw roll value

The objective with the sensor is to calculate the Euler angles with the gyroscope and accelerometer. The angle can be easily found by integrating the gyroscope values, however, this causes gyroscope drift over time and will lead to incorrect angle readings. To mitigate this, we also use the accelerometer readings to correct the angle drift. Typically, the three Euler angles can be calculated with the fused data; the gyroscope gives angle position data in all three axes, the accelerometer gives angle position data in pitch and roll, and the magnetometer gives angle position data in yaw. Since we only have an accelerometer and gyroscope, we can only account for the angle drift in the pitch and roll angles.

Part 3. Bluetooth

Relevant Variables

#define SOP '<'#define EOP '>'bool started = false;bool ended = false;char inData[80];byte index;char delimiters[] = ",";char* valPosition;float angle[] = {0.00, 0.00};

// Read all serial data available, as fast as possible while(Serial.available() > 0) //If there is something in the serial monitor, start while loop { char inChar = Serial.read(); if(inChar == SOP) //If byte from serial monitor(SM) is Start of Packet (SOP), aka <, set index to 0 and start the loop { index = 0; inData[index] = '\0'; started = true; ended = false; } else if(inChar == EOP) //If byte from SM is EOP, aka >, end the loop { ended = true; break; } else //If byte from SM is neither SOP or EOP, input incoming characters into inData character array { if(index < 79) { inData[index] = inChar; index++; inData[index] = '\0'; } } }
//Tokenize the inData character array to split into different angle readings if(started && ended) { valPosition = strtok(inData, delimiters); for(int i = 0; i < 2; i++){ angle[i] = atof(valPosition); valPosition = strtok(NULL, delimiters); }
// Reset for the next packet started = false; ended = false; index = 0; inData[index] = '\0'; }
//Pull values from strings array angle_f2 = angle[0]; angle_r2 = angle[1];

The Bluetooth communication between the two devices is at 115200 baud rate. The Serial.read function used to read the code can only read 1 byte at a time from the serial monitor. Since the readings we are sending are several digits long, we have to store the values in a character array using a while loop. Once the character array is determined for one loop, the array is tokenized and the values are split based on a delimiter. The split values can then be assigned to variables and converted to a float variable using the "atof" function. The code can be found in the Design Output section.

Part 4. Alert

//Calculating the angle differences flex1 = angle_pitch_output - angle_f2 +1; rot1 = angle_roll_output - angle_r2 + 12;
flex = flex1 * 90 / 75; int y = 100 - flex; int x = -y; float rot = map(rot1, x, y, -90, 90); //Remap roll values based on pitch angles
Serial.print(flex); Serial.print(","); Serial.println(rot);
//Vibration alert if(flex < -15 || flex >= -15 && flex < 0 && rot <-15 || flex >= 0 && flex < 15 && rot <-30 || flex >= 15 && flex < 30 && rot <-60 || flex >= 45 && flex < 60 && rot > 60 || flex >= 60 && flex < 75 && rot > 30 || flex >= 75 && flex < 80 && rot > 15 || flex >= 80) { tone(8,750,5); Serial.end(); Serial.begin(115200); } else { Serial.end(); Serial.begin(115200); } delay(25);

This table shows the risky angles associated with a total hip replacement. The values were given to us by our clinical advisor Dr. Schmidt and are based on literary research and personal experience.