From 3e4d85c9a1a1b1f185ed4dfab54cb601e5906e34 Mon Sep 17 00:00:00 2001
From: Corwin Perren <caperren@gmail.com>
Date: Wed, 21 Feb 2018 00:17:31 -0800
Subject: [PATCH] Made proper iris firmware, minus voltage telemetry. 24V is
 working, just need to finish the rest.

---
 software/firmware/iris/iris.ino               | 129 ++++++++++++++++--
 .../firmware/motor_driver/motor_driver.ino    |   5 +-
 2 files changed, 115 insertions(+), 19 deletions(-)

diff --git a/software/firmware/iris/iris.ino b/software/firmware/iris/iris.ino
index 437028a..b9647a5 100644
--- a/software/firmware/iris/iris.ino
+++ b/software/firmware/iris/iris.ino
@@ -1,33 +1,132 @@
+////////// Includes //////////
 #include "SBUS.h"
 #include <ModbusRtu.h>
+#include "Arduino.h" // Needed so I can nicely define things
 
-SBUS x8r(Serial3);
+////////// Hardware / Data Enumerations //////////
+enum HARDWARE {
+    TELEM_24V = A0,
+    TELEM_5V = A1,
+    USB_TELEM_5V = A2,
+    TELEM_3V3 = A3,
 
-const char num_modbus_channels = 16; //Being explicit so I get errors if I try to do something stupid
-uint16_t modbus_data[num_modbus_channels] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0 , 0 , 0, 0, 0, 0, 0};
+    BAD_1 = 3,
+    BAD_2 = 4,
+    BAD_3 = 23,
+    BAD_4 = 24,
 
-Modbus slave(1, 2, 0);
+    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;
 
-uint16_t channels[16];
 uint8_t failSafe;
 uint16_t lostFrames = 0;
 
-const char bad_pins[] = {3, 4, 23, 24,};
+uint16_t telem_24v_scalar = 37989;
+
+////////// 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() {
-  for (int i = 0 ; i < sizeof(bad_pins) / sizeof(bad_pins[0]) ; i++) {
-    pinMode(bad_pins[i], INPUT);
-  }
+    setup_hardware();
+    x8r.begin();
 
-  pinMode(8, INPUT_PULLUP);
+    num_modbus_registers = sizeof(modbus_data) / sizeof(modbus_data[0]);
+    slave.begin(2000000);
 
-  //  analogWriteResolution(16);
-  x8r.begin();
-  slave.begin(2000000); // baud-rate at 19200
+    Serial.begin(9600);
 }
 
 void loop() {
-  x8r.read(&channels[0], &failSafe, &lostFrames);
-  slave.poll( channels, num_modbus_channels );
+    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(){
+    uint16_t v24 = telem_24v_scalar * (analogRead(HARDWARE::TELEM_24V) / 8192.0);
+    Serial.println(v24);
 
 }
+
+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);
+    }
+}
diff --git a/software/firmware/motor_driver/motor_driver.ino b/software/firmware/motor_driver/motor_driver.ino
index 6b9022e..dbe463c 100644
--- a/software/firmware/motor_driver/motor_driver.ino
+++ b/software/firmware/motor_driver/motor_driver.ino
@@ -1,8 +1,7 @@
 ////////// Includes //////////
-// Define the arduino serial port used for modbus here
 #include <ModbusRtu.h>
 
-////////// Hardware / Date Enumerations //////////
+////////// Hardware / Data Enumerations //////////
 enum HARDWARE {
     RS485_EN = 2,
     RS485_RX = 7,
@@ -56,8 +55,6 @@ void setup() {
     num_modbus_registers = sizeof(modbus_data) / sizeof(modbus_data[0]);
     slave.begin(2000000); // baud-rate at 19200
     slave.setTimeOut(150);
-
-    Serial.begin(9600);
 }
 
 void loop() {