add field weakening for command values >1000

Inc/config.h

@@ -98,6 +98,9 @@
 //#define INVERT_L_DIRECTION
 #define BEEPS_BACKWARD 0    // 0 or 1
 
+#define MAXWEAK 400 //maximum field weakening value
+#define MAXCOMMANDWEAK 400 //1000+MAXCOMMANDWEAK is maximum command speed with field weakening at MAXWEAK
+
 //Turbo boost at high speeds while button1 is pressed:
 //#define ADDITIONAL_CODE \
 if (button1 && speedR > 700) { /* field weakening at high speeds */ \
@@ -138,6 +141,7 @@ else {\
   weakl = 0;\
   weakr = 0;
 
+
 // ############################### VALIDATE SETTINGS ###############################
 
 #if defined CONTROL_SERIAL_USART2 && defined CONTROL_ADC

Src/main.c

@@ -64,6 +64,9 @@ extern volatile int pwmr;  // global variable for pwm right. -1000 to 1000
 extern volatile int weakl; // global variable for field weakening left. -1000 to 1000
 extern volatile int weakr; // global variable for field weakening right. -1000 to 1000
 
+float _f_weakl; //for slow start field weakening
+float _f_weakr; //for slow start field weakening
+
 extern uint8_t buzzerFreq;    // global variable for the buzzer pitch. can be 1, 2, 3, 4, 5, 6, 7...
 extern uint8_t buzzerPattern; // global variable for the buzzer pattern. can be 1, 2, 3, 4, 5, 6, 7...
 
@@ -233,14 +236,31 @@ int main(void) {
       		timeout = 0; //values ok, reset timeout
       	}
 
-        cmd1 = CLAMP((int16_t)command.speedl, -1000, 1000);
-        cmd2 = CLAMP((int16_t)command.speedr, -1000, 1000);
+
+
+
+
+        //cmd1 = CLAMP((int16_t)command.speedl, -1000, 1000);
+        //cmd2 = CLAMP((int16_t)command.speedr, -1000, 1000);
+
+        cmd1 = (int16_t)command.speedl; //gets clamped after filtering
+        cmd2 = (int16_t)command.speedr; //gets clamped after filtering
+
         if (command.checksum==0){ //received checksum 0 disables motors
           enable=0;
+          _f_weakl=0;
+          _f_weakr=0;
         }else{ //checksum ok and not intended to disable motors
           enable=1;
         }
+      }else{ //wrong checksum
+        _f_weakl=0;
+        _f_weakr=0;
       }
+
+
+
+
       command.checksum=255; //set 255 as flag "values read"
     #endif
 
@@ -269,6 +289,25 @@ int main(void) {
     speed = (speedL+speedR)/2; //calculate speed. needed for some checks
 
 
+    //## Field weakening ##
+    //FIELD WEAKENING L
+    if (speedLfiltering > 1000) {  //use unclamped speed value
+      weakl = CLAMP(speedLfiltering-1000,0,MAXWEAK);  // slowly add field weakening(turbo), 12s: 400=29kmh
+    }else{
+      weakl=0;
+    }
+     //FIELD WEAKENING R
+    if (speedRfiltering > 1000) {  //use unclamped speed value
+      weakr = CLAMP(speedRfiltering-1000,0,MAXWEAK);  // slowly add field weakening(turbo), 12s: 400=29kmh
+    }else{
+      weakr=0;
+    }
+
+    weakl=(int)CLAMP(weakl,0,MAXWEAK);
+    weakr=(int)CLAMP(weakr,0,MAXWEAK);
+
+
+
     #ifdef ADDITIONAL_CODE
       ADDITIONAL_CODE;
     #endif