UPD: more universal codebase for different vehicles

Inc/config.h

@@ -10,22 +10,49 @@
 #define MILLI_PSI (PSI * 1000)
 #define MILLI_V (V * 1000)
 
+// ################################################################################
+
 #define PWM_FREQ         16000     // PWM frequency in Hz
 #define DEAD_TIME        32        // PWM deadtime
 
-//#define DC_CUR_LIMIT     34         // Motor DC current limit in amps
 #define DC_CUR_LIMIT     35         // Motor DC current limit in amps
 
+// ################################################################################
+
+#define DEBUG_SERIAL_USART2
+//#define DEBUG_SERIAL_USART3
+#define DEBUG_BAUD       115200    // UART baud rate
 //#define DEBUG_SERIAL_SERVOTERM
 #define DEBUG_SERIAL_ASCII
-#define DEBUG_SERIAL_USART2
-#define DEBUG_BAUD       115200    // UART baud rate
 //#define DEBUG_I2C_LCD
 
-//#define CONTROL_PPM                // use PPM CONTROL_PPM
+// ################################################################################
-#define PPM_NUM_CHANNELS 6         // number of PPM channels to receive
 
+// ###### CONTROL VIA RC REMOTE ######
+//#define CONTROL_PPM                // use PPM CONTROL_PPM
+//#define PPM_NUM_CHANNELS 6         // number of PPM channels to receive
+
+// ###### CONTROL VIA TWO POTENTIOMETERS ######
 // #define CONTROL_ADC
+
+// ###### CONTROL VIA NINTENDO NUNCHUCK ######
 #define CONTROL_NUNCHUCK
 
+// ################################################################################
+
+// ###### DRIVING BEHAVIOR ######
+#define FILTER              0.1
+#define SPEED_COEFFICIENT   0.5
+#define STEER_COEFFICIENT   0.5
+
+// ###### BOBBYCAR ######
+// #define FILTER              0.1
+// #define SPEED_COEFFICIENT   1
+// #define STEER_COEFFICIENT   0
+
+// ###### ARMCHAIR ######
+// #define FILTER              0.05
+// #define SPEED_COEFFICIENT   0.5
+// #define STEER_COEFFICIENT   0.2
+
 // #define BEEPS_BACKWARD

Src/bldc.c

@@ -12,7 +12,7 @@ volatile int pwmr = 0;
 volatile int weakl = 0;
 volatile int weakr = 0;
 
-extern volatile int pwmrl;
+extern volatile int speed;
 
 extern volatile adc_buf_t adc_buffer;
 
@@ -132,8 +132,6 @@ int offsetdcl   = 2000;
 int offsetdcr   = 2000;
 
 float batteryVoltage = 40.0;
-float adccmd1;
-float adccmd2;
 
 int curl = 0;
 // int errorl = 0;
@@ -166,7 +164,7 @@ void DMA1_Channel1_IRQHandler() {
 
 
   #ifdef BEEPS_BACKWARD
-    if (pwmrl < -50 && enable == 1) {
+    if (speed < -50 && enable == 1) {
       buzzerFreq = 5;
       buzzerPattern = 1;
     } else if (enable == 1) {
@@ -214,8 +212,8 @@ void DMA1_Channel1_IRQHandler() {
 
   blockPhaseCurrent(posl, adc_buffer.rl1 - offsetrl1, adc_buffer.rl2 - offsetrl2, &curl);
 
-  setScopeChannel(2, (adc_buffer.rl1 - offsetrl1) / 8);
+  //setScopeChannel(2, (adc_buffer.rl1 - offsetrl1) / 8);
-  setScopeChannel(3, (adc_buffer.rl2 - offsetrl2) / 8);
+  //setScopeChannel(3, (adc_buffer.rl2 - offsetrl2) / 8);
 
   buzzerTimer++;
 

Src/control.c

@@ -5,7 +5,6 @@
 #include "config.h"
 
 TIM_HandleTypeDef TimHandle;
-uint16_t ppm_captured_value[PPM_NUM_CHANNELS+1] = {0};
 uint8_t ppm_count = 0;
 uint32_t timeout = 100;
 uint8_t nunchuck_data[6] = {0};
@@ -16,6 +15,9 @@ extern I2C_HandleTypeDef hi2c2;
 DMA_HandleTypeDef hdma_i2c2_rx;
 DMA_HandleTypeDef hdma_i2c2_tx;
 
+#ifdef CONTROL_PPM
+uint16_t ppm_captured_value[PPM_NUM_CHANNELS+1] = {0};
+
 void PPM_ISR_Callback() {
   // Dummy loop with 16 bit count wrap around
   uint16_t rc_delay = TIM2->CNT;
@@ -54,7 +56,7 @@ void PPM_Init() {
   HAL_NVIC_EnableIRQ(EXTI3_IRQn);
   HAL_TIM_Base_Start(&TimHandle);
 }
-
+#endif
 
 void Nunchuck_Init() {
     //-- START -- init WiiNunchuck

Src/main.c

@@ -25,15 +25,21 @@
 
 void SystemClock_Config(void);
 
-extern float adccmd1;
+int cmd1;
-extern float adccmd2;
+int cmd2;
+int cmd3;
+
+int steer;
+int speed;
 
 extern TIM_HandleTypeDef htim_left;
 extern TIM_HandleTypeDef htim_right;
 extern ADC_HandleTypeDef hadc1;
 extern ADC_HandleTypeDef hadc2;
 extern volatile adc_buf_t adc_buffer;
+#ifdef CONTROL_PPM
 extern volatile uint16_t ppm_captured_value[PPM_NUM_CHANNELS+1];
+#endif
 
 extern volatile int pwml;
 extern volatile int pwmr;
@@ -110,8 +116,6 @@ int main(void) {
     Nunchuck_Init();
   #endif
 
-
-
   enable = 1;
 
   while(1) {
@@ -119,62 +123,51 @@ int main(void) {
 
     #ifdef CONTROL_NUNCHUCK
       Nunchuck_Read();
-      adccmd1 = adccmd1 * 0.9 + (float)nunchuck_data[0] * 0.1;
+      cmd1 = CLAMP((nunchuck_data[0] - 127) * 10, -1000, 1000);
-      adccmd2 = adccmd2 * 0.9 + (float)nunchuck_data[1] * 0.1;
+      cmd2 = CLAMP((nunchuck_data[1] - 127) * 10, -1000, 1000);
-      setScopeChannel(0, adccmd1);
+
-      setScopeChannel(1, adccmd2);
+      //uint8_t button1 = (uint8_t)nunchuck_data[5] & 1;
-      setScopeChannel(2, (int)nunchuck_data[5] & 1);
+      //uint8_t button2 = (uint8_t)(nunchuck_data[5] >> 1) & 1;
-      setScopeChannel(3, ((int)nunchuck_data[5] >> 1) & 1);
+
+      timeout = 0;
     #endif
 
-
     #ifdef CONTROL_PPM
-      speedL = -(CLAMP((((ppm_captured_value[1]-500)+(ppm_captured_value[0]-500)/2.0)*(ppm_captured_value[2]/500.0)), -800, 800));
+      cmd1 = CLAMP((ppm_captured_value[0] - 500) * 2, -1000, 1000);
-      speedR = (CLAMP((((ppm_captured_value[1]-500)-(ppm_captured_value[0]-500)/2.0)*(ppm_captured_value[2]/500.0)), -800, 800));
+      cmd2 = CLAMP((ppm_captured_value[1] - 500) * 2, -1000, 1000);
-      if ((speedL < lastSpeedL + 50 && speedL > lastSpeedL - 50) && (speedR < lastSpeedR + 50 && speedR > lastSpeedR - 50) && timeout < 50) {
-        //pwmr = speedR;
-        //pwml = speedL;
-      }
-
-      lastSpeedL = speedL;
-      lastSpeedR = speedR;
-      //setScopeChannel(0, (int)pwmrl);
-      //setScopeChannel(1, (int)speedL);
     #endif
 
     #ifdef CONTROL_ADC
-      //adccmd1 = adccmd1 * 0.9 + (float)adc_buffer.l_rx2 * 0.1; // throttle
+      cmd1 = CLAMP(adc_buffer.l_rx2 - 700, 0, 2350) / 2.35;
-      adccmd2 = adccmd2 * 0.9 + (float)adc_buffer.l_tx2 * 0.1; // button
+      //uint8_t button1 = (uint8_t)(adc_buffer.l_tx2 > 2000);
 
-      pwmrl = pwmrl * 0.9 + (CLAMP(adc_buffer.l_rx2 - 700, 0, 2350) / 2.35) * 0.1 * direction;
+      timeout = 0;
-      // pwmrl has to be 0-1000 (or negative when driving backwards)
-
-      setScopeChannel(0, (int)adccmd1);
-      setScopeChannel(1, (int)adccmd2);
-
-      // adccmd2 = button, ranges 0 in idle and 4096 when pressed
-      if (adccmd2 > 2000 && pwmrl < 300) { // driving backwards at low speeds
-        direction = -0.2;
-      } else {
-        direction = 1;
-      }
-
-      if (adccmd2 > 2000 && pwmrl > 700) { // field weakening at high speeds
-        weakl = pwmrl - 600; // weak should never exceed 400 or 450 MAX!!
-        weakr = pwmrl - 600;
-      } else {
-        weakl = 0;
-        weakr = 0;
-      }
-
-      if (pwml < 1000) {
-        pwml +=1;
-      }
-
-      pwml = pwmrl;
-      pwmr = -pwmrl;
     #endif
 
+    // ####### LOW-PASS FILTER #######
+    speed = speed * (1.0 - FILTER) + cmd1 * FILTER;
+    steer = steer * (1.0 - FILTER) + cmd2 * FILTER;
+
+    setScopeChannel(0, (int)speed);
+    setScopeChannel(1, (int)steer);
+
+    // ####### MIXER #######
+    speedR = CLAMP(speed * SPEED_COEFFICIENT -  steer * STEER_COEFFICIENT, -1000, 1000);
+    speedL = CLAMP(speed * SPEED_COEFFICIENT +  steer * STEER_COEFFICIENT, -1000, 1000);
+
+    setScopeChannel(2, (int)speedR);
+    setScopeChannel(3, (int)speedL);
+
+    // ####### SET OUTPUTS #######
+    if ((speedL < lastSpeedL + 50 && speedL > lastSpeedL - 50) && (speedR < lastSpeedR + 50 && speedR > lastSpeedR - 50) && timeout < 50) {
+      pwmr = speedR;
+      pwml = speedL;
+    }
+
+    lastSpeedL = speedL;
+    lastSpeedR = speedR;
+
+    // ####### LOG TO CONSOLE #######
     consoleScope();
 
     timeout=0;

Src/stm32f1xx_it.c

@@ -168,7 +168,7 @@ void SysTick_Handler(void) {
   /* USER CODE END SysTick_IRQn 1 */
 }
 
-
+#ifdef CONTROL_NUNCHUCK
 extern I2C_HandleTypeDef hi2c2;
 void I2C1_EV_IRQHandler(void)
 {
@@ -207,14 +207,15 @@ void DMA1_Channel5_IRQHandler(void)
 
   /* USER CODE END DMA1_Channel5_IRQn 1 */
 }
+#endif
 
-
+#ifdef CONTROL_PPM
-
 void EXTI3_IRQHandler(void)
 {
     PPM_ISR_Callback();
     __HAL_GPIO_EXTI_CLEAR_IT(GPIO_PIN_3);
 }
+#endif
 
 /******************************************************************************/
 /* STM32F1xx Peripheral Interrupt Handlers                                    */