changes for independant left and right speed serial control

Inc/config.h

@@ -61,11 +61,11 @@
  * Enable warning and/or poweroff and make and flash firmware.
  */
 #define TEMP_FILT_COEF          655       // temperature filter coefficient in fixed-point. coef_fixedPoint = coef_floatingPoint * 2^16. In this case 655 = 0.01 * 2^16
-#define TEMP_CAL_LOW_ADC        1655      // temperature 1: ADC value
+#define TEMP_CAL_LOW_ADC        1716      // temperature 1: ADC value
-#define TEMP_CAL_LOW_DEG_C      358       // temperature 1: measured temperature [°C * 10]. Here 35.8 °C
+#define TEMP_CAL_LOW_DEG_C      210       // temperature 1: measured temperature [°C * 10]. Here 35.8 °C
-#define TEMP_CAL_HIGH_ADC       1588      // temperature 2: ADC value
+#define TEMP_CAL_HIGH_ADC       1600      // temperature 2: ADC value
-#define TEMP_CAL_HIGH_DEG_C     489       // temperature 2: measured temperature [°C * 10]. Here 48.9 °C
+#define TEMP_CAL_HIGH_DEG_C     420       // temperature 2: measured temperature [°C * 10]. Here 48.9 °C
-#define TEMP_WARNING_ENABLE     0         // to beep or not to beep, 1 or 0, DO NOT ACTIVITE WITHOUT CALIBRATION!
+#define TEMP_WARNING_ENABLE     1         // to beep or not to beep, 1 or 0, DO NOT ACTIVITE WITHOUT CALIBRATION!
 #define TEMP_WARNING            600       // annoying fast beeps [°C * 10].  Here 60.0 °C
 #define TEMP_POWEROFF_ENABLE    0         // to poweroff or not to poweroff, 1 or 0, DO NOT ACTIVITE WITHOUT CALIBRATION!
 #define TEMP_POWEROFF           650       // overheat poweroff. (while not driving) [°C * 10]. Here 65.0 °C
@@ -91,15 +91,15 @@
 
 #define USART2_BAUD             38400                   // UART2 baud rate (long wired cable)
 #define USART2_WORDLENGTH       UART_WORDLENGTH_8B      // UART_WORDLENGTH_8B or UART_WORDLENGTH_9B
-// #define CONTROL_SERIAL_USART2                           // left sensor board cable, disable if ADC or PPM is used! For Arduino control check the hoverSerial.ino
+#define CONTROL_SERIAL_USART2                           // left sensor board cable, disable if ADC or PPM is used! For Arduino control check the hoverSerial.ino
-// #define FEEDBACK_SERIAL_USART2                          // left sensor board cable, disable if ADC or PPM is used!
+#define FEEDBACK_SERIAL_USART2                          // left sensor board cable, disable if ADC or PPM is used!
 // #define DEBUG_SERIAL_USART2                             // left sensor board cable, disable if ADC or PPM is used!
 
 #define USART3_BAUD             38400                   // UART3 baud rate (short wired cable)
 #define USART3_WORDLENGTH       UART_WORDLENGTH_8B      // UART_WORDLENGTH_8B or UART_WORDLENGTH_9B
 // #define CONTROL_SERIAL_USART3                           // right sensor board cable, disable if I2C (nunchuck or lcd) is used! For Arduino control check the hoverSerial.ino
 // #define FEEDBACK_SERIAL_USART3                          // right sensor board cable, disable if I2C (nunchuck or lcd) is used!
-#define DEBUG_SERIAL_USART3                             // right sensor board cable, disable if I2C (nunchuck or lcd) is used!
+//#define DEBUG_SERIAL_USART3                             // right sensor board cable, disable if I2C (nunchuck or lcd) is used!
 
 #if defined(FEEDBACK_SERIAL_USART2) || defined(DEBUG_SERIAL_USART2)
 #define UART_DMA_CHANNEL DMA1_Channel7
@@ -122,7 +122,7 @@
  * For middle resting potis: Let the potis in the middle resting position, write value 1 to ADC1_MID and value 2 to ADC2_MID
  * Make, flash and test it.
  */
-#define CONTROL_ADC           // use ADC as input. disable CONTROL_SERIAL_USART2, FEEDBACK_SERIAL_USART2, DEBUG_SERIAL_USART2!
+//#define CONTROL_ADC           // use ADC as input. disable CONTROL_SERIAL_USART2, FEEDBACK_SERIAL_USART2, DEBUG_SERIAL_USART2!
 #define ADC1_MID_POT          // ADC1 middle resting poti: comment-out if NOT a middle resting poti
 #define ADC2_MID_POT          // ADC2 middle resting poti: comment-out if NOT a middle resting poti
 #define ADC1_MIN 0            // min ADC1-value while poti at minimum-position (0 - 4095)

Makefile

@@ -52,7 +52,7 @@ startup_stm32f103xe.s
 #######################################
 # binaries
 #######################################
-PREFIX = arm-none-eabi-
+PREFIX = ~/gcc-arm-none-eabi-7-2018-q2-update/bin/arm-none-eabi-
 CC = $(PREFIX)gcc
 AS = $(PREFIX)gcc -x assembler-with-cpp
 CP = $(PREFIX)objcopy

Src/main.c

@@ -69,8 +69,10 @@ static UART_HandleTypeDef huart;
 #if defined(CONTROL_SERIAL_USART2) || defined(CONTROL_SERIAL_USART3)
 typedef struct{
   uint16_t  start;
-  int16_t   steer;
+  //int16_t   steer;
-  int16_t   speed;
+  int16_t   speedLeft;
+  //int16_t   speed;
+  int16_t   speedRight;
   uint16_t  checksum;
 } Serialcommand;
 static volatile Serialcommand command;
@@ -103,12 +105,18 @@ uint8_t ctrlModReqRaw  = CTRL_MOD_REQ;
 uint8_t ctrlModReq;               // Final control mode request
 static int     cmd1;              // normalized input value. -1000 to 1000
 static int     cmd2;              // normalized input value. -1000 to 1000
-static int16_t steer;             // local variable for steering. -1000 to 1000
+//static int16_t steer;             // local variable for steering. -1000 to 1000
-static int16_t speed;             // local variable for speed. -1000 to 1000
+//static int16_t speed;             // local variable for speed. -1000 to 1000
-static int16_t steerFixdt;        // local fixed-point variable for steering low-pass filter
+//static int16_t steerFixdt;        // local fixed-point variable for steering low-pass filter
-static int16_t speedFixdt;        // local fixed-point variable for speed low-pass filter
+//static int16_t speedFixdt;        // local fixed-point variable for speed low-pass filter
-static int16_t steerRateFixdt;    // local fixed-point variable for steering rate limiter
+static int16_t speedLeftFixdt;        // local fixed-point variable for speedLeft low-pass filter
-static int16_t speedRateFixdt;    // local fixed-point variable for speed rate limiter
+static int16_t speedRightFixdt;        // local fixed-point variable for speedRight low-pass filter
+//static int16_t steerRateFixdt;    // local fixed-point variable for steering rate limiter
+//static int16_t speedRateFixdt;    // local fixed-point variable for speed rate limiter
+static int16_t speedLeftRateFixdt;    // local fixed-point variable for steering rate limiter
+static int16_t speedRightRateFixdt;    // local fixed-point variable for speed rate limiter
+
+static int16_t speed;             // local variable for speed. -1000 to 1000. only used for security checks. will be calculated by speedL and speedR
 
 extern volatile int pwml;         // global variable for pwm left. -1000 to 1000
 extern volatile int pwmr;         // global variable for pwm right. -1000 to 1000
@@ -322,16 +330,30 @@ int main(void) {
     #if defined CONTROL_SERIAL_USART2 || defined CONTROL_SERIAL_USART3
 
       // Handle received data validity, timeout and fix out-of-sync if necessary
-      if (command.start == START_FRAME && command.checksum == (command.start ^ command.steer ^ command.speed)) { 
+      //if (command.start == START_FRAME && command.checksum == (command.start ^ command.steer ^ command.speed)) {
+      if (command.start == START_FRAME && command.checksum == (command.start ^ command.speedLeft ^ command.speedRight)) {
         if (timeoutFlag) {                      // Check for previous timeout flag
           if (timeoutCnt-- <= 0)                // Timeout de-qualification
             timeoutFlag   = 0;                  // Timeout flag cleared
         } else {
-          cmd1            = CLAMP((int16_t)command.steer, -1000, 1000);
+          //cmd1            = CLAMP((int16_t)command.steer, -1000, 1000);
-          cmd2            = CLAMP((int16_t)command.speed, -1000, 1000);         
+          cmd1            = CLAMP((int16_t)command.speedLeft, -1000, 1000);
+          //cmd2            = CLAMP((int16_t)command.speed, -1000, 1000);
+          cmd2            = CLAMP((int16_t)command.speedRight, -1000, 1000);
           command.start   = 0xFFFF;             // Change the Start Frame for timeout detection in the next cycle
           timeoutCnt      = 0;                  // Reset the timeout counter
         }
+
+        // ####### MOTOR ENABLING: Only if the initial input is very small (for SAFETY) #######
+        if (enable == 0 && (cmd1 > -50 && cmd1 < 50) && (cmd2 > -50 && cmd2 < 50)){
+          buzzerPattern = 0;
+          buzzerFreq = 6; HAL_Delay(100);   // make 2 beeps indicating the motor enable
+          buzzerFreq = 4; HAL_Delay(200);
+          buzzerFreq = 0;
+          enable = 1;                       // enable motors
+          consoleLog("-- Motors enabled --\r\n");
+        }
+        
       } else {
         if (timeoutCnt++ >= SERIAL_TIMEOUT) {   // Timeout qualification
           timeoutFlag     = 1;                  // Timeout detected
@@ -357,28 +379,30 @@ int main(void) {
     #endif
 
 
-    // ####### MOTOR ENABLING: Only if the initial input is very small (for SAFETY) #######
+
-    if (enable == 0 && (cmd1 > -50 && cmd1 < 50) && (cmd2 > -50 && cmd2 < 50)){
-      buzzerPattern = 0;
-      buzzerFreq = 6; HAL_Delay(100);   // make 2 beeps indicating the motor enable
-      buzzerFreq = 4; HAL_Delay(200);
-      buzzerFreq = 0;
-      enable = 1;                       // enable motors
-      consoleLog("-- Motors enabled --\r\n");
-    }
 
     // ####### LOW-PASS FILTER #######
+    /*
     rateLimiter16(cmd1, RATE, &steerRateFixdt);
     rateLimiter16(cmd2, RATE, &speedRateFixdt);
     filtLowPass16(steerRateFixdt >> 4, FILTER, &steerFixdt);
     filtLowPass16(speedRateFixdt >> 4, FILTER, &speedFixdt);
     steer = steerFixdt >> 4;  // convert fixed-point to integer
     speed = speedFixdt >> 4;  // convert fixed-point to integer
+    */
+    rateLimiter16(cmd1, RATE, &speedLeftRateFixdt);
+    rateLimiter16(cmd2, RATE, &speedRightRateFixdt);
+    filtLowPass16(speedLeftRateFixdt >> 4, FILTER, &speedLeftFixdt);
+    filtLowPass16(speedRightRateFixdt >> 4, FILTER, &speedRightFixdt);
+    speedL = speedLeftFixdt >> 4;  // convert fixed-point to integer
+    speedR = speedRightFixdt >> 4;  // convert fixed-point to integer
+
+    speed = (speedL+speedR)/2;
 
     // ####### MIXER #######
     // speedR = CLAMP((int)(speed * SPEED_COEFFICIENT -  steer * STEER_COEFFICIENT), -1000, 1000);
     // speedL = CLAMP((int)(speed * SPEED_COEFFICIENT +  steer * STEER_COEFFICIENT), -1000, 1000);
-    mixerFcn(speedFixdt, steerFixdt, &speedR, &speedL);   // This function implements the equations above
+    //mixerFcn(speedFixdt, steerFixdt, &speedR, &speedL);   // This function implements the equations above
 
     // ####### SET OUTPUTS (if the target change is less than +/- 50) #######
     if ((speedL > lastSpeedL-50 && speedL < lastSpeedL+50) && (speedR > lastSpeedR-50 && speedR < lastSpeedR+50) && timeout < TIMEOUT) {