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 cb_imu(){
msg_timestamp = nh.now();
orientation = bno.getQuat();
orientation_msg.header.seq = msg_seq;
orientation_msg.header.stamp = msg_timestamp;
orientation_msg.quaternion.x = orientation.x();
orientation_msg.quaternion.y = orientation.y();
orientation_msg.quaternion.z = orientation.z();
orientation_msg.quaternion.w = orientation.w();
pub_orientation.publish(&orientation_msg);
angular_vel = bno.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);
angular_vel_msg.header.seq = msg_seq;
angular_vel_msg.header.stamp = msg_timestamp;
angular_vel_msg.vector.x = angular_vel.x();
angular_vel_msg.vector.y = angular_vel.y();
angular_vel_msg.vector.z = angular_vel.z();
pub_angular_vel.publish(&angular_vel_msg);
linear_accel = bno.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL);
linear_accel_msg.header.seq = msg_seq;
linear_accel_msg.header.stamp = msg_timestamp;
linear_accel_msg.vector.x = linear_accel.x();
linear_accel_msg.vector.y = linear_accel.y();
linear_accel_msg.vector.z = linear_accel.z();
pub_linear_accel.publish(&linear_accel_msg);
msg_seq++;
}
void init_imu(){
if(!Wire.isEnabled()) Wire.begin();
if(!bno.begin()){
Serial.println("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
while(1);
}
delay(1000);
bno.setExtCrystalUse(true);
orientation_msg.header.frame_id = msg_frame_id;
angular_vel_msg.header.frame_id = msg_frame_id;
linear_accel_msg.header.frame_id = msg_frame_id;
nh.advertise(pub_orientation);
nh.advertise(pub_angular_vel);
nh.advertise(pub_linear_accel);
timer_imu.start();
}
void setup_bno055_2()
{
Particle.publish("imu2", "Starting BNO055 setup");
/* Initialise the sensor */
if(!bno2.begin())
{
Particle.publish("imu2", "could not find BNO055 on I2C bus");
} else {
delay(1000);
Particle.publish("imu2", "BNO055 setup OK");
bno2.setExtCrystalUse(false);
// if setup is successful, start Timer
imu_timer_2.start();
}
s_setup_bno055_2 = false;
}
void setup()
{
Serial.begin(230400);
delay(3000); //3 seconds delay for enabling to see the start up comments on the serial board
#ifdef SERIAL_TRACE
Serial.println("Hello!");
#endif
// Show we are in setup
pinMode(LED_PIN, OUTPUT);
digitalWrite(LED_PIN, HIGH);
// Port setup
pinMode(RELAY_PIN, OUTPUT);
digitalWrite(RELAY_PIN, HIGH);
// Servo & Lidar
g_servo.setup();
g_lidar.begin();
// TODO: Does this increase speed of LIDAR readings
// g_lidar.beginContinuous();
// Motor setup
for(unsigned int i=0; i<countof(g_amotors); ++i) {
g_amotors[i].setup();
attachInterrupt(g_amotors[i].ENCODER_IRQ, c_afnInterrupts[i], CHANGE);
}
// Initialise the IMU (connection is optional)
g_bBNO = g_bno.begin();
delay(1000);
if(g_bBNO) g_bno.setExtCrystalUse(true);
OnDisconnection();
}
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);
}