#include "application.h" #include "SparkFunMAX17043.h" // Include the SparkFun MAX17043 library #include "Adafruit_Sensor.h" #include "Adafruit_BNO055.h" #include "imumaths.h" PRODUCT_ID(1523); PRODUCT_VERSION(2); /* Set the delay between fresh samples */ #define BNO055_SAMPLERATE_DELAY_MS (100) Adafruit_BNO055 bno1 = Adafruit_BNO055(-1, BNO055_ADDRESS_A); Adafruit_BNO055 bno2 = Adafruit_BNO055(-1, BNO055_ADDRESS_B); #define BUZZER_PIN A4 #define BUTTON1_PIN D2 #define BUTTON2_PIN D3 bool s_report_button1_state = false; bool s_report_button2_state = false; bool s_start_battery = false; bool s_report_battery_state = false; bool s_setup_bno055_1 = false; bool s_report_imu_state_1 = false; bool s_setup_bno055_2 = false; bool s_report_imu_state_2 = false;
void OnMotor1Interrupt() { g_amotors[1].onInterrupt(); }
void OnMotor2Interrupt() { g_amotors[2].onInterrupt(); }
void OnMotor3Interrupt() { g_amotors[3].onInterrupt(); }
void (*c_afnInterrupts[4])() = {
OnMotor0Interrupt, OnMotor1Interrupt, OnMotor2Interrupt, OnMotor3Interrupt
};
#define RELAY_PIN 9
//////////////////////////////////////////////////////////////////////////////
//
// IMU
//
Adafruit_BNO055 g_bno = Adafruit_BNO055(55);
bool g_bBNO = false;
//////////////////////////////////////////////////////////////////////////////
//
// LIDAR
//
struct SFeedbackServo {
XL320 m_servo;
static int const c_nServoID = 1;
// static int const c_nMinAngle = -104;
// static int const c_nMaxAngle = 104;
static int const c_nMinPosition = 144;
static int const c_nMaxPosition = 880;
static int const c_nAngleTolerance = 15; // Depends on the pull the cables exert
#include "inc/imu.h"
Adafruit_BNO055 bno = Adafruit_BNO055();
Timer timer_imu(IMU_SAMPLING_PERIOD, cb_imu);
ros::Time msg_timestamp;
uint32_t msg_seq = 0;
const char msg_frame_id[] = "base_link";
geometry_msgs::QuaternionStamped orientation_msg;
geometry_msgs::Vector3Stamped angular_vel_msg, linear_accel_msg;
ros::Publisher pub_orientation("crawler0/orientation", &orientation_msg);
ros::Publisher pub_angular_vel("crawler0/angular_velocity", &angular_vel_msg);
ros::Publisher pub_linear_accel("crawler0/linear_acceleration", &linear_accel_msg);
imu::Quaternion orientation;
imu::Vector<3> linear_accel, angular_vel;
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;
