.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

CHANGELOG.md

@@ -9,10 +9,23 @@ to [Semantic Versioning][sem_ver].
 
 ### Added
 
+### Changed
+
+### Deprecated
+
+### Removed
+
+### Fixed
+
+## [v2.0.0] - 2022-11-23
+
+### Added
+
 - **Doc**: Changelog
 - **Controller**: Complete control over Serial
 - **Acquisition**: Angle sensor acquisition
-- **Acquisition**: PSU current and tension monitoring
+- **Acquisition**: PSU current and tension logging
+- **Calibration**: Wing angle automatic "zeroing"
 
 ### Changed
 
@@ -24,12 +37,6 @@ to [Semantic Versioning][sem_ver].
 - **All**: Use more precise types to limit RAM
 - **Refactor**: Split the code into different files
 
-### Deprecated
-
-### Removed
-
-### Fixed
-
 ## [v1.0.0] - 2022-10-25
 
 ### Added
@@ -39,5 +46,6 @@ to [Semantic Versioning][sem_ver].
 
 [sem_ver]:<https://semver.org/spec/v2.0.0.html>
 [keep_chglog]: <https://keepachangelog.com/en/1.0.0/>
-[Unreleased]: https://gitlab.uliege.be/thlamb/mecharaptor-controller/-/compare/v1.0.0...main
+[Unreleased]: https://gitlab.uliege.be/thlamb/mecharaptor-controller/-/compare/v2.0.0...main
+[v2.0.0]: https://gitlab.uliege.be/thlamb/mecharaptor-controller/-/releases/tag/v2.0.0
 [v1.0.0]: https://gitlab.uliege.be/thlamb/mecharaptor-controller/-/releases/tag/v1.0.0

README.md

@@ -7,8 +7,8 @@ dynamics and its impact on the performance of the aft wing.
 
 The system is actuated by two sets of brushless DC motors (BLDC), each
 controlled with an Engine Speed Controller (ESC). The ESCs are themselves
-controlled using an Arduino Uno, which acts as the "brain" of the whole system.
-This Arduino is the main interface for piloting the setup and logging important
+controlled using an Arduino Uno, which acts as the "brain" of the whole setup.
+This Arduino is the main interface for piloting the system and logging important
 metrics.
 
 This repository contains the Arduino code and the wiring schematics for the
@@ -22,20 +22,20 @@ electrical components of the setup.
   position of the wings.
 
 _Note: A maximum rotation speed is imposed to the BLDC by the ESC. This value
-has been manually set for each ESC using the ESC's terminal (JETIBOX). This
-value is reflected in the Arduino code as well, in so the inputs and outputs are
-calibrated properly._
+has been manually set for each ESC using the ESC's terminal (JETIBOX). The
+maximum BLDC speed is reflected in the Arduino code as well, so the inputs and
+outputs are calibrated properly._
 
 ## Usage
 
 Before powering the Arduino, set the ON/OFF switch in the appropriate position:
 
-- ON: Serial mode (control using a PC)
+- ON: Serial mode (control using a PC connected to the Arduino)
 - OFF: Manual mode (control using the potentiometer)
 
 Further switch changes during the Arduino runtime will not be accounted for.
 
-Then turn plug the ESCs power using batteries or DC power supply.
+Then turn on the ESCs power using batteries or DC power supply.
 
 If the setup is in manual mode, the potentiometer must be turned all the way
 down to initiate the setup. If it is in automatic mode, it will wait for a
@@ -68,36 +68,50 @@ re-open a new one.
 Besides controlling the system, the Arduino also logs the input data and some
 measurements. The most important data are:
 
-- ESC input setting
+- ESC input settings
 - Wing angles (for each wing)
 - PSU current and tension
 
-These logged data are transmitted through the serial connection and saved on the
+These data are transmitted through the serial connection and can be saved on the
 computer that controls the Arduino for later analysis.
 
 A [_json_ configuration file](controller/mechaRaptor.json) is provided in order
 to visualize the output stream in real time with [serial-studio]. This also
 allows the automatic logging and retention of all data in _csv_ spreadsheets.
 
+### Angle calibration
+
+The flapping angle of each wing is measured using an absolute encoder. These
+sensors need to be 'zeroed' properly so the value they output corresponds to
+the actual wing angle.
+This calibration process can be done automatically in Serial mode. It is only
+useful to calibrate the sensors after a re-assembly of the setup. Once the
+calibration has been made a first time, we recommend hard-coding the values
+measured and deactivate the calibration procedure altogether by putting
+`gSKIP_CALIB = true`.
+
 ## Bill of Materials
 
-| Device            | Reference    | Usage    |
-|------------------ | --------------- | --------------- |
-| **BLDC**          | [Plettenberg][plettenberg] Advance 30 <br />_(Discontinued by manufacturer)_  | Motor |
-| **ESC**           | [JETI SPIN 75 Pro Opto][jetispin] | Motor controller |
-| **ESC Terminal**  | [JETIBOX][jetibox] | Program ESC, motor telemetry |
-| **PSU**           | [Electroautomatik][elektro] PSI 8080 - 60 T (1500W) <br />_(Discontinued by manufacturer)_ | DC power supply |
-| **Controller board**  | [Arduino uno (Rev 3)][arduino] | Full setup control, data logging|
-| **Encoders**      | [Broadcom AEAT 6012][AEAT612] | Flapping angle measurement |
-| **Potentiometer** | N/A | Manual control of RPM|
-| **ON/OFF switch** | N/A | Switch between `Serial` and `Manual` control |
-| **Resistors** | N/A | Voltage divider, noise reduction |
-| **Current sensor** | [Allegro SEN0098][SEN0098] | PSU current logging |
+| Device               | Reference                                               | Usage            |
+|--------------------- | ------------------------------------------------------- | ---------------- |
+| **BLDC**             | [Plettenberg][plettenberg] Advance 30[^1]               | Motor            |
+| **ESC**              | [JETI SPIN 75 Pro Opto][jetispin]                       | Motor controller |
+| **ESC Terminal**     | [JETIBOX][jetibox]                                      | Program ESC, motor telemetry |
+| **PSU**              | [Electroautomatik][elektro] PSI 8080 - 60 T (1500W)[^1] | DC power supply |
+| **Controller board** | [Arduino uno (Rev 3)][arduino]                          | Full setup control, data logging|
+| **Encoders**         | [Broadcom AEAT 6012][AEAT612]                           | Flapping angle measurement |
+| **Potentiometer**    | N/A                                                     | Manual control of RPM|
+| **ON/OFF switch**    | N/A                                                     | Switch between `Serial` and `Manual` control |
+| **Resistors**        | 47 k$`\Omega`$, 6.8 k$`\Omega`$                         | Voltage divider & noise reduction |
+| **Current sensor**   | [Allegro SEN0098][SEN0098][^2]                          | PSU current logging |
 
 ## Schematics
 
 To Do (ETA January 2023)
 
+[^1]: Discontinued by manufacturer.
+[^2]: Using an ACS758 chip LCB050B-3370XAA-1044.
+
 [plettenberg]: <https://plettenbergmotors.com/>
 [serial-studio]: <https://serial-studio.github.io/>
 [jetispin]: <https://www.jetimodel.com/katalog/spin-75-pro-opto.htm>
@@ -105,4 +119,4 @@ To Do (ETA January 2023)
 [arduino]: <https://store.arduino.cc/products/arduino-uno-rev3>
 [elektro]: <https://elektroautomatik.de>
 [AEAT612]: <https://docs.broadcom.com/doc/AV02-0188EN>
-[SEN0098]: https://www.allegromicro.com/~/media/Files/Datasheets/ACS758-Datasheet.ashx
+[SEN0098]: <https://www.allegromicro.com/~/media/Files/Datasheets/ACS758-Datasheet.ashx>

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
+*******************************************************************************/
+
+void Calibrate() {
+  String input;
+  uint32_t calibTime = 6000;  // Calibration time in ms
+
+  Serial.println("[INPUT]: Calibrate angles? (Y/n)");
+
+  while (Serial.available() == 0) {}  // Wait for serial input
+  input = Serial.readString();
+
+  if (input.equalsIgnoreCase("n") or input.equalsIgnoreCase("no")) {
+    Serial.println("[INFO]: No calibration required. Using default values.");
+
+  } else {
+
+    Serial.print("[WARNING]: Setup will run for ");
+    Serial.print(calibTime / 1000);
+    Serial.println("s to get the offset and maximum value.");
+    Serial.println("[INPUT]: Type 'Y' to begin.");
+
+    while (Serial.available() == 0) {}  // Wait for serial input
+
+    if (input.equalsIgnoreCase("y")) {
+
+      // Run ESC at min value
+      gEsc1.write(40);
+      gEsc2.write(40);
+
+      GetAngleOffset(calibTime / 2);
+      GetMaxWingPos(calibTime / 2);
+
+      // Stop ESC
+      gEsc1.write(0);
+      gEsc2.write(0);
+
+      Serial.println("[INFO]: Calibration done.");
+      Serial.println("Maxium wing posiions are: ");
+      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();
+  }
+}
+
+// Determine if wing position needs to be offset to prevent wrap-around
+void GetAngleOffset(uint32_t calibTime) {
+
+  unsigned int maxPos[4];
+  unsigned int minPos[4];
+  unsigned int newPos;
+
+  for (uint32_t tStart = millis(); (millis() - tStart) < calibTime;) {
+
+    // Find max and min wing position
+    for (int i = 0; i < sizeof(gCSN_PINS); i++) {
+      newPos = ReadSensor(gCSN_PINS[i], gENC_ORIENT[i], 0);
+      if (i == 0) {
+        maxPos[i] = newPos;
+        minPos[i] = newPos;
+      } else {
+        if (newPos > maxPos[i]) {
+          maxPos[i] = newPos;
+        } else if (newPos < minPos[i]) {
+          minPos[i] = newPos;
+        }
+      }
+    }
+  }
+
+  // Determine if offset is needed
+  for (int i = 0; i < sizeof(gCSN_PINS); i++) {
+    if (maxPos[i] - minPos[i] > ENC_PRECISION / 2) {
+      gAngleOffset[i] = 1;
+    }
+  }
+}
+
+
+unsigned int GetMaxWingPos(uint32_t calibTime) {
+
+  unsigned int newPos;
+
+  for (uint32_t tStart = millis(); (millis() - tStart) < calibTime;) {
+
+    // Find max and min wing position
+    for (int i = 0; i < sizeof(gCSN_PINS); i++) {
+      newPos = ReadSensor(gCSN_PINS[i], gENC_ORIENT[i], gAngleOffset[i]);
+      if (newPos > gMaxWingPos[i]) {
+        gMaxWingPos[i] = newPos;
+      }
+    }
+  }
+}

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
@@ -43,202 +42,210 @@ Repo: https://gitlab.uliege.be/thlamb/mecharaptor-controller
 #define TENS2_PIN A1  // Tension measurement for Aft motor
 
 // 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 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 DIV_RES1 48254.0  // Resistance of R1 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_QUIESCENT_FACTOR 0.5  // Bidirectional chip-> V_IOUTQ = 0.5*VCC
+#define SEN0098_OFFSET 0.0122         // Manually set so current is 0 when no current
 
 #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 = false;  // 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)
 
-const uint8_t gCSN_PINS[] = CSN_PINS;                // Encoder Chip Select pins
-const int8_t gENC_ORIENT[] = ENC_ORIENT;             // Encoder orientation
-const float gCUR_FACT = SEN0098_SENSI / 1000.0;      // Factor for current measurement
-const float gCUR_QOV = SEN0098_VIOUT * SEN0098_VCC;  // [FIXME] Verify
+const uint8_t gCSN_PINS[] = CSN_PINS;     // Encoder Chip Select pins
+const int8_t gENC_ORIENT[] = ENC_ORIENT;  // Encoder orientation
+
+const float gCUR_FACT = SEN0098_SENSI / 1000.0;                 // Factor for current measurement
+const float gCUR_QOV = SEN0098_QUIESCENT_FACTOR * SEN0098_VCC;  // VIOUT_Q
 
 // Other
 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 max_wing_pos[] = 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
-    // SetupCalibration();
+  // Calibrate angle sensors
+  if (!gSKIP_CALIB) {
+    Calibrate();
+  }
 
-    Serial.println("[INFO]: Setup OK, starting now...");
-    delay(1000);  // Ensure all is properly initialized
+  delay(1000);
+  Serial.println("[INFO]: Setup OK, starting now...");
+  delay(500);
 }
 
 
 /*******************************************************************************
   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)
-
-    if (gMode == "SERIAL" and Serial.available()) {
+  int potent = analogRead(POT_PIN);             // Potentiometer value
+  unsigned int wing_angles[sizeof(gCSN_PINS)];  // Wing angles (1L, 1R, 2L, 2R)
 
-        float input = Serial.parseFloat();
-        if (input <= 0) {
-            StopMotor(gEsc_val);
-        } else {
-            gEsc_val = FreqToEsc(input);
-        }
+  if (gMode == "SERIAL" and Serial.available()) {
 
-    } 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]);
-    }
+  // 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) {}
 }
 
 
@@ -246,27 +253,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]) % ENC_PRECISION) + TRUE_MAX_ANGLE - 360);
+  }
+  for (int i = 0; i < 2; 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])-max_wing_pos[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) {
-    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;
+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);
 }
 
 // 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
+// From https://www.youtube.com/watch?v=SiHfjzcqnU4
 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
-
-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
+  Utils
+
+  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
 *******************************************************************************/
 
 // 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 +36,12 @@ float PosToDeg(unsigned int pos) {
   return pos * 360.0 / ENC_PRECISION;
 }
 
+void PrintArray(int array[]) {
+  for (int i = 0; i < 4; i++) {
+    Serial.print(array[i]);
+    if (i < 4 - 1) {
+      Serial.print(", ");
+    }
+  }
+  Serial.println();
+}