void SendSensorData() {
// TODO: Transmit current or make emergency stop if motor current too high
// TODO: Limit number of transmissions, once every 50 ms?
#ifdef SERIAL_TRACE
Serial.println(g_nYaw);
Serial.print('\t');
Serial.print(g_servo.Angle());
Serial.print("\t");
Serial.println(g_lidar.distance());
#else
SSensorData data = {
UINT16_MAX,
0, 0, 0, 0,
g_servo.Angle(),
g_lidar.distance(), // in cm,
g_amotors[0].Pop(),
g_amotors[1].Pop(),
g_amotors[2].Pop(),
g_amotors[3].Pop()
};
if(g_bBNO) {
sensors_event_t event;
g_bno.getEvent(&event);
data.m_nYaw = static_cast<unsigned short>(constrain(round(event.orientation.x * 100), 0, 36000));
g_bno.getCalibration(&data.m_nCalibSystem, &data.m_nCalibGyro, &data.m_nCalibAccel, &data.m_nCalibMag);
}
Serial.write((byte*)&data, sizeof(data));
#endif
}
void loop() {
if (s_report_button1_state)
{
bool button1_state = digitalRead(BUTTON1_PIN);
Particle.publish("button1", String(button1_state));
s_report_button1_state = false;
}
if (s_report_button2_state)
{
bool button2_state = digitalRead(BUTTON2_PIN);
Particle.publish("button2", String(button2_state));
s_report_button2_state = false;
}
if(s_start_battery)
{
setup_battery();
s_start_battery = false;
}
if (s_report_battery_state)
{
double voltage = lipo.getVoltage();
double soc = lipo.getSOC();
Particle.publish("battery", "volt: " + String(voltage) + " pct: " + String(soc));
s_report_battery_state = false;
}
// bno1
// =====
if(s_setup_bno055_1)
{
setup_bno055_1();
}
if(s_report_imu_state_1)
{
uint8_t system, gyro, accel, mag = 0;
bno1.getCalibration(&system, &gyro, &accel, &mag);
imu::Vector<3> euler = bno1.getVector(Adafruit_BNO055::VECTOR_EULER);
String status = String::format("Orient: x=%.0f y=%.0f z=%.0f Cal s:%d g:%d a:%d m:%d",
euler.x(),
euler.y(),
euler.z(),
system,
gyro,
accel,
mag
);
Particle.publish("imu1", status);
s_report_imu_state_1 = false;
}
// bno2
// =====
if(s_setup_bno055_2)
{
setup_bno055_2();
}
if(s_report_imu_state_2)
{
uint8_t system, gyro, accel, mag = 0;
bno2.getCalibration(&system, &gyro, &accel, &mag);
imu::Vector<3> euler = bno2.getVector(Adafruit_BNO055::VECTOR_EULER);
String status = String::format("Orient: x=%.0f y=%.0f z=%.0f Cal s:%d g:%d a:%d m:%d",
euler.x(),
euler.y(),
euler.z(),
system,
gyro,
accel,
mag
);
Particle.publish("imu2", status);
s_report_imu_state_2 = false;
}
delay(250);
}