diff --git a/controller/controller.ino b/controller/controller.ino
index d0757babba568f2b6ab118b5b1a92dfc0546a2d1..e0ee124df9b557a360ee0c4a2147587117e55b03 100644
--- a/controller/controller.ino
+++ b/controller/controller.ino
@@ -45,10 +45,10 @@
 #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 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.007          // [FIXME] VERIFY!
 
 #define GEAR_RATIO 0.1  // Gear ratio between motor and wings
 
@@ -75,10 +75,11 @@ 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
@@ -136,8 +137,9 @@ void setup() {
     Calibrate();
   }
 
+  delay(1000);
   Serial.println("[INFO]: Setup OK, starting now...");
-  delay(1000);  // Ensure all is properly initialized
+  delay(500);
 }
 
 
@@ -265,9 +267,9 @@ 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]) - gMaxWingPos[i] + TRUE_MAX_ANGLE);
+    Serial.print(PosToDeg((wing_angles[i] - gMaxWingPos[i]) % ENC_PRECISION) + TRUE_MAX_ANGLE - 360);
   }
-  for (int i = 0; i < sizeof(psu_vol) / sizeof(psu_vol[0]); i++) {
+  for (int i = 0; i < 2; i++) {
     Serial.print(",");
     Serial.print(psu_vol[i]);
     Serial.print(",");
diff --git a/controller/sensors.ino b/controller/sensors.ino
index 6fbab66e7b57e26727d3bcf6aa141597f903cd4e..f206b3daf110ef39f7f5e154b502ffb64f9512e7 100644
--- a/controller/sensors.ino
+++ b/controller/sensors.ino
@@ -28,7 +28,7 @@ unsigned int ReadSensor(uint8_t csn, int8_t orient, int8_t offset) {
   }
 
   digitalWrite(csn, HIGH);  //deselects the encoder from reading
-  return (1 - orient) / 2 * ENC_PRECISION + orient * ret - offset * ENC_PRECISION / 2;
+  return (1 - orient) / 2 * ENC_PRECISION + orient * ret - (offset * ENC_PRECISION / 2);
 }
 
 // Get the tension and current of a PSU
@@ -38,8 +38,7 @@ void GetPsu(byte vol_pin, byte cur_pin, float &vol, float &cur) {
 }
 
 // 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);
 
diff --git a/controller/utils.ino b/controller/utils.ino
index 6d3f3cbe8c5886bb6409b7d1c4a82b2d5b9d047a..ef39dc39fc424821e10669a99ff3ad9d69591d1e 100644
--- a/controller/utils.ino
+++ b/controller/utils.ino
@@ -37,10 +37,11 @@ float PosToDeg(unsigned int pos) {
 }
 
 void PrintArray(int array[]) {
-  for (int i = 0; i < sizeof(array) / sizeof(array[0]); i++) {
+  for (int i = 0; i < 4; i++) {
     Serial.print(array[i]);
-    if (i < sizeof(array) - 1) {
+    if (i < 4 - 1) {
       Serial.print(", ");
     }
   }
+  Serial.println();
 }