Rover arm pretty much working! Still some minor changes needed probably, but very happy with it. Lots of misc changes.

software/firmware/rdf/rdf.ino

@@ -0,0 +1,153 @@
+////////// Includes //////////
+#include <ModbusRtu.h>
+
+////////// Hardware / Data Enumerations //////////
+enum HARDWARE {
+    RS485_EN = 2,
+    RS485_RX = 7,
+    RS485_TX = 8,
+
+    MOTOR_CURRENT_SENSE = A4,
+    MOTOR_DIRECTION = 19,
+    MOTOR_PWM = 20,
+    MOTOR_SLEEP = 21,
+    MOTOR_FAULT = 22,
+
+    TEMP = A9,
+
+    LED_RED = 1,
+    LED_GREEN = 32,
+    LED_BLUE = 6,
+
+    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
+};
+
+////////// Global Variables //////////
+const uint8_t node_id = 2;
+const uint8_t mobus_serial_port_number = 3;
+
+uint16_t modbus_data[] = {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 rampdown_step = 2000;
+
+////////// Class Instantiations //////////
+Modbus slave(node_id, mobus_serial_port_number, HARDWARE::RS485_EN);
+
+void setup() {
+    setup_hardware();
+
+    num_modbus_registers = sizeof(modbus_data) / sizeof(modbus_data[0]);
+    slave.begin(115200); // baud-rate at 19200
+    slave.setTimeOut(150);
+
+    Serial.begin(9600);
+}
+
+void loop() {
+    poll_modbus();
+    set_leds();
+    set_motor();
+    poll_sensors_and_motor_state();
+}
+
+void setup_hardware(){
+    // Setup pins as inputs / outputs
+    pinMode(HARDWARE::RS485_EN, OUTPUT);
+
+    pinMode(HARDWARE::MOTOR_CURRENT_SENSE, INPUT);
+    pinMode(HARDWARE::MOTOR_DIRECTION, OUTPUT);
+    pinMode(HARDWARE::MOTOR_PWM, OUTPUT);
+    pinMode(HARDWARE::MOTOR_SLEEP, OUTPUT);
+    pinMode(HARDWARE::MOTOR_FAULT, INPUT);
+
+    pinMode(HARDWARE::TEMP, 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::MOTOR_SLEEP, HIGH);
+
+    digitalWrite(HARDWARE::LED_RED, LOW);
+    digitalWrite(HARDWARE::LED_GREEN, HIGH);
+    digitalWrite(HARDWARE::LED_BLUE, HIGH);
+
+    digitalWrite(HARDWARE::LED_BLUE_EXTRA, LOW);
+
+    // Set the PWM resolution to 16-bits
+    analogWriteResolution(16);
+
+    // Change motor PWM frequency so it's not in the audible range
+    analogWriteFrequency(HARDWARE::MOTOR_PWM, 25000);
+
+    // Set teensy to increased analog resolution
+    analogReadResolution(13);
+}
+
+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);
+    }
+}
+
+void set_motor(){
+    if(communication_good){
+        digitalWrite(HARDWARE::MOTOR_DIRECTION, modbus_data[MODBUS_REGISTERS::DIRECTION]);
+        analogWrite(HARDWARE::MOTOR_PWM, modbus_data[MODBUS_REGISTERS::SPEED]);
+        digitalWrite(HARDWARE::MOTOR_SLEEP, modbus_data[MODBUS_REGISTERS::SLEEP]);
+    }else{
+        while(modbus_data[MODBUS_REGISTERS::SPEED] != 0 && modbus_data[MODBUS_REGISTERS::SPEED] > rampdown_step){
+            modbus_data[MODBUS_REGISTERS::SPEED] -= rampdown_step;
+            analogWrite(HARDWARE::MOTOR_PWM, modbus_data[MODBUS_REGISTERS::SPEED]);
+            delay(2);
+        }
+
+        modbus_data[MODBUS_REGISTERS::SPEED] = 0;
+        analogWrite(HARDWARE::MOTOR_PWM, modbus_data[MODBUS_REGISTERS::SPEED]);
+    }
+
+}
+
+void poll_sensors_and_motor_state(){
+    // Not the most elegant calculations, could clean up.
+    modbus_data[MODBUS_REGISTERS::CURRENT] = (uint16_t)(((((analogRead(HARDWARE::MOTOR_CURRENT_SENSE) / 8192.0) * 3.3) - 0.05) / 0.02) * 1000);
+    modbus_data[MODBUS_REGISTERS::FAULT] = !digitalRead(HARDWARE::MOTOR_FAULT);
+    modbus_data[MODBUS_REGISTERS::TEMPERATURE] = (uint16_t)(((((analogRead(HARDWARE::TEMP) / 8192.0) * 3.3) - 0.750) / 0.01) * 1000);
+
+    Serial.println(modbus_data[MODBUS_REGISTERS::CURRENT]);
+}

software/firmware/temp/original_rdf.ino

@@ -0,0 +1,84 @@
+#define Rpin A5                 //Reciever pin is A1
+int sensitivity = 50;           //the lower this number, the more sensitive.
+int state;                      //=1 if can hear ping        =2 if cannot hear ping
+float freq;
+float ambientNoise;
+unsigned long totalDataPoints;
+int dataBuff[3];
+int data[3];
+unsigned long t1,t2,t3;
+int dt1,dt2 =0;
+float dtavg;
+int tcnt =2;
+bool upstate = false;
+bool mode = false;                      //switches between two modes (t/f)
+void setup() {
+  pinMode(Rpin, INPUT);
+  Serial.begin(9600);
+  totalDataPoints = 0;
+  state = 2;
+}
+void dataSet(){
+  for(int i=0;i<3;i++){
+    data[i]=analogRead(Rpin);
+    delay(1);
+  }
+}
+int changeCheck(){
+    int newSignalState = 0;                // 0= no change      1= signal start       2= signal stop
+    if((data[0])>(dataBuff[2]+sensitivity)||data[2] > data[0]+sensitivity)
+      newSignalState = 1;
+    if((data[2] < data[0]-sensitivity)||(data[0]<(dataBuff[2]-sensitivity)))
+      newSignalState = 2;
+    return newSignalState;
+
+}
+void loop() {
+
+  if(Serial.available()){
+    Serial.read();
+    mode = !mode;
+  }
+  if(mode == true){                      //mode 1 is raw output from the antena.
+    Serial.println(analogRead(Rpin));
+    delay(1);
+  }
+  else{
+    for(int i=0;i<3;i++)
+      dataBuff[i] = data[i];
+    dataSet();
+    int change = changeCheck();
+    if(change){
+      state = change;
+      if(change == 1){
+        t1 = millis();
+        dt1 = t1-t2;
+      }else{
+        t2 = millis();
+        dt2 = t2-t1;
+      }
+      if(dt2>dt1-100&&dt1>dt2-100){
+        dtavg = (dtavg*float(tcnt-2)/float(tcnt))+(((dt1+dt2)/2.0)*float(2.0/tcnt));
+        tcnt += 2;
+        freq = 500.0/dtavg;
+      }
+    }
+    if(change ==1 || millis() > dtavg*2+t3){
+      t3 = millis();
+      upstate = true;
+    }
+    if(millis()<t3+dtavg){
+      for(int i=0;i<3;i++){
+        Serial.print(data[i]-ambientNoise);
+        Serial.print(", ");
+        Serial.println(freq);
+      }
+    }else{
+      float avgdat,dtot =0;
+      for(int i=0;i<3;i++)
+        dtot+=data[i];
+      avgdat = dtot/3.0;
+      ambientNoise = ambientNoise*float(totalDataPoints)/float(totalDataPoints+3)+avgdat*(3.0/float(totalDataPoints+3));
+    }
+  }
+}