mirror of
https://github.com/OSURoboticsClub/Rover_2017_2018.git
synced 2025-11-08 18:21:15 +00:00
290 lines
7.2 KiB
C++
290 lines
7.2 KiB
C++
////////// Includes //////////
|
|
#include <ModbusRtu.h>
|
|
#include <Adafruit_BNO055_t3.h>
|
|
#include <ArduinoJson.h>
|
|
|
|
/*
|
|
Imu/data (Imu)
|
|
Imu/raw (Imu)
|
|
Imu/mag (MagneticField)
|
|
Imu/temp (Temperature)
|
|
|
|
---Raw Data (Imu/raw) ---
|
|
LinearAccel (x, y, z)
|
|
AngularVel (x, y, z);
|
|
|
|
--- Filtered Data (Imu/data) ---
|
|
Orientation(z, y, z, w)
|
|
LinearAccel(x, y, z)
|
|
AngularVel(x, y, z)
|
|
|
|
--- Mag Data (Imu/mag) ---
|
|
MagField(x, y, z)
|
|
|
|
--- IMU Temp (Imu/temp)
|
|
temp (deg c)
|
|
*/
|
|
|
|
////////// Hardware / Data Enumerations //////////
|
|
enum HARDWARE {
|
|
GPS_IMU_RS485_EN = 3,
|
|
GPS_IMU_RS485_RX = 9,
|
|
GPS_IMU_RS485_TX = 10,
|
|
|
|
COMMS_RS485_EN = 2,
|
|
COMMS_RS485_RX = 0,
|
|
COMMS_RS485_TX = 1,
|
|
|
|
GPS_UART_RX = 7,
|
|
GPS_UART_TX = 8,
|
|
|
|
IMU_SDA = 18,
|
|
IMU_SCL = 19,
|
|
|
|
WHITE_LED_CONTROL = 11,
|
|
C02_SENSOR = A7,
|
|
MISC_PIN = A8,
|
|
|
|
LED_BLUE_EXTRA = 13
|
|
};
|
|
|
|
enum MODBUS_REGISTERS {
|
|
LED_CONTROL = 0, // Input
|
|
CO2_READING_PPM = 1
|
|
};
|
|
|
|
enum LIGHT_STATES {
|
|
NO_CHANGE = 0,
|
|
LIGHT_OFF = 1,
|
|
LIGHT_FLASH = 2,
|
|
LIGHT_MED = 3,
|
|
LIGHT_HIGH = 4
|
|
};
|
|
|
|
#define GPS_SERIAL_PORT Serial3
|
|
#define GPS_IMU_STREAMING_PORT Serial2
|
|
|
|
////////// Global Variables //////////
|
|
///// Modbus
|
|
const uint8_t node_id = 1;
|
|
const uint8_t mobus_serial_port_number = 1;
|
|
|
|
uint16_t modbus_data[] = {0, 0};
|
|
uint8_t num_modbus_registers = 0;
|
|
|
|
int8_t poll_state = 0;
|
|
bool communication_good = false;
|
|
uint8_t message_count = 0;
|
|
|
|
///// IMU
|
|
imu::Vector<3> linear_accel;
|
|
imu::Vector<3> angular_vel;
|
|
imu::Quaternion quat;
|
|
|
|
char float_decimal_places = 8;
|
|
|
|
///// GPS
|
|
char current_byte = '$';
|
|
String nmea_sentence = "";
|
|
char gps_buffer[255];
|
|
unsigned char buffer_count = 0;
|
|
|
|
////////// Class Instantiations //////////
|
|
Modbus slave(node_id, mobus_serial_port_number, HARDWARE::COMMS_RS485_EN);
|
|
|
|
Adafruit_BNO055 bno = Adafruit_BNO055(WIRE_BUS, -1, BNO055_ADDRESS_A, I2C_MASTER, I2C_PINS_18_19, I2C_PULLUP_INT, I2C_RATE_100, I2C_OP_MODE_IMM);
|
|
|
|
const char baud115200[] = "PUBX,41,1,3,3,115200,0";
|
|
|
|
void setup() {
|
|
// Debugging
|
|
Serial.begin(9600);
|
|
// while (!Serial);
|
|
|
|
// Raw pin setup
|
|
setup_hardware();
|
|
|
|
// Setup modbus serial communication
|
|
num_modbus_registers = sizeof(modbus_data) / sizeof(modbus_data[0]);
|
|
slave.begin(115200); // baud-rate at 19200
|
|
slave.setTimeOut(150);
|
|
|
|
// GPS & IMU serial streaming setup
|
|
GPS_IMU_STREAMING_PORT.begin(115200);
|
|
GPS_IMU_STREAMING_PORT.transmitterEnable(HARDWARE::GPS_IMU_RS485_EN);
|
|
|
|
// IMU Setup
|
|
Serial.println("Setting up IMU");
|
|
if (!bno.begin()) {
|
|
/* There was a problem detecting the BNO055 ... check your connections */
|
|
Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
|
|
while (1);
|
|
}
|
|
Serial.println("IMU Online. Setting to external crystal.");
|
|
bno.setExtCrystalUse(true);
|
|
Serial.println("IMU Configured.");
|
|
|
|
// GPS Setup
|
|
GPS_SERIAL_PORT.begin(9600);
|
|
|
|
}
|
|
|
|
void loop() {
|
|
// Reset JSON for next loop
|
|
StaticJsonBuffer<1000> jsonBuffer;
|
|
JsonObject& root = jsonBuffer.createObject();
|
|
|
|
// Do normal polling
|
|
poll_modbus();
|
|
set_leds();
|
|
send_imu_stream_line(root);
|
|
process_gps_and_send_if_ready(root);
|
|
process_white_led_command();
|
|
get_co2_data();
|
|
|
|
// Print JSON and newline
|
|
root.printTo(GPS_IMU_STREAMING_PORT);
|
|
GPS_IMU_STREAMING_PORT.println();
|
|
|
|
|
|
}
|
|
|
|
void setup_hardware() {
|
|
// Setup pins as inputs / outputs
|
|
pinMode(HARDWARE::WHITE_LED_CONTROL, OUTPUT);
|
|
pinMode(HARDWARE::C02_SENSOR, INPUT);
|
|
pinMode(HARDWARE::MISC_PIN, OUTPUT);
|
|
|
|
pinMode(HARDWARE::LED_BLUE_EXTRA, OUTPUT);
|
|
|
|
// Set default pin states
|
|
digitalWrite(HARDWARE::WHITE_LED_CONTROL, LOW);
|
|
digitalWrite(HARDWARE::LED_BLUE_EXTRA, LOW);
|
|
|
|
// Set teensy to increased analog resolution
|
|
analogReadResolution(13);
|
|
|
|
}
|
|
|
|
void send_imu_stream_line(JsonObject &root) {
|
|
JsonObject& imu_object = root.createNestedObject("imu");
|
|
|
|
quat = bno.getQuat();
|
|
linear_accel = bno.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL);
|
|
angular_vel = bno.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);
|
|
|
|
imu_object["ox"] = quat.x();
|
|
imu_object["oy"] = quat.y();
|
|
imu_object["oz"] = quat.z();
|
|
imu_object["ow"] = quat.w();
|
|
|
|
imu_object["lax"] = linear_accel.x();
|
|
imu_object["lay"] = linear_accel.y();
|
|
imu_object["laz"] = linear_accel.z();
|
|
|
|
imu_object["avx"] = angular_vel.x();
|
|
imu_object["avy"] = angular_vel.y();
|
|
imu_object["avz"] = angular_vel.z();
|
|
|
|
//
|
|
// /* Display calibration status for each sensor. */
|
|
// uint8_t system, gyro, accel, mag = 0;
|
|
// bno.getCalibration(&system, &gyro, &accel, &mag);
|
|
// Serial.print("CALIBRATION: Sys=");
|
|
// Serial.print(system, DEC);
|
|
// Serial.print(" Gyro=");
|
|
// Serial.print(gyro, DEC);
|
|
// Serial.print(" Accel=");
|
|
// Serial.print(accel, DEC);
|
|
// Serial.print(" Mag=");
|
|
// Serial.print(mag, DEC);
|
|
}
|
|
|
|
void process_gps_and_send_if_ready(JsonObject &root) {
|
|
root["gps"] = "";
|
|
|
|
char num_in_bytes = GPS_SERIAL_PORT.available();
|
|
|
|
if (num_in_bytes > 0) {
|
|
for (char i = 0 ; i < num_in_bytes ; i++) {
|
|
char in_byte = GPS_SERIAL_PORT.read();
|
|
|
|
if (in_byte != '\n' && in_byte != '\r') {
|
|
gps_buffer[buffer_count] = in_byte;
|
|
buffer_count++;
|
|
}
|
|
|
|
if (in_byte == '\r') {
|
|
gps_buffer[buffer_count] = '\0';
|
|
root["gps"] = gps_buffer;
|
|
buffer_count = 0;
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
void process_white_led_command() {
|
|
uint16_t light_command = modbus_data[MODBUS_REGISTERS::LED_CONTROL];
|
|
|
|
|
|
if (light_command == LIGHT_STATES::LIGHT_MED) {
|
|
digitalWrite(HARDWARE::WHITE_LED_CONTROL, HIGH);
|
|
delay(100); // wait for a second
|
|
digitalWrite(HARDWARE::WHITE_LED_CONTROL, LOW);
|
|
delay(100); // wait for a second
|
|
} else if (light_command == LIGHT_STATES::LIGHT_FLASH) {
|
|
digitalWrite(HARDWARE::WHITE_LED_CONTROL, HIGH);
|
|
delay(100); // wait for a second
|
|
digitalWrite(HARDWARE::WHITE_LED_CONTROL, LOW);
|
|
delay(100); // wait for a second
|
|
digitalWrite(HARDWARE::WHITE_LED_CONTROL, HIGH);
|
|
delay(100); // wait for a second
|
|
digitalWrite(HARDWARE::WHITE_LED_CONTROL, LOW);
|
|
delay(100);
|
|
} else if (light_command == LIGHT_STATES::LIGHT_HIGH) {
|
|
digitalWrite(HARDWARE::WHITE_LED_CONTROL, HIGH);
|
|
delay(750); // wait for a second
|
|
digitalWrite(HARDWARE::WHITE_LED_CONTROL, LOW);
|
|
delay(100);
|
|
} else if (light_command == LIGHT_STATES::LIGHT_OFF) {
|
|
digitalWrite(HARDWARE::WHITE_LED_CONTROL, HIGH);
|
|
delay(2000); // wait for a second
|
|
digitalWrite(HARDWARE::WHITE_LED_CONTROL, LOW);
|
|
delay(2000);
|
|
}
|
|
|
|
modbus_data[MODBUS_REGISTERS::LED_CONTROL] = LIGHT_STATES::NO_CHANGE;
|
|
}
|
|
|
|
void get_co2_data() {
|
|
int voltage = ((analogRead(HARDWARE::C02_SENSOR) / 8192.0) * 3300);
|
|
|
|
if (voltage < 400 || voltage > 2000) {
|
|
modbus_data[MODBUS_REGISTERS::CO2_READING_PPM] = 9999;
|
|
} else {
|
|
modbus_data[MODBUS_REGISTERS::CO2_READING_PPM] = map(voltage, 400, 2000, 0, 5000);
|
|
}
|
|
|
|
// Serial.println(modbus_data[MODBUS_REGISTERS::CO2_READING_PPM]);
|
|
|
|
}
|
|
|
|
void poll_modbus() {
|
|
poll_state = slave.poll(modbus_data, num_modbus_registers);
|
|
communication_good = !slave.getTimeOutState();
|
|
}
|
|
|
|
void set_leds() {
|
|
if (poll_state > 4) {
|
|
message_count++;
|
|
if (message_count > 2) {
|
|
digitalWrite(HARDWARE::LED_BLUE_EXTRA, !digitalRead(HARDWARE::LED_BLUE_EXTRA));
|
|
message_count = 0;
|
|
}
|
|
} else if (!communication_good) {
|
|
digitalWrite(HARDWARE::LED_BLUE_EXTRA, LOW);
|
|
}
|
|
}
|