From c7c0b768ac17b6bd2907fb83ea03b14a0bcd11a4 Mon Sep 17 00:00:00 2001
From: 0xFEEDC0DE64 <daniel@brunner.ninja>
Date: Mon, 19 Oct 2020 18:08:49 +0200
Subject: [PATCH] Implemented MOTOR_TEST mode

---
 CMakeLists.txt |  4 ++++
 config.h       |  6 +++---
 main.cpp       | 45 +++++++++++++++++++++++++++++++++++++++++++--
 3 files changed, 50 insertions(+), 5 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index fdc41ba..e6a2b5a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -33,6 +33,10 @@ include_directories(
     bobbycar-protocol
 )
 
+#add_definitions(-DMOTOR_TEST)
+#add_definitions(-DFEATURE_IGNORE_OTHER_MOTOR)
+add_definitions(-DFEATURE_BUTTON)
+
 add_executable(firmware.elf
     STM32CubeF1/Drivers/STM32F1xx_HAL_Driver/Src/stm32f1xx_hal.c
     STM32CubeF1/Drivers/STM32F1xx_HAL_Driver/Src/stm32f1xx_hal_flash.c
diff --git a/config.h b/config.h
index a20bf05..ca2da02 100644
--- a/config.h
+++ b/config.h
@@ -2,10 +2,10 @@
 
 #define PWM_FREQ            16000     // PWM frequency in Hz
 #define DEAD_TIME              48     // PWM deadtime
-#ifdef VARIANT_TRANSPOTTER
-  #define DELAY_IN_MAIN_LOOP    2
+#ifdef MOTOR_TEST
+  #define DELAY_IN_MAIN_LOOP    10
 #else
-  #define DELAY_IN_MAIN_LOOP    5     // in ms. default 5. it is independent of all the timing critical stuff. do not touch if you do not know what you are doing.
+  #define DELAY_IN_MAIN_LOOP    5
 #endif
 #define TIMEOUT                 5     // number of wrong / missing input commands before emergency off
 #define A2BIT_CONV             50     // A to bit for current conversion on ADC. Example: 1 A = 50, 2 A = 100, etc
diff --git a/main.cpp b/main.cpp
index 03180e6..90ebbba 100644
--- a/main.cpp
+++ b/main.cpp
@@ -212,15 +212,47 @@ int main()
 //#define UART_DMA_CHANNEL DMA1_Channel2
     //UART3_Init();
 
+#ifdef MOTOR_TEST
+    int pwm = 0;
+    int8_t dir = 1;
+#else
     HAL_UART_Receive_DMA(&huart2, (uint8_t *)&command, sizeof(command));
+#endif
 
     for (;;) {
         HAL_Delay(DELAY_IN_MAIN_LOOP); //delay in ms
 
+#ifndef MOTOR_TEST
         parseCommand();
+#endif
 
         timeout = 0;
 
+#ifdef MOTOR_TEST
+        left.state.enable = false;
+        left.state.ctrlMod = ControlMode::Voltage;
+        left.state.ctrlTyp = ControlType::Sinusoidal;
+        left.state.pwm = pwm;
+        left.state.iMotMax = 1;
+
+        right.state.enable = true;
+        right.state.ctrlMod = ControlMode::Voltage;
+        right.state.ctrlTyp = ControlType::Sinusoidal;
+        right.state.pwm = pwm;
+        right.state.iMotMax = 1;
+
+        constexpr auto pwmMax = 500;
+
+        pwm += dir;
+        if (pwm > pwmMax) {
+          pwm = pwmMax;
+          dir = -1;
+        } else if (pwm < -pwmMax) {
+          pwm = -pwmMax;
+          dir = 1;
+        }
+#endif
+
         // ####### CALC BOARD TEMPERATURE #######
         filtLowPass32(adc_buffer.temp, TEMP_FILT_COEF, &board_temp_adcFixdt);
         board_temp_adcFilt  = (int16_t)(board_temp_adcFixdt >> 20);  // convert fixed-point to integer
@@ -228,6 +260,7 @@ int main()
 
         sendFeedback();
 
+#ifdef FEATURE_BUTTON
         if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN))
         {
             left.state.enable = right.state.enable = 0;           // disable motors
@@ -240,6 +273,7 @@ int main()
                 poweroff();                                         // release power-latch
             }
         }
+#endif
 
         main_loop_counter++;
         timeout++;
@@ -323,8 +357,15 @@ void updateMotors()
     constexpr int32_t pwm_margin = 100;        /* This margin allows to always have a window in the PWM signal for proper Phase currents measurement */
 
     /* Make sure to stop BOTH motors in case of an error */
-    const bool enableLFin = left.state.enable && left.rtY.z_errCode == 0 && right.rtY.z_errCode == 0;
-    const bool enableRFin = right.state.enable && left.rtY.z_errCode == 0 && right.rtY.z_errCode == 0;
+
+#ifdef FEATURE_IGNORE_OTHER_MOTOR
+    constexpr bool ignoreOtherMotor = false;
+#else
+    constexpr bool ignoreOtherMotor = true;
+#endif
+
+    const bool enableLFin = left.state.enable && left.rtY.z_errCode == 0 && (right.rtY.z_errCode == 0 || ignoreOtherMotor);
+    const bool enableRFin = right.state.enable && (left.rtY.z_errCode == 0 || ignoreOtherMotor) && right.rtY.z_errCode == 0;
 
     // ========================= LEFT MOTOR ============================
     // Get hall sensors values