148 lines
3.6 KiB
C++
148 lines
3.6 KiB
C++
void plot() {
|
|
// Time
|
|
// Serial.print("dT:");
|
|
// Serial.println(dT);
|
|
// Serial.print(" ");
|
|
// Serial.print("dT_s:");
|
|
// Serial.println(dT_s);
|
|
// Serial.print(" ");
|
|
|
|
// IMU
|
|
|
|
// Serial.print("RollRate:");
|
|
// Serial.println(pitch_rate);
|
|
|
|
// Serial.print("Accelerometer_Pitch:");
|
|
// Serial.println(acc_pitch);
|
|
|
|
// Serial.print("Pitch:");
|
|
// Serial.println(pitch);
|
|
// Serial.print ( "," );
|
|
// Serial.println ( gt );
|
|
// Serial.print ( " " );
|
|
// Serial.println ( acc_pitch);
|
|
|
|
|
|
// Remote control
|
|
// Serial.print("ch1:");
|
|
// Serial.print(pwm_time_ch1);
|
|
// Serial.print(" ");
|
|
// Serial.print("ch2:");
|
|
// Serial.print(pwm_time_ch2);
|
|
// Serial.print("ch1mapped:");
|
|
// Serial.print(rem_turn_speed_ref);
|
|
// Serial.print(" ");
|
|
// Serial.print("ch2mapped:");
|
|
// Serial.println(rem_speed_ref);
|
|
|
|
|
|
// Encoders
|
|
// Serial.print("m1Raw:");
|
|
// Serial.println(m1Raw);
|
|
|
|
// Serial.print("m2Raw:");
|
|
// Serial.println(m2Raw);
|
|
|
|
// // Motors
|
|
// Serial.print("SpeedControllerOut:");
|
|
// Serial.println(SC_cont_out);
|
|
|
|
// Serial.print("BalanceOLControllerOut:");
|
|
// Serial.println(OL_cont_out);
|
|
|
|
// Serial.print("BalanceILControllerOut:");
|
|
// Serial.println(IL_cont_out);
|
|
|
|
// Serial.print("TurnControllerOut:");
|
|
// Serial.println(TC_cont_out);
|
|
|
|
// Serial.print("M1_CMD:");
|
|
// Serial.println(M1_Speed_CMD);
|
|
|
|
// Serial.print("M2_CMD:");
|
|
// Serial.println(M2_Speed_CMD);
|
|
|
|
// Serial.print("M1_Ang_Vel:");
|
|
// Serial.print(motor_ang_vel[0][0]);
|
|
// Serial.print(" ");
|
|
// Serial.print("M2_Ang_Vel:");
|
|
// Serial.print(motor_ang_vel[0][1]);
|
|
// Serial.print(" ");
|
|
// Serial.print("botLinVel:");
|
|
// Serial.println(vel_Matrix[0][0]);
|
|
// Serial.print(" ");
|
|
// Serial.print("botAngVel:");
|
|
// Serial.println(vel_Matrix[1][0]);
|
|
|
|
// //PS3 Controller
|
|
// if (Ps3.isConnected()) {
|
|
// Serial.print("lx:");
|
|
// Serial.print(Ps3.data.analog.stick.lx);
|
|
// Serial.print(",");
|
|
// Serial.print("ly:");
|
|
// Serial.print(Ps3.data.analog.stick.ly);
|
|
// Serial.print(",");
|
|
// Serial.print("rx:");
|
|
// Serial.print(Ps3.data.analog.stick.rx);
|
|
// Serial.print(",");
|
|
// Serial.print("ry:");
|
|
// Serial.println(Ps3.data.analog.stick.ry);
|
|
// }
|
|
|
|
int i = 0;
|
|
data[i] = watchdog++;
|
|
data[i += 1] = balancingOn << 1;
|
|
i = PackInt(i += 1, M1_Speed_CMD);
|
|
i = PackInt(i, M2_Speed_CMD);
|
|
i = PackFloat(i, acc_pitch);
|
|
i = PackFloat(i, pitch);
|
|
i = PackFloat(i, pitch_rate);
|
|
i = PackFloat(i, rem_speed_ref);
|
|
i = PackFloat(i, rem_turn_speed_ref);
|
|
i = PackFloat(i, SC_cont_out);
|
|
i = PackFloat(i, TC_cont_out);
|
|
i = PackFloat(i, OL_cont_out);
|
|
i = PackFloat(i, ref_IL);
|
|
i = PackFloat(i, act_IL);
|
|
i = PackFloat(i, error_IL);
|
|
i = PackFloat(i, IL_cont_out);
|
|
i = PackFloat(i, iError_IL);
|
|
i = PackFloat(i, IL_anti_windup);
|
|
i = PackFloat(i, speedCmd1);
|
|
i = PackFloat(i, speedCmd2);
|
|
i = PackFloat(i, vel_Matrix[0][0]);
|
|
i = PackFloat(i, vel_Matrix[1][0]);
|
|
i = PackFloat(i, motor_ang_vel[0][0]);
|
|
i = PackFloat(i, motor_ang_vel[1][0]);
|
|
i = PackLong(i, m1Raw);
|
|
i = PackLong(i, m2Raw);
|
|
}
|
|
|
|
int PackInt(int _i, int value) {
|
|
data[_i] = (value & 0x00FF) ;
|
|
data[_i + 1] = (value & 0xFF00)>>8;
|
|
return _i + 2;
|
|
}
|
|
|
|
union FloatToBytes {
|
|
float value;
|
|
byte bytes[4];
|
|
};
|
|
|
|
int PackFloat(int _i, float value) {
|
|
FloatToBytes converter;
|
|
converter.value = value;
|
|
for(int j = 0; j < 4; j++) {
|
|
data[_i + j] = converter.bytes[j];
|
|
}
|
|
return _i + 4;
|
|
}
|
|
|
|
// int PackFloat(int _i, float value) {
|
|
// data[_i] = (value & 0x000000FF) ;
|
|
// data[_i + 2] = (value & 0x0000FF00)>>8;
|
|
// data[_i + 3] = (value & 0x00FF0000)>>16;
|
|
// data[_i + 4] = (value & 0xFF000000)>>24;
|
|
// return _i + 4;
|
|
// }
|