refact, style: use clang-format

.clang-format

+# Source: https://github.com/arduino/tooling-project-assets/tree/main/other/clang-format-configuration
+---
+AccessModifierOffset: -2
+AlignAfterOpenBracket: Align
+AlignArrayOfStructures: None
+AlignConsecutiveAssignments: None
+AlignConsecutiveBitFields: None
+AlignConsecutiveDeclarations: None
+AlignConsecutiveMacros: None
+AlignEscapedNewlines: DontAlign
+AlignOperands: Align
+AlignTrailingComments: true
+AllowAllArgumentsOnNextLine: true
+AllowAllConstructorInitializersOnNextLine: true
+AllowAllParametersOfDeclarationOnNextLine: true
+AllowShortBlocksOnASingleLine: Always
+AllowShortCaseLabelsOnASingleLine: true
+AllowShortEnumsOnASingleLine: true
+AllowShortFunctionsOnASingleLine: Empty
+AllowShortIfStatementsOnASingleLine: AllIfsAndElse
+AllowShortLambdasOnASingleLine: Empty
+AllowShortLoopsOnASingleLine: true
+AlwaysBreakAfterDefinitionReturnType: None
+AlwaysBreakAfterReturnType: None
+AlwaysBreakBeforeMultilineStrings: false
+AlwaysBreakTemplateDeclarations: No
+AttributeMacros:
+  - __capability
+BasedOnStyle: LLVM
+BinPackArguments: true
+BinPackParameters: true
+BitFieldColonSpacing: Both
+BraceWrapping:
+  AfterCaseLabel: false
+  AfterClass: false
+  AfterControlStatement: Never
+  AfterEnum: false
+  AfterFunction: false
+  AfterNamespace: false
+  AfterObjCDeclaration: false
+  AfterStruct: false
+  AfterUnion: false
+  AfterExternBlock: false
+  BeforeCatch: false
+  BeforeElse: false
+  BeforeLambdaBody: false
+  BeforeWhile: false
+  IndentBraces: false
+  SplitEmptyFunction: true
+  SplitEmptyRecord: true
+  SplitEmptyNamespace: true
+BreakAfterJavaFieldAnnotations: false
+BreakBeforeBinaryOperators: NonAssignment
+BreakBeforeBraces: Attach
+BreakBeforeConceptDeclarations: false
+BreakBeforeInheritanceComma: false
+BreakBeforeTernaryOperators: true
+BreakConstructorInitializers: BeforeColon
+BreakConstructorInitializersBeforeComma: false
+BreakInheritanceList: BeforeColon
+BreakStringLiterals: false
+ColumnLimit: 0
+CommentPragmas: ''
+CompactNamespaces: false
+ConstructorInitializerAllOnOneLineOrOnePerLine: false
+ConstructorInitializerIndentWidth: 2
+ContinuationIndentWidth: 2
+Cpp11BracedListStyle: false
+DeriveLineEnding: true
+DerivePointerAlignment: true
+DisableFormat: false
+EmptyLineAfterAccessModifier: Leave
+EmptyLineBeforeAccessModifier: Leave
+ExperimentalAutoDetectBinPacking: false
+FixNamespaceComments: false
+ForEachMacros:
+  - foreach
+  - Q_FOREACH
+  - BOOST_FOREACH
+IfMacros:
+  - KJ_IF_MAYBE
+IncludeBlocks: Preserve
+IncludeCategories:
+  - Regex: '^"(llvm|llvm-c|clang|clang-c)/'
+    Priority: 2
+    SortPriority: 0
+    CaseSensitive: false
+  - Regex: '^(<|"(gtest|gmock|isl|json)/)'
+    Priority: 3
+    SortPriority: 0
+    CaseSensitive: false
+  - Regex: '.*'
+    Priority: 1
+    SortPriority: 0
+    CaseSensitive: false
+IncludeIsMainRegex: ''
+IncludeIsMainSourceRegex: ''
+IndentAccessModifiers: false
+IndentCaseBlocks: true
+IndentCaseLabels: true
+IndentExternBlock: Indent
+IndentGotoLabels: false
+IndentPPDirectives: None
+IndentRequires: true
+IndentWidth: 2
+IndentWrappedFunctionNames: false
+InsertTrailingCommas: None
+JavaScriptQuotes: Leave
+JavaScriptWrapImports: true
+KeepEmptyLinesAtTheStartOfBlocks: true
+LambdaBodyIndentation: Signature
+Language: Cpp
+MacroBlockBegin: ''
+MacroBlockEnd: ''
+MaxEmptyLinesToKeep: 100000
+NamespaceIndentation: None
+ObjCBinPackProtocolList: Auto
+ObjCBlockIndentWidth: 2
+ObjCBreakBeforeNestedBlockParam: true
+ObjCSpaceAfterProperty: false
+ObjCSpaceBeforeProtocolList: true
+PPIndentWidth: -1
+PackConstructorInitializers: BinPack
+PenaltyBreakAssignment: 1
+PenaltyBreakBeforeFirstCallParameter: 1
+PenaltyBreakComment: 1
+PenaltyBreakFirstLessLess: 1
+PenaltyBreakOpenParenthesis: 1
+PenaltyBreakString: 1
+PenaltyBreakTemplateDeclaration: 1
+PenaltyExcessCharacter: 1
+PenaltyIndentedWhitespace: 1
+PenaltyReturnTypeOnItsOwnLine: 1
+PointerAlignment: Right
+QualifierAlignment: Leave
+ReferenceAlignment: Pointer
+ReflowComments: false
+RemoveBracesLLVM: false
+SeparateDefinitionBlocks: Leave
+ShortNamespaceLines: 0
+SortIncludes: Never
+SortJavaStaticImport: Before
+SortUsingDeclarations: false
+SpaceAfterCStyleCast: false
+SpaceAfterLogicalNot: false
+SpaceAfterTemplateKeyword: false
+SpaceAroundPointerQualifiers: Default
+SpaceBeforeAssignmentOperators: true
+SpaceBeforeCaseColon: false
+SpaceBeforeCpp11BracedList: false
+SpaceBeforeCtorInitializerColon: true
+SpaceBeforeInheritanceColon: true
+SpaceBeforeParens: ControlStatements
+SpaceBeforeParensOptions:
+  AfterControlStatements: true
+  AfterForeachMacros: true
+  AfterFunctionDefinitionName: false
+  AfterFunctionDeclarationName: false
+  AfterIfMacros: true
+  AfterOverloadedOperator: false
+  BeforeNonEmptyParentheses: false
+SpaceBeforeRangeBasedForLoopColon: true
+SpaceBeforeSquareBrackets: false
+SpaceInEmptyBlock: false
+SpaceInEmptyParentheses: false
+SpacesBeforeTrailingComments: 2
+SpacesInAngles: Leave
+SpacesInCStyleCastParentheses: false
+SpacesInConditionalStatement: false
+SpacesInContainerLiterals: false
+SpacesInLineCommentPrefix:
+  Minimum: 0
+  Maximum: -1
+SpacesInParentheses: false
+SpacesInSquareBrackets: false
+Standard: Auto
+StatementAttributeLikeMacros:
+  - Q_EMIT
+StatementMacros:
+  - Q_UNUSED
+  - QT_REQUIRE_VERSION
+TabWidth: 2
+UseCRLF: false
+UseTab: Never
+WhitespaceSensitiveMacros:
+  - STRINGIZE
+  - PP_STRINGIZE
+  - BOOST_PP_STRINGIZE
+  - NS_SWIFT_NAME
+  - CF_SWIFT_NAME

controller/calibration.ino

 /*******************************************************************************
-Calibration
-
-'Calibrate' the wing absolute rotary encoders.
-First we unwrap the measured position by shifting the bits in order to avoid
-wrapping if this is needed.
-Then we determine the maximum position so we can make it match the maximum angle
-attainable.
-
---------------------------------------------------------------------------------
-(c) Copyright 2022 University of Liege
-Author: Thomas Lambert <t.lambert@uliege.be>
-ULiege - Aeroelasticity and Experimental Aerodynamics
-MIT License
-Repo: https://gitlab.uliege.be/thlamb/mecharaptor-controller
+  Calibration
+
+  'Calibrate' the wing absolute rotary encoders.
+  First we unwrap the measured position by shifting the bits in order to avoid
+  wrapping if this is needed.
+  Then we determine the maximum position so we can make it match the maximum angle
+  attainable.
+
+  --------------------------------------------------------------------------------
+  (c) Copyright 2022 University of Liege
+  Author: Thomas Lambert <t.lambert@uliege.be>
+  ULiege - Aeroelasticity and Experimental Aerodynamics
+  MIT License
+  Repo: https://gitlab.uliege.be/thlamb/mecharaptor-controller
 *******************************************************************************/
 
 void Calibrate() {
@@ -48,26 +48,19 @@ void Calibrate() {
 
       Serial.println("[INFO]: Calibration done.");
       Serial.println("Maxium wing posiions are: ");
-      for (int i = 0; i < sizeof(gCSN_PINS); i++) {
-        Serial.print(gMaxWingPos[i]);
-        if (i < sizeof(gCSN_PINS)-1){
-          Serial.print(", ");
-        }
-      Serial.println("Offsets are: ");
-      for (int i = 0; i < sizeof(gCSN_PINS); i++) {
-        Serial.print(gAngleOffset[i]);
-        if (i < sizeof(gCSN_PINS)-1){
-          Serial.print(", ");
-        }
-      }
+      PrintArray(gMaxWingPos);
 
+      Serial.println("Offsets are: ");
+      PrintArray(gAngleOffset);
     } else {
       Serial.println("[INFO]: Calibration aborted. Using default values.");
     }
   }
 
   // Flush Serial before continuing
-  while(Serial.available()){Serial.read();}
+  while (Serial.available()) {
+    Serial.read();
+  }
 }
 
 // Determine if wing position needs to be offset to prevent wrap-around
@@ -79,7 +72,7 @@ void GetAngleOffset() {
   unsigned int minPos[4];
   unsigned int newPos;
 
-  for (uint32_t tStart = millis(); (millis()-tStart) < calibTime; ) {
+  for (uint32_t tStart = millis(); (millis() - tStart) < calibTime;) {
 
     // Find max and min wing position
     for (int i = 0; i < sizeof(gCSN_PINS); i++) {
@@ -87,12 +80,10 @@ void GetAngleOffset() {
       if (i == 0) {
         maxPos[i] = newPos;
         minPos[i] = newPos;
-      }
-      else {
+      } else {
         if (newPos > maxPos[i]) {
           maxPos[i] = newPos;
-        }
-        else if (newPos < minPos[i]) {
+        } else if (newPos < minPos[i]) {
           minPos[i] = newPos;
         }
       }
@@ -101,7 +92,7 @@ void GetAngleOffset() {
 
   // Determine if offset is needed
   for (int i = 0; i < sizeof(gCSN_PINS); i++) {
-    else if (maxPos[i] - minPos[i] > ENC_PRECISION/2) {
+    if (maxPos[i] - minPos[i] > ENC_PRECISION / 2) {
       gAngleOffset[i] = 1;
     }
   }
@@ -116,7 +107,7 @@ unsigned int GetMaxWingPos() {
   unsigned int minPos[4];
   unsigned int newPos;
 
-  for (uint32_t tStart = millis(); (millis()-tStart) < calibTime; ) {
+  for (uint32_t tStart = millis(); (millis() - tStart) < calibTime;) {
 
     // Find max and min wing position
     for (int i = 0; i < sizeof(gCSN_PINS); i++) {
@@ -126,5 +117,4 @@ unsigned int GetMaxWingPos() {
       }
     }
   }
-
 }

controller/controller.ino

 /*******************************************************************************
-Controller
-
-Controller for the MechaRaptor tandem flapping wing system.
-
-Currently, both ESC are fed the same signal, which means that the two sets of
-wings flap at the same frequency. The phase between the two sets will be
-adjusted manually by setting their initial positions as desired.
---------------------------------------------------------------------------------
-(c) Copyright 2022 University of Liege
-Author: Thomas Lambert <t.lambert@uliege.be>
-ULiege - Aeroelasticity and Experimental Aerodynamics
-MIT License
-Repo: https://gitlab.uliege.be/thlamb/mecharaptor-controller
+  Controller
+
+  Controller for the MechaRaptor tandem flapping wing system.
+
+  Currently, both ESC are fed the same signal, which means that the two sets of
+  wings flap at the same frequency. The phase between the two sets will be
+  adjusted manually by setting their initial positions as desired.
+  ------------------------------------------------------------------------------
+  (c) Copyright 2022 University of Liege
+  Author: Thomas Lambert <t.lambert@uliege.be>
+  ULiege - Aeroelasticity and Experimental Aerodynamics
+  MIT License
+  Repo: https://gitlab.uliege.be/thlamb/mecharaptor-controller
 *******************************************************************************/
 
 
@@ -23,8 +23,7 @@ Repo: https://gitlab.uliege.be/thlamb/mecharaptor-controller
 /*******************************************************************************
   Macros (for pins and fixed components only!)
 *******************************************************************************/
-
-#define BAUD_RATE 115200 // baud rate for serial
+#define BAUD_RATE 115200  // baud rate for serial
 
 // Pins
 #define ESC1_PIN 10   // ESC Front motor
@@ -44,34 +43,35 @@ Repo: https://gitlab.uliege.be/thlamb/mecharaptor-controller
 
 // Values
 #define DIV_RES1 47800.0  // Resistance of R1 in the voltage dividers
-#define DIV_RES2 6800.0  // Resistance of R2 in the voltage dividers
+#define DIV_RES2 6800.0   // Resistance of R2 in the voltage dividers
 
 #define SEN0098_VCC 5         // Supply voltage of the current sensors
 #define SEN0098_SENSI 40.0    // Chip ACS758LCB-050B-PFF-T
 #define SEN0098_VIOUT 0.12    // [FIXME] VERIFY!
 #define SEN0098_OFFSET 0.007  // [FIXME] VERIFY!
 
-
 #define GEAR_RATIO 0.1  // Gear ratio between motor and wings
 
 // Calibration of encoders (NEEDS TO BE REDONE WHEN ENCODERS ARE MOVED)
-#define ENC_PRECISION 4096        // 12-Bits encoders (nb of points per rot)
-#define ENC_ORIENT {1, -1, 1, -1} // Encoder orientation
-#define MAX_WING_POS {222.1, 117.77, 117, 117} // Maximum wing angle (returned by encoder value)
-#define TRUE_MAX_ANGLE 38.07       // Maximum wing angle (measured on setup)
+#define ENC_PRECISION 4096  // 12-Bits encoders (nb of points per rot)
+#define ENC_ORIENT \
+  { 1, -1, 1, -1 }  // Encoder orientation
+#define MAX_WING_POS \
+  { 222.1, 117.77, 117, 117 }  // Maximum wing angle (returned by encoder value)
+#define TRUE_MAX_ANGLE 38.07   // Maximum wing angle (measured on setup)
+
 
 /*******************************************************************************
   Globals (for things that I can change manually)
-*******************************************************************************/
-
-const boolean gSKIP_CALIB = true; // Skip calibration or not
+ *******************************************************************************/
+const boolean gSKIP_CALIB = true;  // Skip calibration or not
 
 // Constant values that can be tweaked here or in the ESC before run
 const uint16_t gMAX_SIG = 2000;  // Max signal to the ESC [ms]
 const uint16_t gMIN_SIG = 1000;  // Min signal to the ESC [ms]
 
-const uint16_t gMAX_RPM = 3000;     // Motor maximum RPM (set in ESC)
-const uint16_t gMIN_RPM = 750;      // Motor minimum RPM (set in ESC)
+const uint16_t gMAX_RPM = 3000;    // Motor maximum RPM (set in ESC)
+const uint16_t gMIN_RPM = 750;     // Motor minimum RPM (set in ESC)
 const uint8_t gMAX_ESC_VAL = 160;  // Value at which motor is at max RPM
 const uint8_t gMIN_ESC_VAL = 40;   // Min value for ESC to start motor (trial-error)
 
@@ -84,167 +84,166 @@ const float gCUR_QOV = SEN0098_VIOUT * SEN0098_VCC;  // [FIXME] Verify
 Servo gEsc1;  // ESC for front motor
 Servo gEsc2;  // ESC for aft motor
 
-uint8_t gEsc_val = 0;  // ESC angle (to use with the Servo library) [0-180]
-String gMode;      // Mode of operation
-uint16_t gAngleOffset[] = {0, 0, 0, 0};    // Encoder offset (prevent wrap-around)
-uint16_t gMaxWingPos[] = MAX_WING_POS;    // Encoder maximum recorder position
-
+uint8_t gEsc_val = 0;                      // ESC angle (to use with the Servo library) [0-180]
+String gMode;                              // Mode of operation
+uint16_t gAngleOffset[] = { 0, 0, 0, 0 };  // Encoder offset (prevent wrap-around)
+uint16_t gMaxWingPos[] = MAX_WING_POS;     // Encoder maximum recorder position
 
 
 /*******************************************************************************
   ARDUINO SETUP
-*******************************************************************************/
+ *******************************************************************************/
 void setup() {
-    Serial.begin(BAUD_RATE);
-
-    // Init ESCs
-    Serial.print("[INFO]: Attaching ESCs ... ");
-    delay(5000);  // Prevents motor from starting automatically with serial
-    gEsc1.attach(ESC1_PIN, gMIN_SIG, gMAX_SIG);
-    gEsc2.attach(ESC2_PIN, gMIN_SIG, gMAX_SIG);
-    Serial.println("done!");
-
-    // Init switch
-    pinMode(SWITCH_PIN, INPUT);
-
-    // Init angle sensors
-    for (int i = 0; i < sizeof(gCSN_PINS); i++) {
-        pinMode(gCSN_PINS[i], OUTPUT);
-        digitalWrite(gCSN_PINS[i], HIGH);
-    }
-    pinMode(CLK_PIN, OUTPUT);
-    pinMode(DO_PIN, INPUT);
-    digitalWrite(CLK_PIN, HIGH);
-
-    // Initialize modes
-    int switch_state = digitalRead(SWITCH_PIN);
-    if (switch_state == LOW) {
-        gMode = "MANUAL";
-        Serial.println("[WARNING]: Control motors using potentiometer");
-
-    } else {
-        gEsc1.write(0);  // Must be initialized at 0 otherwise will not start
-        gEsc2.write(0);
+  Serial.begin(BAUD_RATE);
+
+  // Init ESCs
+  Serial.print("[INFO]: Attaching ESCs ... ");
+  delay(5000);  // Prevents motor from starting automatically with serial
+  gEsc1.attach(ESC1_PIN, gMIN_SIG, gMAX_SIG);
+  gEsc2.attach(ESC2_PIN, gMIN_SIG, gMAX_SIG);
+  Serial.println("done!");
+
+  // Init switch
+  pinMode(SWITCH_PIN, INPUT);
+
+  // Init angle sensors
+  for (int i = 0; i < sizeof(gCSN_PINS); i++) {
+    pinMode(gCSN_PINS[i], OUTPUT);
+    digitalWrite(gCSN_PINS[i], HIGH);
+  }
+  pinMode(CLK_PIN, OUTPUT);
+  pinMode(DO_PIN, INPUT);
+  digitalWrite(CLK_PIN, HIGH);
+
+  // Initialize modes
+  int switch_state = digitalRead(SWITCH_PIN);
+  if (switch_state == LOW) {
+    gMode = "MANUAL";
+    Serial.println("[WARNING]: Control motors using potentiometer");
+
+  } else {
+    gEsc1.write(0);  // Must be initialized at 0 otherwise will not start
+    gEsc2.write(0);
 
-        gMode = "SERIAL";
-        Serial.println("[WARNING]: Control motors using serial");
+    gMode = "SERIAL";
+    Serial.println("[WARNING]: Control motors using serial");
 
-        gEsc_val = SetupSerial();
-    }
+    gEsc_val = SetupSerial();
+  }
 
-    // Calibrate angle sensors
-    if (!gSKIP_CALIB) {
-      SetupCalibration();
-    }
+  // Calibrate angle sensors
+  if (!gSKIP_CALIB) {
+    Calibrate();
+  }
 
-    Serial.println("[INFO]: Setup OK, starting now...");
-    delay(1000);  // Ensure all is properly initialized
+  Serial.println("[INFO]: Setup OK, starting now...");
+  delay(1000);  // Ensure all is properly initialized
 }
 
 
 /*******************************************************************************
   ARDUINO LOOP
-    Control the system
-      - In serial mode, we impose values based on serial inputs
-      - In manual mode, we use the potentiometer
-    Acquisition
-      - Get values from the 4 rotary encoders
-*******************************************************************************/
+  Control the system
+  - In serial mode, we impose values based on serial inputs
+  - In manual mode, we use the potentiometer
+  Acquisition
+  - Get values from the 4 rotary encoders
+ *******************************************************************************/
 void loop() {
 
-    int potent = analogRead(POT_PIN);             // Potentiometer value
-    unsigned int wing_angles[sizeof(gCSN_PINS)];  // Wing angles (1L, 1R, 2L, 2R)
+  int potent = analogRead(POT_PIN);             // Potentiometer value
+  unsigned int wing_angles[sizeof(gCSN_PINS)];  // Wing angles (1L, 1R, 2L, 2R)
 
-    if (gMode == "SERIAL" and Serial.available()) {
+  if (gMode == "SERIAL" and Serial.available()) {
 
-        float input = Serial.parseFloat();
-        if (input <= 0) {
-            StopMotor(gEsc_val);
-        } else {
-            gEsc_val = FreqToEsc(input);
-        }
-
-    } else if (gMode == "MANUAL") {
-        gEsc_val = map(potent, 0, 1023, 0, 180);
+    float input = Serial.parseFloat();
+    if (input <= 0) {
+      StopMotor(gEsc_val);
+    } else {
+      gEsc_val = FreqToEsc(input);
     }
 
-    // Write value to ESCs
-    gEsc1.write(gEsc_val);
-    gEsc2.write(gEsc_val);
+  } else if (gMode == "MANUAL") {
+    gEsc_val = map(potent, 0, 1023, 0, 180);
+  }
 
-    // Read value from rotary encoders
-    for (int i = 0; i < sizeof(gCSN_PINS); i++) {
-        wing_angles[i] = ReadSensor(gCSN_PINS[i], gENC_ORIENT[i], gAngleOffset[i]);
-    }
+  // Write value to ESCs
+  gEsc1.write(gEsc_val);
+  gEsc2.write(gEsc_val);
+
+  // Read value from rotary encoders
+  for (int i = 0; i < sizeof(gCSN_PINS); i++) {
+    wing_angles[i] = ReadSensor(gCSN_PINS[i], gENC_ORIENT[i], gAngleOffset[i]);
+  }
 
-    // Read tension and current from PSU
-    float psu_cur[2], psu_vol[2];
-    GetPsu(TENS1_PIN, CUR1_PIN, psu_vol[0], psu_cur[0]);
-    GetPsu(TENS2_PIN, CUR2_PIN, psu_vol[1], psu_cur[1]);
+  // Read tension and current from PSU
+  float psu_cur[2], psu_vol[2];
+  GetPsu(TENS1_PIN, CUR1_PIN, psu_vol[0], psu_cur[0]);
+  GetPsu(TENS2_PIN, CUR2_PIN, psu_vol[1], psu_cur[1]);
 
-    // Output all to serial for treatment with serial-studio
-    SerialPrint(gMode, potent, gEsc_val, wing_angles, psu_vol, psu_cur);
+  // Output all to serial for treatment with serial-studio
+  SerialPrint(gMode, potent, gEsc_val, wing_angles, psu_vol, psu_cur);
 }
 
 
 /*******************************************************************************
   OTHER FUNCTIONS
-*******************************************************************************/
+ *******************************************************************************/
 
 // Setup serial
 // Get the initial value for the ESCs in serial mode
 float SetupSerial() {
-    float input;
-
-    float min_freq = RpmToFreq(gMIN_RPM);
-    float max_freq = RpmToFreq(gMAX_RPM);
-
-    Serial.print("[INPUT]: Input a flapping frequency between ");
-    Serial.print(min_freq);
-    Serial.print(" and ");
-    Serial.print(max_freq);
-    Serial.println(" [Hz]");
-
-    while (input > max_freq or input < min_freq) {
-        while (Serial.available() == 0) {}  // Wait for serial input
-
-        input = Serial.parseFloat();
-        if (input > max_freq or input < min_freq) {
-            Serial.print("[ERROR]: Frequency must be between ");
-            Serial.print(min_freq);
-            Serial.print(" and ");
-            Serial.print(max_freq);
-            Serial.print(" [Hz]. Entered: ");
-            Serial.println(input);
-        }
+  float input;
+
+  float min_freq = RpmToFreq(gMIN_RPM);
+  float max_freq = RpmToFreq(gMAX_RPM);
+
+  Serial.print("[INPUT]: Input a flapping frequency between ");
+  Serial.print(min_freq);
+  Serial.print(" and ");
+  Serial.print(max_freq);
+  Serial.println(" [Hz]");
+
+  while (input > max_freq or input < min_freq) {
+    while (Serial.available() == 0) {}  // Wait for serial input
+
+    input = Serial.parseFloat();
+    if (input > max_freq or input < min_freq) {
+      Serial.print("[ERROR]: Frequency must be between ");
+      Serial.print(min_freq);
+      Serial.print(" and ");
+      Serial.print(max_freq);
+      Serial.print(" [Hz]. Entered: ");
+      Serial.println(input);
     }
-    Serial.print("[INFO]: Frequency correctly set to ");
-    Serial.print(input);
-    Serial.println(" Hz.");
+  }
+  Serial.print("[INFO]: Frequency correctly set to ");
+  Serial.print(input);
+  Serial.println(" Hz.");
 
-    return FreqToEsc(input);
+  return FreqToEsc(input);
 }
 
 
 // Gracefully stops the motor in serial mode
 void StopMotor(int old_esc_val) {
 
-    Serial.println("[INFO] MOTOR STOP INITIATED");
+  Serial.println("[INFO] MOTOR STOP INITIATED");
 
-    if (old_esc_val > gMIN_ESC_VAL) {
-        Serial.println("[INFO] Slowing down motor for 3 sec...");
-        gEsc1.write(gMIN_ESC_VAL);
-        gEsc2.write(gMIN_ESC_VAL);
-        delay(3000);
-    }
+  if (old_esc_val > gMIN_ESC_VAL) {
+    Serial.println("[INFO] Slowing down motor for 3 sec...");
+    gEsc1.write(gMIN_ESC_VAL);
+    gEsc2.write(gMIN_ESC_VAL);
+    delay(3000);
+  }
 
-    gEsc1.write(0);
-    gEsc2.write(0);
-    Serial.println("[INFO] Motor stopped completely");
+  gEsc1.write(0);
+  gEsc2.write(0);
+  Serial.println("[INFO] Motor stopped completely");
 
-    // Lock system in infinite loop to prevent any restart
-    delay(1000);
-    while (1) {}
+  // Lock system in infinite loop to prevent any restart
+  delay(1000);
+  while (1) {}
 }
 
 
@@ -252,27 +251,27 @@ void StopMotor(int old_esc_val) {
 // Output data to use with serial-studio
 void SerialPrint(String mode, int potent, int esc_val, unsigned int wing_angles[], float psu_vol[], float psu_cur[]) {
 
-    float freq = esc_val * (RpmToFreq(gMAX_RPM) / 180.0);
-
-    Serial.print("/*");  // Beginning of data stream
-    Serial.print(mode);
+  float freq = esc_val * (RpmToFreq(gMAX_RPM) / 180.0);
+
+  Serial.print("/*");  // Beginning of data stream
+  Serial.print(mode);
+  Serial.print(",");
+  Serial.print(millis() / 1000.0);
+  Serial.print(",");
+  Serial.print(potent * 5.0 / 1023.0);  // Converts pot val into proper tension
+  Serial.print(",");
+  Serial.print(esc_val);
+  Serial.print(",");
+  Serial.print(freq);
+  for (int i = 0; i < sizeof(gCSN_PINS); i++) {
     Serial.print(",");
-    Serial.print(millis() / 1000.0);
+    Serial.print(PosToDeg(wing_angles[i]) - gMaxWingPos[i] + TRUE_MAX_ANGLE);
+  }
+  for (int i = 0; i < sizeof(psu_vol); i++) {
     Serial.print(",");
-    Serial.print(potent * 5.0 / 1023.0);  // Converts pot val into proper tension
+    Serial.print(psu_vol[i]);
     Serial.print(",");
-    Serial.print(esc_val);
-    Serial.print(",");
-    Serial.print(freq);
-    for (int i = 0; i < sizeof(gCSN_PINS); i++) {
-        Serial.print(",");
-        Serial.print(PosToDeg(wing_angles[i])-gMaxWingPos[i] + TRUE_MAX_ANGLE);
-    }
-    for (int i = 0; i < sizeof(psu_vol); i++) {
-        Serial.print(",");
-        Serial.print(psu_vol[i]);
-        Serial.print(",");
-        Serial.print(psu_cur[i]);
-    }
-    Serial.println("*/");  // End of data stream
+    Serial.print(psu_cur[i]);
+  }
+  Serial.println("*/");  // End of data stream
 }

controller/sensors.ino

 /*******************************************************************************
-Sensors
+  Sensors
 
-Functions for sensors and acquisition.
+  Functions for sensors and acquisition.
 
---------------------------------------------------------------------------------
-(c) Copyright 2022 University of Liege
-Author: Thomas Lambert <t.lambert@uliege.be>
-ULiege - Aeroelasticity and Experimental Aerodynamics
-MIT License
-Repo: https://gitlab.uliege.be/thlamb/mecharaptor-controller
+  --------------------------------------------------------------------------------
+  (c) Copyright 2022 University of Liege
+  Author: Thomas Lambert <t.lambert@uliege.be>
+  ULiege - Aeroelasticity and Experimental Aerodynamics
+  MIT License
+  Repo: https://gitlab.uliege.be/thlamb/mecharaptor-controller
 *******************************************************************************/
 
 //Read 12-bit rotary encoder (RobinL modified by linarism on forum.arduino.cc)
 unsigned int ReadSensor(uint8_t csn, int8_t orient, int8_t offset) {
-    unsigned int ret = 0;
-
-    digitalWrite(csn, LOW);
-    delayMicroseconds(1);  //Waiting for Tclkfe
-
-    //Passing 12 times, from 0 to 11
-    for (int x = 0; x < 12; x++) {
-        digitalWrite(CLK_PIN, LOW);
-        delayMicroseconds(1);  //Tclk/2
-        digitalWrite(CLK_PIN, HIGH);
-        delayMicroseconds(1);                    //Tdo valid, like Tclk/2
-        ret = (ret << 1) | digitalRead(DO_PIN);  //shift all the entering data to the left and past the pin state to it. 1e bit is MSB
-    }
-
-    digitalWrite(csn, HIGH);  //deselects the encoder from reading
-    return (1-orient)/2 * ENC_PRECISION + orient*ret - offset * ENC_PRECISION/2;
+  unsigned int ret = 0;
+
+  digitalWrite(csn, LOW);
+  delayMicroseconds(1);  //Waiting for Tclkfe
+
+  //Passing 12 times, from 0 to 11
+  for (int x = 0; x < 12; x++) {
+    digitalWrite(CLK_PIN, LOW);
+    delayMicroseconds(1);  //Tclk/2
+    digitalWrite(CLK_PIN, HIGH);
+    delayMicroseconds(1);                    //Tdo valid, like Tclk/2
+    ret = (ret << 1) | digitalRead(DO_PIN);  //shift all the entering data to the left and past the pin state to it. 1e bit is MSB
+  }
+
+  digitalWrite(csn, HIGH);  //deselects the encoder from reading
+  return (1 - orient) / 2 * ENC_PRECISION + orient * ret - offset * ENC_PRECISION / 2;
 }
 
 // Get the tension and current of a PSU
 void GetPsu(byte vol_pin, byte cur_pin, float &vol, float &cur) {
-    vol = GetTension(vol_pin);
-    cur = GetCurrent(cur_pin);
+  vol = GetTension(vol_pin);
+  cur = GetCurrent(cur_pin);
 }
 
 // Measure current consumed by module
 // https://www.youtube.com/watch?v=SiHfjzcqnU4
 // [FIXME] Check properly
 float GetCurrent(byte cur_pin) {
-    float volt_raw = (5.0 / 1023.0) * analogRead(cur_pin);
+  float volt_raw = (5.0 / 1023.0) * analogRead(cur_pin);
 
-    float volt = volt_raw * -gCUR_QOV + SEN0098_OFFSET;  // Trimming value to make current equal to 0 when no current
-    return volt / gCUR_FACT;                             // Actual current
+  float volt = volt_raw * -gCUR_QOV + SEN0098_OFFSET;  // Trimming value to make current equal to 0 when no current
+  return volt / gCUR_FACT;                             // Actual current
 }
 
 // Measure the tension provided by the PSU
 float GetTension(byte vol_pin) {
-    float vol_raw = analogRead(vol_pin) * (5.0 / 1023.0);
+  float vol_raw = analogRead(vol_pin) * (5.0 / 1023.0);
 
-    return vol_raw * (DIV_RES1 + DIV_RES2) / (DIV_RES2);
+  return vol_raw * (DIV_RES1 + DIV_RES2) / (DIV_RES2);
 }

controller/utils.ino

 /*******************************************************************************
-Utils
+  Utils
 
-Miscellaneous utilities.
+  Miscellaneous utilities.
 
---------------------------------------------------------------------------------
-(c) Copyright 2022 University of Liege
-Author: Thomas Lambert <t.lambert@uliege.be>
-ULiege - Aeroelasticity and Experimental Aerodynamics
-MIT License
-Repo: https://gitlab.uliege.be/thlamb/mecharaptor-controller
+  --------------------------------------------------------------------------------
+  (c) Copyright 2022 University of Liege
+  Author: Thomas Lambert <t.lambert@uliege.be>
+  ULiege - Aeroelasticity and Experimental Aerodynamics
+  MIT License
+  Repo: https://gitlab.uliege.be/thlamb/mecharaptor-controller
 *******************************************************************************/
 
 // Converts frequence to ESC angle to use with Servo library
 int FreqToEsc(float freq) {
 
-    // Need to scale times 100 so we can have better precision
-    int esc_val = freq * (180.0 / RpmToFreq(gMAX_RPM));
+  // Need to scale times 100 so we can have better precision
+  int esc_val = freq * (180.0 / RpmToFreq(gMAX_RPM));
 
-    if (esc_val < gMIN_ESC_VAL) {
-        esc_val = gMIN_ESC_VAL;
-    } else if (esc_val > gMAX_ESC_VAL) {
-        esc_val = gMAX_ESC_VAL;
-    }
+  if (esc_val < gMIN_ESC_VAL) {
+    esc_val = gMIN_ESC_VAL;
+  } else if (esc_val > gMAX_ESC_VAL) {
+    esc_val = gMAX_ESC_VAL;
+  }
 
-    return esc_val;
+  return esc_val;
 }
 
 // Converts RPM to Flapping frequency
 float RpmToFreq(int rpm) {
-    return rpm * GEAR_RATIO / 60.0;
+  return rpm * GEAR_RATIO / 60.0;
 }
 
 
@@ -37,3 +37,11 @@ float PosToDeg(unsigned int pos) {
   return pos * 360.0 / ENC_PRECISION;
 }
 
+void PrintArray(int array[]) {
+  for (int i = 0; i < sizeof(array); i++) {
+    Serial.print(array[i]);
+    if (i < sizeof(array) - 1) {
+      Serial.print(", ");
+    }
+  }
+}