change signs when computing least squares

robotology · Jan 12, 2023 · 23abf0a · 23abf0a
1 parent 299353e
commit 23abf0a
Show file tree

Hide file tree

Showing 2 changed files with 30 additions and 8 deletions.
diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/motorhal/encoder.c b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/motorhal/encoder.c
@@ -216,6 +216,17 @@ void encoderReset()
 uint16_t encoderGetElectricalAngle(void)
 {
 
+    uint32_t qenc = electricalOffset + (__HAL_TIM_GET_COUNTER(&htim2) * encoderConvFactor) & 0xFFFF;
+
+    static uint16_t cnt = 0;
+    if(++cnt % 10000 ==0)
+    {
+        char msg[64];
+        snprintf(msg, 64, "enc: %u, hall: %u", qenc, encoderForcedValue);
+        embot::core::print(msg);
+    }
+
+
     if (MainConf.encoder.resolution == 0)
     {
         return encoderForcedValue;
@@ -229,7 +240,7 @@ uint16_t encoderGetElectricalAngle(void)
         }
     }
 
-    return electricalOffset + (__HAL_TIM_GET_COUNTER(&htim2) * encoderConvFactor) & 0xFFFF;
+    return qenc;
 }
 
 uint16_t encoderGetUncalibrated(void)

diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/motorhal/pwm.c b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/motorhal/pwm.c
@@ -303,19 +303,28 @@ static uint8_t updateHallStatus(void)
                 {
                     calibration_step = 2;
 
-                    int32_t offset = int16_t(MainConf.pwm.hall_offset - minHallAngleDelta - border[0]);
-                    offset += int16_t(MainConf.pwm.hall_offset - minHallAngleDelta + hallAngleStep * Deg2iCub - border[1]);
-                    offset += int16_t(MainConf.pwm.hall_offset - minHallAngleDelta + 2 * hallAngleStep * Deg2iCub - border[2]);
-                    offset += int16_t(MainConf.pwm.hall_offset - minHallAngleDelta + 3 * hallAngleStep * Deg2iCub - border[3]);
-                    offset += int16_t(MainConf.pwm.hall_offset - minHallAngleDelta + 4 * hallAngleStep * Deg2iCub - border[4]);
-                    offset += int16_t(MainConf.pwm.hall_offset - minHallAngleDelta + 5 * hallAngleStep * Deg2iCub - border[5]);
+                    int32_t offset = 0;
+                    offset += int16_t(border[0] - MainConf.pwm.hall_offset + minHallAngleDelta);
+                    offset += int16_t(border[1] - MainConf.pwm.hall_offset + minHallAngleDelta - hallAngleStep * Deg2iCub);
+                    offset += int16_t(border[2] - MainConf.pwm.hall_offset + minHallAngleDelta - 2 * hallAngleStep * Deg2iCub);
+                    offset += int16_t(border[3] - MainConf.pwm.hall_offset + minHallAngleDelta - 3 * hallAngleStep * Deg2iCub);
+                    offset += int16_t(border[4] - MainConf.pwm.hall_offset + minHallAngleDelta - 4 * hallAngleStep * Deg2iCub);
+                    offset += int16_t(border[5] - MainConf.pwm.hall_offset + minHallAngleDelta - 5 * hallAngleStep * Deg2iCub);
 
                     offset /= numHallSectors;
 
                     embot::core::print("CALIBRATED\n");
 
-                    encoderCalibrate(-int16_t(offset));
+                    encoderCalibrate(int16_t(offset));
+
+                    char msg[128];
+                    snprintf(msg, 128, "s: %u, encu: %u, off: %d, hall: %u", sector_index, border[sector_index], int16_t(offset), angle);
+                    embot::core::print(msg);
                 }
+
+                char msg[128];
+                snprintf(msg, 128, "s: %u, encu: %u, hall: %u", sector_index, border[sector_index], angle);
+                embot::core::print(msg);
             }
             /*
             2) Update the forced value even if it is not used when the encoder is calibrated.
@@ -331,6 +340,8 @@ static uint8_t updateHallStatus(void)
             }
         }
     }
+
+
     // update the old sector and hall status
     sector_old = sector;
     hallStatus_old = hallStatus;