diff --git a/test_stand_firmware/test_stand_firmware.ino b/test_stand_firmware/test_stand_firmware.ino
index 7f244ac5f923fa9ac6f29e300200cb68b53170c7..8aa506f4dcc6b276116f35adfc1d708904dc36df 100644
--- a/test_stand_firmware/test_stand_firmware.ino
+++ b/test_stand_firmware/test_stand_firmware.ino
@@ -1,109 +1,164 @@
-#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);
+
+}