The second part:
float Diff_Cal (float X, float Y, float Z, float X_prev, float Y_prev, float Z_prev)
{
float Delta;
Delta = sqrt(pow(X-X_prev, 2) + pow(Y-Y_prev, 2) + pow(Z-Z_prev,2));
return Delta;
}
//Calculating Motion Smoothness
float Smooth (float A0, float A1, float dt)
{
float S;
S = (A1-A0)/dt;
return S;
}
void loop()
{
//Checking Start Signal
if (PC_msg = 0)
{
PC_msg = Serial.read();
}
else if (PC_msg == 1)
{
t_start = millis();
PC_msg = -1;
}
else if (PC_msg == 2)
{
PC_msg = 0;
}
//Setting V0 to the previous value
V0_A = V1_A;
V0_B = V1_B;
//Setting A0 to the previous value
Acc0_A = Acc1_A;
Acc0_B = Acc1_B;
//Calculating Radians and Depths from electronic degrees
Ang_AT = EdegToRad(EAng_AT);
Ang_AS = EdegToRad(EAng_AS);
Depth_AZ = EdegToDepth (EAng_AZ);
Ang_BT = EdegToRad(EAng_BT);
Ang_BS = EdegToRad(EAng_BS);
Depth_BZ = EdegToDepth (EAng_BZ);
//Computing coordinates
//Saving previous coordinates for path length calculation
X_A_Coord_prev = Coord_A[0];
Y_A_Coord_prev = Coord_A[1];
Z_A_Coord_prev = Coord_A[2];
X_B_Coord_prev = Coord_B[0];
Y_B_Coord_prev = Coord_B[1];
Z_B_Coord_prev = Coord_B[2];
//Port A calculations
//Coordinates
PortCoord (Coord_A, Ang_AT, Ang_AS, Depth_AZ);
//Path
Delta_A = Diff_Cal(Coord_A[0], Coord_A[1], Coord_A[2], X_A_Coord_prev, Y_A_Coord_prev, Z_A_Coord_prev);
Path_A = Path_A + Delta_A;
//Port B calculations
//Coordinates
PortCoord (Coord_B, Ang_BT, Ang_BS, Depth_BZ);
//Path
Delta_B = Diff_Cal(Coord_B[0], Coord_B[1], Coord_B[2], X_B_Coord_prev, Y_B_Coord_prev, Z_B_Coord_prev);
Path_B = Path_B + Delta_B;
//Calculating velocity, acceleration and smoothness
//Setting t1---------------
//Set t1
t1 = millis();
//Calculating elapsed time between two samples in seconds
dt = abs(t1-t0);
//-------------------------
//Port A
//Velocity
V1_A = Delta_A / dt;
//Average velocity
VSum_A = VSum_A + V1_A;
VAvg_A = VSum_A / datacounter;
//Max Velocity
if (V1_A > VMax_A) {VMax_A = V1_A;}
//Acceleration
Acc1_A = (2 * Delta_A)/ pow(dt, 2);
//Average acceleration
AccSum_A = AccSum_A + Acc1_A;
AccAvg_A = AccSum_A / datacounter;
//Max Acceleration
if (Acc1_A > AccMax_A) {AccMax_A = Acc1_A;}
//Smoothness
S_A = Smooth(Acc0_A, Acc1_A, dt);
//Economy of Motion
Eco_A = Path_A / Time;
//Port_B
V1_B = Delta_B / dt;;
//Acceleration
Acc1_B = (2 * Delta_B)/ pow(dt, 2);
//Smoothness
S_B = Smooth(Acc0_B, Acc1_B, dt);
//Average velocity
VSum_B = VSum_B + V1_B;
VAvg_B = VSum_B / datacounter;
//Max Velocity
if (V1_B > VMax_B) {VMax_B = V1_B;}
//Average acceleration
AccSum_B = AccSum_B + Acc1_B;
AccAvg_B = AccSum_B / datacounter;
//Max Acceleration
if (Acc1_B > AccMax_B) {AccMax_B = Acc1_B;}
//Economy of Motion
Eco_B = Path_B / Time;
//Writing coordinates to serial port Format: Time; X_A; Y_A; Z_A; Path_A; Vel_A; VAvg_A; VMax_A; Acc_A; AccAvg_A; AccMax_A; Smooth_A; Eco_A; X_B; Y_B; Z_B; Path_B; Vel_B; VAvg_B; VMax_B; Acc_B; AccAvg_B; AccMax_B; Smooth_B; Eco_B
//Display angles (for debug)
/*
Serial.print("AT: ");
Serial.print(EAng_AT, 2);
Serial.print(" => ");
Serial.print(Ang_AT, 2);
Serial.print(", AS: ");
Serial.print(EAng_AS, 2);
Serial.print(" => ");
Serial.print(Ang_AS, 2);
Serial.print(", AZ: ");
Serial.print(EAng_AZ, 2);
Serial.print(" => ");
Serial.print(Depth_AZ, 2);
Serial.print(", BT: ");
Serial.print(EAng_BT, 2);
Serial.print(" => ");
Serial.print(Ang_BT, 2);
Serial.print(", BS: ");
Serial.print(EAng_BS, 2);
Serial.print(" => ");
Serial.print(Ang_BS, 2);
Serial.print(", BZ: ");
Serial.print(EAng_BZ, 2);
Serial.print(" => ");
Serial.println(Depth_BZ, 2);
//=========================
*/
Serial.print(Time, 2);
Serial.print(", ");
Serial.print(Coord_A[0], 2);
Serial.print(", ");
Serial.print(Coord_A[1], 2);
Serial.print(", ");
Serial.print(Coord_A[2], 2);
Serial.print(", ");
Serial.print(Path_A, 2);
Serial.print(", ");
Serial.print(V1_A, 2);
Serial.print(", ");
Serial.print(VAvg_A, 2);
Serial.print(", ");
Serial.print(VMax_A, 2);
Serial.print(", ");
Serial.print(Acc1_A, 2);
Serial.print(", ");
Serial.print(AccAvg_A, 2);
Serial.print(", ");
Serial.print(AccMax_A, 2);
Serial.print(", ");
Serial.print(S_A, 2);
Serial.print(", ");
Serial.print(Eco_A, 2);
Serial.print(", ");
Serial.print(Coord_B[0], 2);
Serial.print(", ");
Serial.print(Coord_B[1], 2);
Serial.print(", ");
Serial.print(Coord_B[2], 2);
Serial.print(", ");
Serial.print(Path_B, 2);
Serial.print(", ");
Serial.print(V1_B, 2);
Serial.print(", ");
Serial.print(VAvg_B, 2);
Serial.print(", ");
Serial.print(VMax_B, 2);
Serial.print(", ");
Serial.print(Acc1_B, 2);
Serial.print(", ");
Serial.print(AccAvg_B, 2);
Serial.print(", ");
Serial.print(AccMax_B, 2);
Serial.print(", ");
Serial.print(S_B, 2);
Serial.print(", ");
Serial.println(Eco_B, 2);
//Setting t1
t0 = millis();
Time = (t0 - t_start) / 1000.00;
datacounter = datacounter + 1.00;
}