caperren/osurc_mars_rover_2017_2018
1
0
Fork 0
mirror of https://github.com/OSURoboticsClub/Rover_2017_2018.git synced 2025-11-08 18:21:15 +00:00
Code Issues Packages Projects Releases Wiki Activity

Added first version of tower firmware. Spits out GPS and IMU data on port RS485-1 and talks over modbus on RS585-2. Modbus side doesn't do anything atm.

Browse Source
This commit is contained in:
Corwin Perren
2018-07-05 20:22:51 -07:00
parent 8a83b6f140
commit 9c848e8368
1 changed files with 159 additions and 91 deletions
Download Patch File Download Diff File Expand all files Collapse all files

250
software/firmware/tower/tower.ino
View File

@@ -1,9 +1,7 @@
#include <NMEAGPS.h> ////////// Includes //////////
#include <ublox/ubxGPS.h> #include <ModbusRtu.h>
#include <NeoGPS_cfg.h>
#include <Adafruit_BNO055_t3.h> #include <Adafruit_BNO055_t3.h>
/* /*
Imu/data (Imu) Imu/data (Imu)
Imu/raw (Imu) Imu/raw (Imu)
@@ -26,60 +24,88 @@
temp (deg c) temp (deg c)
*/ */
Adafruit_BNO055 bno = Adafruit_BNO055(WIRE_BUS, -1, BNO055_ADDRESS_A, I2C_MASTER, I2C_PINS_29_30, I2C_PULLUP_INT, I2C_RATE_100, I2C_OP_MODE_ISR); ////////// 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,
#define gpsPort Serial2 GPS_UART_RX = 7,
#define commsPort Serial3 GPS_UART_TX = 8,
#define GPS_PORT_NAME "Serial2"
const unsigned char ubxRate5Hz[] = { 0x06, 0x08, 0x06, 0x00, 200, 0x00, 0x01, 0x00, 0x01, 0x00 }; IMU_SDA = 18,
const unsigned char ubxRate10Hz[] = { 0x06, 0x08, 0x06, 0x00, 100, 0x00, 0x01, 0x00, 0x01, 0x00 }; IMU_SCL = 19,
const char baud38400 [] = "PUBX,41,1,3,3,38400,0"; WS2812_DATA = 11,
const char baud57600 [] = "PUBX,41,1,3,3,57600,0"; C02_SENSOR = A7,
const char baud115200[] = "PUBX,41,1,3,3,115200,0"; MISC_PIN = A8,
LED_BLUE_EXTRA = 13
};
enum MODBUS_REGISTERS {
DIRECTION = 0, // Input
SPEED = 1, // Input
SLEEP = 2, // Input
CURRENT = 3, // Output
FAULT = 4, // Output
TEMPERATURE = 5 // Output
};
#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> linear_accel;
imu::Vector<3> angular_vel; imu::Vector<3> angular_vel;
imu::Quaternion quat; imu::Quaternion quat;
char current_byte = '$';
String nmea_sentence = "";
String output_sentence;
char float_decimal_places = 8; char float_decimal_places = 8;
void sendUBX( const unsigned char *progmemBytes, size_t len ) ///// GPS
{ char current_byte = '$';
gpsPort.write( 0xB5 ); // SYNC1 String nmea_sentence = "";
gpsPort.write( 0x62 ); // SYNC2
uint8_t a = 0, b = 0; ////////// Class Instantiations //////////
while (len-- > 0) { Modbus slave(node_id, mobus_serial_port_number, HARDWARE::COMMS_RS485_EN);
uint8_t c = pgm_read_byte( progmemBytes++ );
a += c;
b += a;
gpsPort.write( c );
}
gpsPort.write( a ); // CHECKSUM A 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_ISR);
gpsPort.write( b ); // CHECKSUM B
}
void setup() { void setup() {
// Debugging
Serial.begin(9600); Serial.begin(9600);
while (!Serial);
Serial.println("Booting up");
// put your setup code here, to run once:
commsPort.begin(115200);
commsPort.transmitterEnable(3);
delay(250); // Raw pin setup
gpsPort.begin(9600); setup_hardware();
// sendUBX( ubxRate5Hz, sizeof(ubxRate5Hz) );
// 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"); Serial.println("Setting up IMU");
if (!bno.begin()) { if (!bno.begin()) {
/* There was a problem detecting the BNO055 ... check your connections */ /* There was a problem detecting the BNO055 ... check your connections */
@@ -90,75 +116,117 @@ void setup() {
bno.setExtCrystalUse(true); bno.setExtCrystalUse(true);
Serial.println("IMU Configured."); Serial.println("IMU Configured.");
// GPS Setup
GPS_SERIAL_PORT.begin(9600);
} }
void loop() { void loop() {
poll_modbus();
set_leds();
send_imu_stream_line();
process_gps_and_send_if_ready();
if (gpsPort.available() > 0 ) { }
nmea_sentence = "";
do {
if (gpsPort.available() > 0) {
current_byte = gpsPort.read();
if (current_byte != '\r' and current_byte != '\n') { void setup_hardware() {
nmea_sentence += current_byte; // Setup pins as inputs / outputs
} pinMode(HARDWARE::WS2812_DATA, OUTPUT);
} pinMode(HARDWARE::C02_SENSOR, INPUT);
} while (current_byte != '\r'); pinMode(HARDWARE::MISC_PIN, OUTPUT);
commsPort.println(nmea_sentence);
}
pinMode(HARDWARE::LED_BLUE_EXTRA, OUTPUT);
// Set default pin states
digitalWrite(HARDWARE::LED_BLUE_EXTRA, LOW);
// Set teensy to increased analog resolution
analogReadResolution(13);
}
void send_imu_stream_line() {
quat = bno.getQuat(); quat = bno.getQuat();
linear_accel = bno.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL); linear_accel = bno.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL);
angular_vel = bno.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE); angular_vel = bno.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);
commsPort.print("ox:"); GPS_IMU_STREAMING_PORT.print("ox:");
commsPort.print(quat.x(), float_decimal_places); GPS_IMU_STREAMING_PORT.print(quat.x(), float_decimal_places);
commsPort.print(","); GPS_IMU_STREAMING_PORT.print(",");
commsPort.print("oy:"); GPS_IMU_STREAMING_PORT.print("oy:");
commsPort.print(quat.y(), float_decimal_places); GPS_IMU_STREAMING_PORT.print(quat.y(), float_decimal_places);
commsPort.print(","); GPS_IMU_STREAMING_PORT.print(",");
commsPort.print("oz:"); GPS_IMU_STREAMING_PORT.print("oz:");
commsPort.print(quat.z(), float_decimal_places); GPS_IMU_STREAMING_PORT.print(quat.z(), float_decimal_places);
commsPort.print(","); GPS_IMU_STREAMING_PORT.print(",");
commsPort.print("ow:"); GPS_IMU_STREAMING_PORT.print("ow:");
commsPort.print(quat.w(), float_decimal_places); GPS_IMU_STREAMING_PORT.print(quat.w(), float_decimal_places);
commsPort.print(","); GPS_IMU_STREAMING_PORT.print(",");
commsPort.print("lax:"); GPS_IMU_STREAMING_PORT.print("lax:");
commsPort.print(linear_accel.x(), float_decimal_places); GPS_IMU_STREAMING_PORT.print(linear_accel.x(), float_decimal_places);
commsPort.print(","); GPS_IMU_STREAMING_PORT.print(",");
commsPort.print("lay:"); GPS_IMU_STREAMING_PORT.print("lay:");
commsPort.print(linear_accel.y(), float_decimal_places); GPS_IMU_STREAMING_PORT.print(linear_accel.y(), float_decimal_places);
commsPort.print(","); GPS_IMU_STREAMING_PORT.print(",");
commsPort.print("laz:"); GPS_IMU_STREAMING_PORT.print("laz:");
commsPort.print(linear_accel.z(), float_decimal_places); GPS_IMU_STREAMING_PORT.print(linear_accel.z(), float_decimal_places);
commsPort.print(","); GPS_IMU_STREAMING_PORT.print(",");
commsPort.print("avx:"); GPS_IMU_STREAMING_PORT.print("avx:");
commsPort.print(angular_vel.x(), float_decimal_places); GPS_IMU_STREAMING_PORT.print(angular_vel.x(), float_decimal_places);
commsPort.print(","); GPS_IMU_STREAMING_PORT.print(",");
commsPort.print("avy:"); GPS_IMU_STREAMING_PORT.print("avy:");
commsPort.print(angular_vel.y(), float_decimal_places); GPS_IMU_STREAMING_PORT.print(angular_vel.y(), float_decimal_places);
commsPort.print(","); GPS_IMU_STREAMING_PORT.print(",");
commsPort.print("avz:"); GPS_IMU_STREAMING_PORT.print("avz:");
commsPort.print(angular_vel.z(), float_decimal_places); GPS_IMU_STREAMING_PORT.print(angular_vel.z(), float_decimal_places);
commsPort.println(); GPS_IMU_STREAMING_PORT.println();
// //
// /* Display calibration status for each sensor. */ // /* Display calibration status for each sensor. */
// uint8_t system, gyro, accel, mag = 0; // uint8_t system, gyro, accel, mag = 0;
// bno.getCalibration(&system, &gyro, &accel, &mag); // bno.getCalibration(&system, &gyro, &accel, &mag);
// commsPort.print("CALIBRATION: Sys="); // Serial.print("CALIBRATION: Sys=");
// commsPort.print(system, DEC); // Serial.print(system, DEC);
// commsPort.print(" Gyro="); // Serial.print(" Gyro=");
// commsPort.print(gyro, DEC); // Serial.print(gyro, DEC);
// commsPort.print(" Accel="); // Serial.print(" Accel=");
// commsPort.print(accel, DEC); // Serial.print(accel, DEC);
// commsPort.print(" Mag="); // Serial.print(" Mag=");
// commsPort.print(mag, DEC); // Serial.print(mag, DEC);
}
void process_gps_and_send_if_ready() {
if (GPS_SERIAL_PORT.available() > 0 ) {
current_byte = GPS_SERIAL_PORT.read();
if (current_byte != '\r' && current_byte != '\n') {
nmea_sentence += current_byte;
}
if (current_byte == '\r') {
GPS_IMU_STREAMING_PORT.println(nmea_sentence);
nmea_sentence = "";
}
}
}
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);
}
} }