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();
}