TranspOtterNG firmware ready

Inc/config.h

@@ -18,12 +18,12 @@
 #define BAT_CALIB_REAL_VOLTAGE        43.0       // input voltage measured by multimeter
 #define BAT_CALIB_ADC                 1704       // adc-value measured by mainboard (value nr 5 on UART debug output)
 
-#define BAT_NUMBER_OF_CELLS     10        // normal Hoverboard battery: 10s
+#define BAT_NUMBER_OF_CELLS     9        // normal Hoverboard battery: 10s
 #define BAT_LOW_LVL1_ENABLE     0         // to beep or not to beep, 1 or 0
-#define BAT_LOW_LVL1            3.6       // gently beeps at this voltage level. [V/cell]
+#define BAT_LOW_LVL1            3.2       // gently beeps at this voltage level. [V/cell]
 #define BAT_LOW_LVL2_ENABLE     1         // to beep or not to beep, 1 or 0
-#define BAT_LOW_LVL2            3.5       // your battery is almost empty. Charge now! [V/cell]
+#define BAT_LOW_LVL2            3.0       // your battery is almost empty. Charge now! [V/cell]
-#define BAT_LOW_DEAD            3.37      // undervoltage poweroff. (while not driving) [V/cell]
+#define BAT_LOW_DEAD            2.6      // undervoltage poweroff. (while not driving) [V/cell]
 
 #define DC_CUR_LIMIT     15         // DC current limit in amps per motor. so 15 means it will draw 30A out of your battery. it does not disable motors, it is a soft current limit.
 
@@ -45,10 +45,10 @@
 
 // ############################### SERIAL DEBUG ###############################
 
-#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!
-#define DEBUG_BAUD       115200     // UART baud rate
+//#define DEBUG_BAUD       115200     // UART baud rate
 //#define DEBUG_SERIAL_SERVOTERM
-#define DEBUG_SERIAL_ASCII          // "1:345 2:1337 3:0 4:0 5:0 6:0 7:0 8:0\r\n"
+//#define DEBUG_SERIAL_ASCII          // "1:345 2:1337 3:0 4:0 5:0 6:0 7:0 8:0\r\n"
 
 // ############################### INPUT ###############################
 
@@ -72,13 +72,12 @@
 
 // ###### TRANSPOTTER FIRMWARE #######
 #define CONTROL_GAMETRAK
-//#define INVERT_GAMETRAK
 #define SUPPORT_LCD
 #define SUPPORT_NUNCHUCK
 //#define SUPPORT_REMOTE
 
 #define VEL_P     0.9
-#define ROT_P     1.2
+#define ROT_P     -1.2
 
 
 // ###### CONTROL VIA NINTENDO NUNCHUCK ######
@@ -96,7 +95,7 @@
 // - weakr and weakl: field weakening for extra boost at high speed (speedR > 700 and speedL > 700). 0 to ~400
 
 #define FILTER              0.1  // lower value == softer filter. do not use values <0.01, you will get float precision issues.
-#define SPEED_COEFFICIENT   0.5  // higher value == stronger. 0.0 to ~2.0?
+#define SPEED_COEFFICIENT   0.9  // higher value == stronger. 0.0 to ~2.0?
 #define STEER_COEFFICIENT   0.5  // higher value == stronger. if you do not want any steering, set it to 0.0; 0.0 to 1.0
 #define INVERT_R_DIRECTION
 #define INVERT_L_DIRECTION

Src/control.c

@@ -84,6 +84,13 @@ void PPM_Init() {
 }
 #endif
 
+uint8_t Nunchuck_Ping() {
+  if (HAL_I2C_Master_Receive(&hi2c2,0xA4,(uint8_t*)nunchuck_data, 1, 10) == HAL_OK) {
+    return 1;
+  }
+  return 0;
+}
+
 void Nunchuck_Init() {
     //-- START -- init WiiNunchuck
   i2cBuffer[0] = 0xF0;
@@ -101,18 +108,18 @@ void Nunchuck_Init() {
 
 void Nunchuck_Read() {
   i2cBuffer[0] = 0x00;
-  HAL_I2C_Master_Transmit(&hi2c2,0xA4,(uint8_t*)i2cBuffer, 1, 100);
+  HAL_I2C_Master_Transmit(&hi2c2,0xA4,(uint8_t*)i2cBuffer, 1, 10);
-  HAL_Delay(5);
+  HAL_Delay(3);
-  if (HAL_I2C_Master_Receive(&hi2c2,0xA4,(uint8_t*)nunchuck_data, 6, 100) == HAL_OK) {
+  if (HAL_I2C_Master_Receive(&hi2c2,0xA4,(uint8_t*)nunchuck_data, 6, 10) == HAL_OK) {
     timeout = 0;
   } else {
     timeout++;
   }
 
-  if (timeout > 3) {
+  /*if (timeout > 3) {
     HAL_Delay(50);
     Nunchuck_Init();
-  }
+  }*/
 
   //setScopeChannel(0, (int)nunchuck_data[0]);
   //setScopeChannel(1, (int)nunchuck_data[1]);

Src/hd44780.c

@@ -13,10 +13,11 @@ uint8_t LCDerrorFlag = 0;
 void LCD_WaitForBusyFlag(LCD_PCF8574_HandleTypeDef* handle) {
 	uint8_t flag;
 	LCD_GetBusyFlag(handle, &flag);
-	uint32_t startTick=HAL_GetTick();
+	//uint32_t startTick=HAL_GetTick();
-	while (flag == 1 && HAL_GetTick()-startTick<handle->pcf8574.PCF_I2C_TIMEOUT) {
+	//while (flag == 1 && HAL_GetTick()-startTick<handle->pcf8574.PCF_I2C_TIMEOUT) {
-		LCD_GetBusyFlag(handle, &flag);
+	//	LCD_GetBusyFlag(handle, &flag);
-	}
+	//}
+	return;
 }
 
 LCD_RESULT LCD_I2C_WriteOut(LCD_PCF8574_HandleTypeDef* handle) {
@@ -61,7 +62,7 @@ LCD_RESULT LCD_Init(LCD_PCF8574_HandleTypeDef* handle) {
 		return LCD_ERROR;
 	}
 
-	HAL_Delay(50);
+	//HAL_Delay(50);
 	LCD_StateWriteBit(handle, 0, LCD_PIN_RS);
 	LCD_StateWriteBit(handle, 0, LCD_PIN_RW);
 	LCD_StateWriteBit(handle, 0, LCD_PIN_E);
@@ -155,17 +156,17 @@ LCD_RESULT LCD_GetBusyFlag(LCD_PCF8574_HandleTypeDef* handle, uint8_t* flag) {
 
 	LCD_StateWriteBit(handle, 1, LCD_PIN_E);
 
-	PCF8574_Read(&handle->pcf8574, flag);
+	//PCF8574_Read(&handle->pcf8574, flag);
 
-	*flag &= 1 << handle->pins[LCD_PIN_D7];
+	//*flag &= 1 << handle->pins[LCD_PIN_D7];
-	*flag >>= handle->pins[LCD_PIN_D7];
+	//*flag >>= handle->pins[LCD_PIN_D7];
 
 	LCD_StateWriteBit(handle, 0, LCD_PIN_E);
 
 	LCD_StateWriteBit(handle, 1, LCD_PIN_E);
 
-	uint8_t flag2;
+	//uint8_t flag2;
-	PCF8574_Read(&handle->pcf8574, &flag2);
+	//PCF8574_Read(&handle->pcf8574, &flag2);
 
 	LCD_StateWriteBit(handle, 0, LCD_PIN_E);
 	LCD_StateWriteBit(handle, 0, LCD_PIN_RW);

Src/main.c

@@ -30,6 +30,14 @@
 LCD_PCF8574_HandleTypeDef lcd;
 extern I2C_HandleTypeDef hi2c2;
 
+extern uint8_t LCDerrorFlag;
+
+uint8_t nunchuck_connected = 0;
+
+float steering;
+int feedforward;
+
+
 void longBeep(void);
 void shortBeep(void);
 
@@ -272,15 +280,15 @@ int main(void) {
     I2C_Init();
     HAL_Delay(50);
     lcd.pcf8574.PCF_I2C_ADDRESS = 0x27;
-      lcd.pcf8574.PCF_I2C_TIMEOUT = 5;
+    lcd.pcf8574.PCF_I2C_TIMEOUT = 1;
-      lcd.pcf8574.i2c = hi2c2;
+    lcd.pcf8574.i2c = hi2c2;
-      lcd.NUMBER_OF_LINES = NUMBER_OF_LINES_2;
+    lcd.NUMBER_OF_LINES = NUMBER_OF_LINES_2;
-      lcd.type = TYPE0;
+    lcd.type = TYPE0;
 
-      if(LCD_Init(&lcd)!=LCD_OK){
+    if(LCD_Init(&lcd)!=LCD_OK){
-          // error occured
+        // error occured
-          //TODO while(1);
+        //TODO while(1);
-      }
+    }
 
     LCD_ClearDisplay(&lcd);
     HAL_Delay(5);
@@ -298,41 +306,6 @@ int main(void) {
   while(1) {
     HAL_Delay(DELAY_IN_MAIN_LOOP); //delay in ms
 
-    #ifdef CONTROL_NUNCHUCK
-      Nunchuck_Read();
-      cmd1 = CLAMP((nunchuck_data[0] - 127) * 8, -1000, 1000); // x - axis. Nunchuck joystick readings range 30 - 230
-      cmd2 = CLAMP((nunchuck_data[1] - 128) * 8, -1000, 1000); // y - axis
-
-      button1 = (uint8_t)nunchuck_data[5] & 1;
-      button2 = (uint8_t)(nunchuck_data[5] >> 1) & 1;
-    #endif
-
-    #ifdef CONTROL_PPM
-      cmd1 = CLAMP((ppm_captured_value[0] - 500) * 2, -1000, 1000);
-      cmd2 = CLAMP((ppm_captured_value[1] - 500) * 2, -1000, 1000);
-      button1 = ppm_captured_value[5] > 500;
-      float scale = ppm_captured_value[2] / 1000.0f;
-    #endif
-
-    #ifdef CONTROL_ADC
-      // ADC values range: 0-4095, see ADC-calibration in config.h
-      cmd1 = CLAMP(adc_buffer.l_tx2 - ADC1_MIN, 0, ADC1_MAX) / (ADC1_MAX / 1000.0f);  // ADC1
-      cmd2 = CLAMP(adc_buffer.l_rx2 - ADC2_MIN, 0, ADC2_MAX) / (ADC2_MAX / 1000.0f);  // ADC2
-
-      // use ADCs as button inputs:
-      button1 = (uint8_t)(adc_buffer.l_tx2 > 2000);  // ADC1
-      button2 = (uint8_t)(adc_buffer.l_rx2 > 2000);  // ADC2
-
-      timeout = 0;
-    #endif
-
-    #ifdef CONTROL_SERIAL_USART2
-      cmd1 = CLAMP((int16_t)command.steer, -1000, 1000);
-      cmd2 = CLAMP((int16_t)command.speed, -1000, 1000);
-
-      timeout = 0;
-    #endif
-
     #ifdef CONTROL_GAMETRAK
     if(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {
       enable = 0;
@@ -360,32 +333,100 @@ int main(void) {
     }
 
 
+
+
+
     uint16_t distance = CLAMP((adc_buffer.l_rx2) - 180, 0, 4095);
-    float steering = (adc_buffer.l_tx2 - 2048) / 2048.0;
+    steering = (adc_buffer.l_tx2 - 2048) / 2048.0;
-    int feedforward = ((distance - (int)(setDistance * 1345)));
+    feedforward = ((distance - (int)(setDistance * 1345)));
-
+    if (nunchuck_connected == 0) {
-    speedL = speedL * 0.8f + (CLAMP(feedforward +  ((steering)*((float)MAX(ABS(feedforward), 50)) * ROT_P), -750, 750) * -0.2f);
+      speedL = speedL * 0.8f + (CLAMP(feedforward +  ((steering)*((float)MAX(ABS(feedforward), 50)) * ROT_P), -850, 850) * -0.2f);
-    speedR = speedR * 0.8f + (CLAMP(feedforward -  ((steering)*((float)MAX(ABS(feedforward), 50)) * ROT_P), -750, 750) * -0.2f);
+      speedR = speedR * 0.8f + (CLAMP(feedforward -  ((steering)*((float)MAX(ABS(feedforward), 50)) * ROT_P), -850, 850) * -0.2f);
-
+      if ((speedL < lastSpeedL + 50 && speedL > lastSpeedL - 50) && (speedR < lastSpeedR + 50 && speedR > lastSpeedR - 50)) {
-    if ((speedL < lastSpeedL + 50 && speedL > lastSpeedL - 50) && (speedR < lastSpeedR + 50 && speedR > lastSpeedR - 50)) {
+        if (distance - (int)(setDistance * 1345) > 0) {
-      if (distance - (int)(setDistance * 1345) > -300) {
-        pwml = speedL;
-        pwmr = -speedR;
-        if (checkRemote) {
-          if (!HAL_GPIO_ReadPin(LED_PORT, LED_PIN)) {
-            enable = 1;
-          } else {
-            enable = 0;
-          }
-        } else {
           enable = 1;
         }
-      } else {
+        if (distance - (int)(setDistance * 1345) > -300) {
-        enable = 0;
+          #ifdef INVERT_R_DIRECTION
+          pwmr = -speedR;
+          #endif
+          #ifndef INVERT_R_DIRECTION
+          pwmr = speedR;
+          #endif
+
+          #ifdef INVERT_L_DIRECTION
+          pwml = -speedL;
+          #endif
+          #ifndef INVERT_L_DIRECTION
+          pwml = speedL;
+          #endif
+
+          if (checkRemote) {
+            if (!HAL_GPIO_ReadPin(LED_PORT, LED_PIN)) {
+              //enable = 1;
+            } else {
+              enable = 0;
+            }
+          }
+        } else {
+          enable = 0;
+        }
       }
+      lastSpeedL = speedL;
+      lastSpeedR = speedR;
+
+      timeout = 0;
+    } else {
+      Nunchuck_Read();
+      enable = 1;
+      cmd1 = CLAMP((nunchuck_data[0] - 127) * 8, -1000, 1000); // x - axis. Nunchuck joystick readings range 30 - 230
+      cmd2 = CLAMP((nunchuck_data[1] - 128) * 8, -1000, 1000); // y - axis
+
+        // ####### LOW-PASS FILTER #######
+      steer = steer * (1.0 - FILTER) + cmd1 * FILTER;
+      speed = speed * (1.0 - FILTER) + cmd2 * FILTER;
+
+
+      // ####### MIXER #######
+      speedR = CLAMP(speed * SPEED_COEFFICIENT -  steer * STEER_COEFFICIENT, -900, 900);
+      speedL = CLAMP(speed * SPEED_COEFFICIENT +  steer * STEER_COEFFICIENT, -900, 900);
+
+      // ####### SET OUTPUTS #######
+      if ((speedL < lastSpeedL + 100 && speedL > lastSpeedL - 100) && (speedR < lastSpeedR + 100 && speedR > lastSpeedR - 100) && timeout < TIMEOUT) {
+        #ifdef INVERT_R_DIRECTION
+          pwmr = speedR;
+        #else
+          pwmr = -speedR;
+        #endif
+        #ifdef INVERT_L_DIRECTION
+          pwml = speedL;
+        #else
+          pwml = -speedL;
+        #endif
+      }
+      else if (timeout > TIMEOUT) {
+        pwml = 0;
+        pwmr = 0;
+        enable = 0;
+
+        LCD_SetLocation(&lcd, 0, 0);
+      	LCD_WriteString(&lcd, "Len:");
+        LCD_SetLocation(&lcd, 8, 0);
+        LCD_WriteString(&lcd, "m(");
+        LCD_SetLocation(&lcd, 14, 0);
+        LCD_WriteString(&lcd, "m)");
+
+        HAL_Delay(1000);
+
+        nunchuck_connected = 0;
+      }
+      lastSpeedL = speedL;
+      lastSpeedR = speedR;
     }
 
 
+
+
     if ((distance / 1345.0) - setDistance > 0.5 && (lastDistance / 1345.0) - setDistance > 0.5) { // Error, robot too far away!
       enable = 0;
       longBeep();
@@ -400,81 +441,47 @@ int main(void) {
       poweroff();
     }
 
+    #ifdef SUPPORT_NUNCHUCK
+    if (counter % 500 == 0) {
+      if (nunchuck_connected == 0 && enable == 0) {
+        if (Nunchuck_Ping()) {
+          HAL_Delay(500);
+          Nunchuck_Init();
+          #ifdef SUPPORT_LCD
+          LCD_SetLocation(&lcd, 0, 0);
+          LCD_WriteString(&lcd, "Nunchuck Control");
+          #endif
+          timeout = 0;
+          HAL_Delay(1000);
+          nunchuck_connected = 1;
+        }
+      }
+    }
+    #endif
+
     #ifdef SUPPORT_LCD
     if (counter % 100 == 0) {
-      LCD_SetLocation(&lcd, 4, 0);
+      if (LCDerrorFlag == 1 && enable == 0) {
-      LCD_WriteFloat(&lcd,distance/1345.0,2);
-      LCD_SetLocation(&lcd, 10, 0);
-      LCD_WriteFloat(&lcd,setDistance,2);
-      LCD_SetLocation(&lcd, 4, 1);
-      LCD_WriteFloat(&lcd,batteryVoltage, 1);
-      LCD_SetLocation(&lcd, 11, 1);
-      LCD_WriteFloat(&lcd,MAX(ABS(currentR), ABS(currentL)),2);
-    }
-    #endif
-    #endif
 
-    #ifndef CONTROL_GAMETRAK
+      } else {
-    // ####### LOW-PASS FILTER #######
+        if (nunchuck_connected == 0) {
-    steer = steer * (1.0 - FILTER) + cmd1 * FILTER;
+          LCD_SetLocation(&lcd, 4, 0);
-    speed = speed * (1.0 - FILTER) + cmd2 * FILTER;
+          LCD_WriteFloat(&lcd,distance/1345.0,2);
-
+          LCD_SetLocation(&lcd, 10, 0);
-
+          LCD_WriteFloat(&lcd,setDistance,2);
-    // ####### MIXER #######
+        }
-    speedR = CLAMP(speed * SPEED_COEFFICIENT -  steer * STEER_COEFFICIENT, -1000, 1000);
+        LCD_SetLocation(&lcd, 4, 1);
-    speedL = CLAMP(speed * SPEED_COEFFICIENT +  steer * STEER_COEFFICIENT, -1000, 1000);
+        LCD_WriteFloat(&lcd,batteryVoltage, 1);
-
+        LCD_SetLocation(&lcd, 11, 1);
-
+        LCD_WriteFloat(&lcd,MAX(ABS(currentR), ABS(currentL)),2);
-    #ifdef ADDITIONAL_CODE
+      }
-      ADDITIONAL_CODE;
-    #endif
-
-
-    // ####### SET OUTPUTS #######
-    if ((speedL < lastSpeedL + 50 && speedL > lastSpeedL - 50) && (speedR < lastSpeedR + 50 && speedR > lastSpeedR - 50) && timeout < TIMEOUT) {
-    #ifdef INVERT_R_DIRECTION
-      pwmr = speedR;
-    #else
-      pwmr = -speedR;
-    #endif
-    #ifdef INVERT_L_DIRECTION
-      pwml = -speedL;
-    #else
-      pwml = speedL;
-    #endif
-    }
-
-    lastSpeedL = speedL;
-    lastSpeedR = speedR;
-
-
-    if (inactivity_timeout_counter % 25 == 0) {
-      // ####### CALC BOARD TEMPERATURE #######
-      board_temp_adc_filtered = board_temp_adc_filtered * 0.99 + (float)adc_buffer.temp * 0.01;
-      board_temp_deg_c = ((float)TEMP_CAL_HIGH_DEG_C - (float)TEMP_CAL_LOW_DEG_C) / ((float)TEMP_CAL_HIGH_ADC - (float)TEMP_CAL_LOW_ADC) * (board_temp_adc_filtered - (float)TEMP_CAL_LOW_ADC) + (float)TEMP_CAL_LOW_DEG_C;
-
-      // ####### DEBUG SERIAL OUT #######
-      #ifdef CONTROL_ADC
-        setScopeChannel(0, (int)adc_buffer.l_tx2);  // 1: ADC1
-        setScopeChannel(1, (int)adc_buffer.l_rx2);  // 2: ADC2
-      #endif
-      setScopeChannel(2, (int)speedR);  // 3: output speed: 0-1000
-      setScopeChannel(3, (int)speedL);  // 4: output speed: 0-1000
-      setScopeChannel(4, (int)adc_buffer.batt1);  // 5: for battery voltage calibration
-      setScopeChannel(5, (int)(batteryVoltage * 100.0f));  // 6: for verifying battery voltage calibration
-      setScopeChannel(6, (int)board_temp_adc_filtered);  // 7: for board temperature calibration
-      setScopeChannel(7, (int)board_temp_deg_c);  // 8: for verifying board temperature calibration
-      consoleScope();
     }
 
 
-    // ####### POWEROFF BY POWER-BUTTON #######
-    if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN) && weakr == 0 && weakl == 0) {
-      enable = 0;
-      while (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {}
-      poweroff();
-    }
     #endif
+    counter++;
+    #endif
+
 
 
     // ####### BEEP AND EMERGENCY POWEROFF #######

Src/setup.c

@@ -238,7 +238,7 @@ void I2C_Init()
   HAL_NVIC_EnableIRQ(DMA1_Channel5_IRQn);
 
   hi2c2.Instance = I2C2;
-  hi2c2.Init.ClockSpeed = 100000;
+  hi2c2.Init.ClockSpeed = 200000;
   hi2c2.Init.DutyCycle = I2C_DUTYCYCLE_2;
   hi2c2.Init.OwnAddress1 = 0;
   hi2c2.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;