diff --git a/CHANGELOG.md b/CHANGELOG.md
index fb7b6dc960e37f8421d2e0cdac4551a06a46e037..9e29969348fa9d8877b963e7b23022173b6e3457 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -13,6 +13,7 @@ to [Semantic Versioning][sem_ver].
 - **Controller**: Complete control over Serial
 - **Acquisition**: Angle sensor acquisition
 - **Acquisition**: PSU current and tension monitoring
+- **Calibration**: Wing angle automatic calibration
 
 ### Changed
 
diff --git a/README.md b/README.md
index 4f9510943699f07668b19d8ebb2efdd44f98feb6..1afb415b4dd42f7ec6dde1bec78ee6d2cdf03058 100644
--- a/README.md
+++ b/README.md
@@ -79,6 +79,18 @@ 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 'calibrated' properly so the value they output corresponds to
+the actual wing angle.
+This calibration process be done automatically in Serial mode. At the end of the
+calibration, the Serial console will output the offset and the max angle values
+measured. This calibration is only necessary for the first run of the setup (or
+after a re-assembly). So the values measured during the calibration can be
+hard-coded in the code and the calibration functions can be by-passed by setting
+`gSKIP_CALIB = true`.
+
 ## Bill of Materials
 
 | Device            | Reference    | Usage    |
@@ -92,12 +104,14 @@ allows the automatic logging and retention of all data in _csv_ spreadsheets.
 | **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 |
+| **Current sensor** | [Allegro SEN0098][SEN0098][^1] | PSU current logging |
 
 ## Schematics
 
 To Do (ETA January 2023)
 
+[^1]: 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>
diff --git a/controller/calibration.ino b/controller/calibration.ino
new file mode 100644
index 0000000000000000000000000000000000000000..afde8ded3b053818bd379530c40f5175a080b889
--- /dev/null
+++ b/controller/calibration.ino
@@ -0,0 +1,130 @@
+/*******************************************************************************
+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;
+
+  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.println("[WARNING]: Setup will run for 8s 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();
+      GetMaxWingPos();
+
+      // Stop ESC
+      gEsc1.write(0);
+      gEsc2.write(0);
+
+      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(", ");
+        }
+      }
+
+    } 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() {
+
+  uint16_t calibTime = 4000;
+
+  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++) {
+    else if (maxPos[i] - minPos[i] > ENC_PRECISION/2) {
+      gAngleOffset[i] = 1;
+    }
+  }
+}
+
+
+unsigned int GetMaxWingPos() {
+
+  uint16_t calibTime = 4000;
+
+  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], gAngleOffset[i]);
+      if (newPos > maxPos[i]) {
+        gMaxWingPos[i] = newPos;
+      }
+    }
+  }
+
+}
diff --git a/controller/controller.ino b/controller/controller.ino
index d95f95f98a48fd3d66f5fc959548ed8edad41f39..4317a67beea9792477d5da62a6991bf645928c89 100644
--- a/controller/controller.ino
+++ b/controller/controller.ino
@@ -64,6 +64,8 @@ Repo: https://gitlab.uliege.be/thlamb/mecharaptor-controller
   Globals (for things that I can change manually)
 *******************************************************************************/
 
+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]
@@ -84,7 +86,9 @@ 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
+uint16_t gAngleOffset[] = {0, 0, 0, 0};    // Encoder offset (prevent wrap-around)
+uint16_t gMaxWingPos[] = MAX_WING_POS;    // Encoder maximum recorder position
+
 
 
 /*******************************************************************************
@@ -129,7 +133,9 @@ void setup() {
     }
 
     // Calibrate angle sensors
-    // SetupCalibration();
+    if (!gSKIP_CALIB) {
+      SetupCalibration();
+    }
 
     Serial.println("[INFO]: Setup OK, starting now...");
     delay(1000);  // Ensure all is properly initialized
@@ -168,7 +174,7 @@ void loop() {
 
     // Read value from rotary encoders
     for (int i = 0; i < sizeof(gCSN_PINS); i++) {
-        wing_angles[i] = ReadSensor(gCSN_PINS[i], gENC_ORIENT[i]);
+        wing_angles[i] = ReadSensor(gCSN_PINS[i], gENC_ORIENT[i], gAngleOffset[i]);
     }
 
     // Read tension and current from PSU
@@ -260,7 +266,7 @@ void SerialPrint(String mode, int potent, int esc_val, unsigned int wing_angles[
     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);
+        Serial.print(PosToDeg(wing_angles[i])-gMaxWingPos[i] + TRUE_MAX_ANGLE);
     }
     for (int i = 0; i < sizeof(psu_vol); i++) {
         Serial.print(",");
diff --git a/controller/sensors.ino b/controller/sensors.ino
index f847951a6452f461a648b8b125cdec2f0c85d607..dfa641eda62a018e281eb8d31110ff7aa3e20fc0 100644
--- a/controller/sensors.ino
+++ b/controller/sensors.ino
@@ -12,7 +12,7 @@ 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 ReadSensor(uint8_t csn, int8_t orient, int8_t offset) {
     unsigned int ret = 0;
 
     digitalWrite(csn, LOW);
@@ -28,7 +28,7 @@ unsigned int ReadSensor(uint8_t csn, int8_t orient) {
     }
 
     digitalWrite(csn, HIGH);  //deselects the encoder from reading
-    return (1-orient)/2 * ENC_PRECISION + orient*ret;
+    return (1-orient)/2 * ENC_PRECISION + orient*ret - offset * ENC_PRECISION/2;
 }
 
 // Get the tension and current of a PSU