MoneyMan9204
New member
I'm using the Teensy 4.1 and trying to hook up and read from 2 different IMUs (Adafruit BNO055 and MPU6050). I have them hoked up to Wire (MPU6050) and Wire2 (BNO055). I was testing just using Wire2 with the BNO055, but couldn't get it to read anything from it. I would like to know if what I'm trying to do is possible and if so, what code/libraries need to be used to do so. Below is the code I'm running trying to get just the BNO055 to work on Wire2. There's a lot of extra definitions that can be ignored as they're for other parts of my project.
Sorry there's a bunch of unrelated variables and stuff. I got the code from someone else and don't want to delete things for other parts of the code.
I was also wondering if I should connect both IMUs to the same I2C, it's been a while since I learned I2C.
Thank you
#include <Servo.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <Wire.h>
#include <SPI.h>
#include <ubx.h>
#include <SD.h>
#include "mission_parameters.h"
#include "hardware_parameters.h"
#include "definitions.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
//--- Create Hardware Instances ---//
bfs::Ubx gnss(&Serial2); // Teensy 4.1
Servo pitch_servo;
Servo roll_servo;
Adafruit_BNO055 bno = Adafruit_BNO055(55);
//^^^ Create Hardware Instances ^^^//
//--- Initialize FSM States ---//
typedef enum {
STOW = 0,
WORKING = 1,
} STATE;
STATE current_state = STOW;
STATE next_state = STOW;
//^^^ Initialize FSM States ^^^//
//--- Initialize FSM variables ---//
bool relay_flag = false; // Flag to enable WORKING State
bool relay_signal = false; // Relay digital read signal
int relay_counter = 0;
int gnss_fix = 0;
int gnss_fix_signal = 0;
int gnss_fix_prev = 0;
int gnss_fail_counter = 0;
int gnss_threshold = 5;
bool stow_flag = 1;
bool work_flag = 0;
unsigned long last_sd_write_time;
//^^^ Initialize FSM variables ^^^//
//--- GNSS Data Structures ---//
PositionGeo current_pos;
struct PositionGeo cmp[2];
float current_distance_from_line;
float current_bearing_from_origin;
float line_bearing;
float line_length;
//^^^ GNSS Data Structures ^^^//
//--- Gimbal Measurements ---//
float current_roll_angle;
float current_pitch_angle;
float target_roll_angle;
float target_pitch_angle;
//^^^ Gimbal Measurements ^^^//
//--- Adam's Control Loop ---//
float roll_input;
float pitch_input;
float roll_cmd;
float pitch_cmd;
float roll_angle;
float pitch_angle;
float roll_err;
float pitch_err;
float sgn_roll_err;
float sgn_pitch_err;
float dummy;
float roll_write_in;
float pitch_write_in;
//^^^ Adam's Control Loop ^^^//
float prev_fix_timer = millis();
void setup() {
Serial.begin(115200); // Establish Serial connection
roll_servo.attach(roll_servo_pin); // Attach roll servo and center
pitch_servo.attach(pitch_servo_pin); // Attach pitch servo and center
delay(3000);
//--- Initialize IMU device ---//
Wire2.begin();
Wire2.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
delay(2000);
if(!bno.begin())
{
/* There was a problem detecting the BNO055 ... check your connections */
Serial.println("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
while(1);
}
delay(1000);
bno.setExtCrystalUse(true);
// load and configure the DMP
//--- Stow Gimbals ---//
roll_servo.write(servo_center_angle);
delay(1000);
pitch_servo.write(stow_angle);
//^^^ Stow Gimbals ^^^//
}
void loop() {
imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER);
current_pitch_angle = euler.z();
current_roll_angle = euler.y();
// target_roll_angle = r2d * atan2(current_distance_from_line, closest_rel_height);
// target_roll_angle = max(min_roll_angle, min(target_roll_angle, max_roll_angle));
target_roll_angle = 20;
target_pitch_angle = 0;
Serial.print("Pitch Reading:\t");
Serial.println(current_pitch_angle);
Serial.print("Roll Reading:\t");
Serial.println(current_roll_angle);
// Serial.print("Target Roll:\t");
// Serial.println(target_roll_angle);
// Serial.print("Target Pitch:\t");
// Serial.println(target_pitch_angle);
roll_err = current_roll_angle - target_roll_angle;
pitch_err = current_pitch_angle - target_pitch_angle;
roll_input = current_roll_angle - roll_err;
pitch_input = current_pitch_angle - pitch_err;
roll_servo.write(90 + roll_err);
pitch_servo.write(90 - pitch_err);
// sgn_pitch_err = pitch_err / fabs(pitch_err);
// delay(2000);
}
Sorry there's a bunch of unrelated variables and stuff. I got the code from someone else and don't want to delete things for other parts of the code.
I was also wondering if I should connect both IMUs to the same I2C, it's been a while since I learned I2C.
Thank you