Ejemplo n.º 1
0
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();
}
Ejemplo n.º 2
0
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;
}
Ejemplo n.º 3
0
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();
}