mirror of
https://github.com/OSURoboticsClub/Rover_2017_2018.git
synced 2025-11-08 18:21:15 +00:00
132 lines
3.1 KiB
C++
132 lines
3.1 KiB
C++
////////// Includes //////////
|
|
#include "SBUS.h"
|
|
#include <ModbusRtu.h>
|
|
#include "Arduino.h" // Needed so I can nicely define things
|
|
|
|
////////// Hardware / Data Enumerations //////////
|
|
enum HARDWARE {
|
|
TELEM_24V = A0,
|
|
TELEM_5V = A1,
|
|
USB_TELEM_5V = A2,
|
|
TELEM_3V3 = A3,
|
|
|
|
BAD_1 = 3,
|
|
BAD_2 = 4,
|
|
BAD_3 = 23,
|
|
BAD_4 = 24,
|
|
|
|
LED_RED = 1,
|
|
LED_GREEN = 32,
|
|
LED_BLUE = 6,
|
|
|
|
LED_BLUE_EXTRA = 13
|
|
};
|
|
|
|
enum MODBUS_REGISTERS {
|
|
// 0-15 are directly written from SBUS Class
|
|
VOLTAGE_24 = 16,
|
|
VOLTAGE_5 = 17,
|
|
USB_VOLTAGE_5 = 18,
|
|
VOLTAGE_3V3 = 19
|
|
};
|
|
|
|
////////// Global Variables //////////
|
|
const uint8_t node_id = 1;
|
|
const uint8_t mobus_serial_port_number = 2;
|
|
|
|
#define SBUS_HARDWARE_PORT Serial3
|
|
|
|
uint16_t modbus_data[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
|
|
uint8_t num_modbus_registers = 0;
|
|
|
|
int8_t poll_state = 0;
|
|
bool communication_good = false;
|
|
uint8_t message_count = 0;
|
|
|
|
uint8_t failSafe;
|
|
uint16_t lostFrames = 0;
|
|
|
|
uint16_t telem_24v_scalar = 37500;
|
|
|
|
////////// Class Instantiations //////////
|
|
SBUS x8r(SBUS_HARDWARE_PORT);
|
|
Modbus slave(node_id, mobus_serial_port_number, 0); // 0 in thrid param means regular UART
|
|
|
|
void setup() {
|
|
setup_hardware();
|
|
x8r.begin();
|
|
|
|
num_modbus_registers = sizeof(modbus_data) / sizeof(modbus_data[0]);
|
|
slave.begin(115200);
|
|
|
|
Serial.begin(9600);
|
|
}
|
|
|
|
void loop() {
|
|
poll_sbus();
|
|
poll_modbus();
|
|
set_leds();
|
|
poll_sensors();
|
|
}
|
|
|
|
void setup_hardware(){
|
|
// Setup pins as inputs / outputs
|
|
pinMode(HARDWARE::TELEM_24V, INPUT);
|
|
pinMode(HARDWARE::TELEM_5V, INPUT);
|
|
pinMode(HARDWARE::USB_TELEM_5V, INPUT);
|
|
pinMode(HARDWARE::TELEM_3V3, INPUT);
|
|
|
|
pinMode(HARDWARE::BAD_1, INPUT);
|
|
pinMode(HARDWARE::BAD_2, INPUT);
|
|
pinMode(HARDWARE::BAD_3, INPUT);
|
|
pinMode(HARDWARE::BAD_4, INPUT);
|
|
|
|
pinMode(HARDWARE::LED_RED, OUTPUT);
|
|
pinMode(HARDWARE::LED_GREEN, OUTPUT);
|
|
pinMode(HARDWARE::LED_BLUE, OUTPUT);
|
|
|
|
pinMode(HARDWARE::LED_BLUE_EXTRA, OUTPUT);
|
|
|
|
// Set default pin states
|
|
digitalWrite(HARDWARE::LED_RED, LOW);
|
|
digitalWrite(HARDWARE::LED_GREEN, HIGH);
|
|
digitalWrite(HARDWARE::LED_BLUE, HIGH);
|
|
|
|
digitalWrite(HARDWARE::LED_BLUE_EXTRA, LOW);
|
|
|
|
// Set teensy to increased analog resolution
|
|
analogReadResolution(13);
|
|
}
|
|
|
|
|
|
void poll_sbus(){
|
|
x8r.read(&modbus_data[0], &failSafe, &lostFrames);
|
|
}
|
|
|
|
void poll_sensors(){
|
|
modbus_data[MODBUS_REGISTERS::VOLTAGE_24] = telem_24v_scalar * (analogRead(HARDWARE::TELEM_24V) / 8192.0);
|
|
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
digitalWrite(HARDWARE::LED_GREEN, LOW);
|
|
digitalWrite(HARDWARE::LED_RED, HIGH);
|
|
}else if(!communication_good){
|
|
digitalWrite(HARDWARE::LED_BLUE_EXTRA, LOW);
|
|
digitalWrite(HARDWARE::LED_GREEN, HIGH);
|
|
digitalWrite(HARDWARE::LED_RED, LOW);
|
|
}
|
|
}
|