feat: PSU sensor and angle calibration

controller/controller.ino

@@ -35,22 +35,28 @@ Repo: https://gitlab.uliege.be/thlamb/mecharaptor-controller
 #define CSN_PINS \
   { 6, 5, 4, 3 }  // Encoder Chip Select pins
 
-#define CUR1_PIN A1   // Current measurement for Front motor
+#define CUR1_PIN A4   // Current measurement for Front motor
 #define CUR2_PIN A2   // Current measurement for Aft motor
 #define TENS1_PIN A3  // Tension measurement for Front motor
-#define TENS2_PIN A4  // Tension measurement for Aft motor
+#define TENS2_PIN A1  // Tension measurement for Aft motor
 
 // Values
-#define DIV_RES1 9000.0  // Resistance of R1 in the voltage dividers
-#define DIV_RES2 1000.0  // Resistance of R2 in the voltage dividers
+#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 20.0    // [FIXME] VERIFY!
 #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 37.9       // Maximum wing angle (measured on setup)
 
 /*******************************************************************************
   Globals (for things that I can change manually)
@@ -66,6 +72,8 @@ 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 uint16_t gMAX_WING_POS[] = MAX_WING_POS;       // Encoder maximum recorder position
 const float gCUR_FACT = SEN0098_SENSI / 1000.0;      // Factor for current measurement
 const float gCUR_QOV = SEN0098_VIOUT * SEN0098_VCC;  // [FIXME] Verify
 
@@ -75,6 +83,7 @@ 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[4];      // Encoder maximum recorder position
 
 
 /*******************************************************************************
@@ -134,7 +143,7 @@ void setup() {
 void loop() {
 
     int potent = analogRead(POT_PIN);             // Potentiometer value
-    uint16_t wing_angles[sizeof(gCSN_PINS)];  // Wing angles (1L, 1R, 2L, 2R)
+    unsigned int wing_angles[sizeof(gCSN_PINS)];  // Wing angles (1L, 1R, 2L, 2R)
 
     if (gMode == "SERIAL" and Serial.available()) {
 
@@ -155,7 +164,7 @@ void loop() {
 
     // Read value from rotary encoders
     for (int i = 0; i < sizeof(gCSN_PINS); i++) {
-        wing_angles[i] = ReadSensor(gCSN_PINS[i]);
+        wing_angles[i] = ReadSensor(gCSN_PINS[i], gENC_ORIENT[i]);
     }
 
     // Read tension and current from PSU
@@ -251,8 +260,8 @@ void StopMotor(int old_esc_val) {
 
 
 //Read 12-bit rotary encoder (RobinL modified by linarism on forum.arduino.cc)
-uint16_t ReadSensor(uint8_t csn) {
-    uint16_t ret = 0;
+unsigned int ReadSensor(uint8_t csn, int8_t orient) {
+    unsigned int ret = 0;
 
     digitalWrite(csn, LOW);
     delayMicroseconds(1);  //Waiting for Tclkfe
@@ -267,12 +276,12 @@ uint16_t ReadSensor(uint8_t csn) {
     }
 
     digitalWrite(csn, HIGH);  //deselects the encoder from reading
-    return ret;
+    return (1-orient)/2 * ENC_PRECISION + orient*ret;
 }
 
 
 // Output data to use with serial-studio
-void SerialPrint(String mode, int potent, int esc_val, uint16_t wing_angles[], float psu_vol[], float psu_cur[]) {
+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);
 
@@ -288,7 +297,7 @@ void SerialPrint(String mode, int potent, int esc_val, uint16_t wing_angles[], f
     Serial.print(freq);
     for (int i = 0; i < sizeof(gCSN_PINS); i++) {
         Serial.print(",");
-        Serial.print(wing_angles[i]);
+        Serial.print(PosToDeg(wing_angles[i])-gMAX_WING_POS[i] + TRUE_MAX_ANGLE);
     }
     for (int i = 0; i < sizeof(psu_vol); i++) {
         Serial.print(",");
@@ -317,7 +326,15 @@ float GetCurrent(byte cur_pin) {
 
 // Measure the tension provided by the PSU
 float GetTension(byte vol_pin) {
-    int 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);
 }
+
+// Convert encoder position to degree
+float PosToDeg(unsigned int pos){
+
+  return pos * 360.0 / ENC_PRECISION;
+
+}
+