Replace test_stand_firmware.ino, switched to serial polling to lag issues,...

Replace test_stand_firmware.ino, switched to serial polling to lag issues, testing of math effectiveness could be conducted

test_stand_firmware/test_stand_firmware.ino

-#include <movingAvg.h>
-
-const int DATA_PIN = A7;
-const int BUTTON_PIN = 2;
-int buttonState = 0;
-const boolean NO_HANDSHAKE = false;
-const int SEND_CODE = 65;
-int incomingByte = 0;
-
-int val = 0;
-const double TEN_BIT_SCALAR = 0.3516; // 360/1024
-int homePosition;
-
-// Rate variables
-long lastTime = 0;
-long currTime = 0;
-long lastReading = 0;
-long currReading = 0;
-int prevRate = 0;
-long deltaTime = 0;
-long deltaReading = 0;
-
-double rate;
-
-// Long Press Variables
-long buttonTimer = 0;
-const long LONG_PRESS_MILLIS = 250;
-boolean buttonActive = false;
-boolean longPressActive = false;
-boolean isInPositionMode = true;
-movingAvg mySensor(10);
-
-void serialFlush() {
-  while (Serial.available() > 0) {
-    char t = Serial.read();
-  }
-}
-
-void setup() {
-  // put your setup code here, to run once:
-  Serial.begin(9600);
-  pinMode(BUTTON_PIN, INPUT);
-  pinMode(LED_BUILTIN, OUTPUT);
-  homePosition = analogRead(DATA_PIN);
-  homePosition += 512;
-  lastTime = millis();
-  lastReading = analogRead(DATA_PIN);
-  
-  mySensor.begin();
-}
-
-void loop() {
-  currTime = millis();
-  currReading = analogRead(DATA_PIN);
-
-  if (digitalRead(BUTTON_PIN) == HIGH) {
-    if (buttonActive == false) { // Button was not pressed last cycle
-      buttonActive = true;
-      buttonTimer = millis();
-    }
-    if (((millis() - buttonTimer) > LONG_PRESS_MILLIS) && (longPressActive == false)) { // Detected a long press, swap modes
-      longPressActive = true;
-      isInPositionMode = !isInPositionMode;
-    }
-  }
-  else {
-    if (buttonActive == true) { // Button was being pressed last cycle
-      if (longPressActive == true) { // Button was being long pressed last cycle, user has released the long press
-        longPressActive = false;
-      }
-      else { // Short press occured, grab current position regardless of mode
-        homePosition = analogRead(DATA_PIN);
-        homePosition += 512;
-      }
-      buttonActive = false;
-    }
-  }
-
-  // LED on if we're in position mode, off if we're in rate mode
-  digitalWrite(LED_BUILTIN, isInPositionMode);
-
-  if (isInPositionMode == true) {
-    double absoluteDegrees = TEN_BIT_SCALAR * (double) currReading;
-    double relativeDegrees = TEN_BIT_SCALAR * (double) homePosition;
-    double displayPosition = absoluteDegrees - relativeDegrees;
-    displayPosition += (displayPosition < 0) ? (360) : (0);
-    Serial.println((displayPosition-180));
-    delay(100);
-  }
-  else { // We're in rate mode
-    
-    deltaTime = currTime - lastTime;
-    deltaReading = currReading - lastReading;
-    rate = 1000 * ((TEN_BIT_SCALAR * deltaReading) / (deltaTime));
-    
-    // Thanks josh for the fix
-    if(abs(rate - prevRate) > 360) {
-      
-    }else{
-      mySensor.reading(rate);
-      Serial.println(mySensor.getAvg()*-1);
-      delay(50);
-    }
-    prevRate = rate;
-    }
-  // Reset vars for next cycle
-  lastTime = currTime;
-  lastReading = currReading;
-}
+#include <movingAvg.h>
+unsigned long test_time = 0;
+const int DATA_PIN = A7;
+const int BUTTON_PIN = 2;
+boolean isStreaming = false;
+int f = 200;
+int w = 0;
+unsigned long time_step = 50000;
+
+const double TEN_BIT_SCALAR = 0.3516; // 360/1024
+int homePosition;
+double displayPosition;
+
+// Rate variables
+unsigned long lastTime = 0;
+unsigned long currTime = 0;
+int lastReading = 0;
+int currReading = 0;
+double deltaTime = 1;
+double deltaReading = 0;
+unsigned long buttonTimer = 0;
+int i = 0;
+
+double rate;
+
+// Long Press Variables
+const long LONG_PRESS = 250;
+boolean isInPositionMode = true;
+movingAvg myRate(20);
+
+void rateMode(){
+  deltaTime = currTime - lastTime; // Find change in time
+  deltaReading = currReading - lastReading; // Find change in position
+  deltaReading += (deltaReading < -512) ? (1024) : (0);
+  deltaReading -= (deltaReading > 512) ? (1024) : (0);
+  
+  // Rate is change in position divided by change in time (multiply by 10^6 to convert microseconds to seconds)
+  rate = TEN_BIT_SCALAR * deltaReading * 1000000 / deltaTime;
+ 
+  myRate.reading(rate);
+  rate = myRate.getAvg();
+  
+  if(isStreaming && !(isInPositionMode)){
+    Serial.println(rate);
+  }
+  
+}
+
+void positionMode(){
+  displayPosition = TEN_BIT_SCALAR * (double)(currReading);
+  displayPosition += (displayPosition < 0) ? (360) : (0);
+  displayPosition -= 180;
+  if(isStreaming && isInPositionMode){
+    Serial.println(displayPosition);
+  }
+}
+
+void handleButton(){
+  // If button was pressed
+  if(digitalRead(BUTTON_PIN) == HIGH){ 
+    buttonTimer = millis();
+    while(digitalRead(BUTTON_PIN) == HIGH){   
+    }
+    if(millis() - buttonTimer > LONG_PRESS){ // Detected a long press, swap modes
+      isInPositionMode = !isInPositionMode;
+    }
+    else{ // Short press occured, grab current position to reset home position regardless of mode
+      homePosition = analogRead(DATA_PIN);
+      homePosition += 512;
+    }
+    // LED on if we're in position mode, off if we're in rate mode
+    digitalWrite(LED_BUILTIN, isInPositionMode);
+  }
+}
+
+void handleSerial(){
+  if(Serial.available() > 0){
+    char readChar = Serial.read();
+    switch(readChar){
+      case 'h': // Set current position to be zero
+        homePosition = analogRead(DATA_PIN);
+        homePosition += 512;
+        break;
+      case 'w': // Change wait time between cycles (in microseconds)
+        w = Serial.readStringUntil('!').toInt();
+        if(w > 0 && w < 1000000){
+          time_step = w;
+        }
+        break;
+      case 't': // Toggle mode
+        isInPositionMode = !isInPositionMode;
+        // LED on if we're in position mode, off if we're in rate mode
+        digitalWrite(LED_BUILTIN, isInPositionMode);
+        break;
+      case 'p': // Set to position mode
+        isInPositionMode = true;
+        // LED on if we're in position mode, off if we're in rate mode
+        digitalWrite(LED_BUILTIN, isInPositionMode);
+        break;
+      case 'r': // Set to rate mode
+        isInPositionMode = false;
+        // LED on if we're in position mode, off if we're in rate mode
+        digitalWrite(LED_BUILTIN, isInPositionMode);
+        break;
+      case 'P': // Print position
+        Serial.println(displayPosition);
+        break;
+      case 'R': // Print rate
+        Serial.println(rate);
+        break;
+      case 'A': // Print value based on mode
+        if(isInPositionMode){
+          Serial.println(displayPosition);
+        }
+        else{
+          Serial.println(rate);
+        }
+        break;
+      case 's': // Enable Streaming
+        isStreaming = true;
+        break;
+      case 'd': // Disable Streaming
+        isStreaming = false;
+        break;
+      case 'x': // Print raw value 
+        Serial.println(analogRead(DATA_PIN));
+        break;
+    }
+  }
+}
+
+void setup() {
+  // put your setup code here, to run once:
+  Serial.begin(9600);
+  pinMode(BUTTON_PIN, INPUT);
+  pinMode(DATA_PIN, INPUT);
+  pinMode(LED_BUILTIN, OUTPUT);
+  homePosition = analogRead(DATA_PIN); // Capture home position
+  homePosition += 512; // Offset home position to middle value
+  lastTime = micros();
+  lastReading = analogRead(DATA_PIN);
+  // LED on if we're in position mode, off if we're in rate mode
+  digitalWrite(LED_BUILTIN, isInPositionMode);
+  myRate.begin();
+}
+
+void loop() {  
+    currTime = micros();
+    currReading = (analogRead(DATA_PIN) - homePosition + 1024) % 1024;
+
+    // Switch mode after long button press, zero position after short button press
+    handleButton();
+    // Find the position
+    positionMode();
+    // Find the rate
+    rateMode();
+    // Handle commands from the PC
+    handleSerial();
+    // Reset vars for next cycle
+    lastTime = currTime;
+    lastReading = currReading;
+    delayMicroseconds(time_step);
+ 
+}