2foc.3.3.38: solves the overcurrent problem (#551)

robotology · Jan 9, 2025 · cf69040 · cf69040
1 parent d407b75
commit cf69040
Show file tree

Hide file tree

Showing 4 changed files with 10 additions and 23 deletions.
diff --git a/emBODY/eBcode/arch-dspic/board/2foc/appl/2FOC-V3/include/can_icubProto.h b/emBODY/eBcode/arch-dspic/board/2foc/appl/2FOC-V3/include/can_icubProto.h
@@ -29,7 +29,7 @@
 /************ FIRMWARE AND CAN PROTOCOL VERSION DEFINITION *******************************************/
 #define FW_VERSION_MAJOR          3
 #define FW_VERSION_MINOR          3
-#define FW_VERSION_BUILD          37
+#define FW_VERSION_BUILD          38
 
 #define CAN_PROTOCOL_VERSION_MAJOR      1
 #define CAN_PROTOCOL_VERSION_MINOR      6

diff --git a/emBODY/eBcode/arch-dspic/board/2foc/appl/2FOC-V3/src/2FOC.c b/emBODY/eBcode/arch-dspic/board/2foc/appl/2FOC-V3/src/2FOC.c
@@ -525,8 +525,6 @@ int alignRotor(volatile int* IqRef)
 
 // DMA0 IRQ Service Routine used for FOC loop
 
-volatile int Iafbk = 0;
-volatile int Icfbk = 0;
 volatile int ElDegfbk = 0;
 volatile int CurLimfbk = 0;
 volatile int VqFbk = 0;
@@ -558,30 +556,21 @@ void __attribute__((__interrupt__, no_auto_psv)) _DMA0Interrupt(void)
     // read and compensate ADC offset by MeasCurrParm.Offseta, Offsetc
     // scale currents by MeasCurrParm.qKa, qKc
 
-    short Ia_raw = (int)(__builtin_mulss(MeasCurrParm.Offseta-ADCBuffer[0],MeasCurrParm.qKa)>>14); // TEST 
-    short Ic_raw = (int)(__builtin_mulss(MeasCurrParm.Offsetc-ADCBuffer[1],MeasCurrParm.qKc)>>14); // TEST
+    //ParkParm.qIa = (int)(__builtin_mulss(MeasCurrParm.Offseta-ADCBuffer[0],MeasCurrParm.qKa)>>14); // TEST 
+    //ParkParm.qIc = (int)(__builtin_mulss(MeasCurrParm.Offsetc-ADCBuffer[1],MeasCurrParm.qKc)>>14); // TEST
 
     // Ix_raw = 64 is equal to 49.03 mA current here
     // since we have 10 bits resolution, left aligned, with LSB = 49.03 mA
 
     // gain = 64/49.03
 
-    static short Ia_raw_old = 0, Ic_raw_old = 0;
-
     // here we have a first stage filtering
     // each sample is mediated with the previous one
-    ParkParm.qIa = (Ia_raw+Ia_raw_old)/6;
-    ParkParm.qIc = (Ic_raw+Ic_raw_old)/6;
-
-    Ia_raw_old = Ia_raw;
-    Ic_raw_old = Ic_raw;
-
-    // gain = (64/49.03) * (2/3)
-
-    Iafbk = Ia_raw;
-    Icfbk = Ic_raw;
-
+    ParkParm.qIa = (MeasCurrParm.Offseta-ADCBuffer[0]) / 3;
+    ParkParm.qIc = (MeasCurrParm.Offsetc-ADCBuffer[1]) / 3;
     ParkParm.qIb = -ParkParm.qIa-ParkParm.qIc;
+
+    // gain = (64/49.03) * (1/3)
 
     int enc = 0;
     static int enc_start_sec = 0;
@@ -1160,9 +1149,9 @@ void EnableDrive()
 {
     bDriveEnabled = 1;
 
+#if 0
     static BOOL uncalibrated = TRUE;
 
-#if 1
     if (uncalibrated)
     {
         uncalibrated = FALSE;

diff --git a/emBODY/eBcode/arch-dspic/board/2foc/appl/2FOC-V3/src/ADC.c b/emBODY/eBcode/arch-dspic/board/2foc/appl/2FOC-V3/src/ADC.c
@@ -104,8 +104,8 @@ int ADCCalibrateOffset(void)
     long ADCVdcZero = 0;
 
     // Put an 'accettable' value in case of error in the calibration process
-    MeasCurrParm.Offseta = 0x200;
-    MeasCurrParm.Offsetc = 0x200;
+    MeasCurrParm.Offseta = 0;
+    MeasCurrParm.Offsetc = 0;
 
     // Turn on ADC module
     AD1CON1bits.ADON = 1;

diff --git a/emBODY/eBcode/arch-dspic/board/2foc/appl/2FOC-V3/src/can_icubProto_trasmitter.c b/emBODY/eBcode/arch-dspic/board/2foc/appl/2FOC-V3/src/can_icubProto_trasmitter.c
@@ -57,8 +57,6 @@ extern volatile char IKs;
 
 extern volatile tMotorConfig MotorConfig;
 
-extern volatile int Iafbk;
-extern volatile int Icfbk;
 extern volatile int ElDegfbk;
 extern volatile int iQprot;
 extern volatile int maxCountfbk;