Add ControlMode For Hover

Signed-off-by: Lukas Bachschwell <lukas@lbsfilm.at>

platformio.ini

@@ -35,6 +35,6 @@ monitor_speed = 115200
 
 lib_deps =
   # ServoESP32
-  olikraus/U8g2@^2.35.19
+  # olikraus/U8g2@^2.35.19
-  DallasTemperature
+  # DallasTemperature
   Adafruit NeoPixel

src/hoverusart.h

@@ -31,6 +31,7 @@
 #define SERIAL_BAUD 115200       // [-] Baud rate for built-in Serial (used for the Serial Monitor)
 #define START_FRAME 0xABCD       // [-] Start frme definition for reliable serial communication
 // #define DEBUG_RX                 // [-] Debug received data. Prints all bytes to serial (comment-out to disable)
+#define wheelCircumference 52.00
 
 #define HoverSerial Serial2
 // GPIOS: 16:RXD 17:TXD
@@ -42,11 +43,20 @@ byte *p;                // Pointer declaration for the new received data
 byte incomingByte;
 byte incomingBytePrev;
 
+uint8_t sendTemperature = 0;
+uint8_t sendTemperatureDecimals = 0;
+uint8_t sendVoltage = 0;
+uint8_t sendVoltageDecimals = 0;
+uint8_t sendSpeed = 0;
+uint8_t sendSpeedDecimals = 0;
+
+int16_t ctrl_mode = 1;
 typedef struct
 {
     uint16_t start;
     int16_t steer;
     int16_t speed;
+    int16_t ctrl_mode; // 1 vlt // 2 trq
     uint16_t checksum;
 } SerialCommand;
 SerialCommand Command;
@@ -83,7 +93,8 @@ void Send(int16_t uSteer, int16_t uSpeed)
     Command.start = (uint16_t)START_FRAME;
     Command.steer = (int16_t)uSteer;
     Command.speed = (int16_t)uSpeed;
-    Command.checksum = (uint16_t)(Command.start ^ Command.steer ^ Command.speed);
+    Command.ctrl_mode = (int16_t)ctrl_mode;
+    Command.checksum = (uint16_t)(Command.start ^ Command.steer ^ Command.speed ^ Command.ctrl_mode);
 
     // Write to Serial
     HoverSerial.write((uint8_t *)&Command, sizeof(Command));
@@ -148,6 +159,26 @@ void Receive()
             Serial.print(Feedback.boardTemp);
             Serial.print(" 7: ");
             Serial.println(Feedback.cmdLed);
+
+            sendVoltage = abs(floor(Feedback.batVoltage / 100));
+            sendVoltageDecimals = Feedback.batVoltage - sendVoltage * 100;
+
+            Serial.println();
+            Serial.print(" Volt: ");
+            Serial.print(sendVoltage);
+            Serial.print(" . ");
+            Serial.println(sendVoltageDecimals);
+
+            sendTemperature = abs(floor(Feedback.boardTemp / 10));
+            sendTemperatureDecimals = Feedback.boardTemp - sendTemperature * 10;
+
+            // average the two rpms and calculate the speed
+            // mm/rpm*3600
+            float kmh = wheelCircumference * (-Feedback.speedR_meas + Feedback.speedL_meas) / 2 * 60 / 100000;
+            // Serial.print(kmh); Serial.print(" "); Serial.println(direction[0]);
+            Serial.printf("KMH, %f0.2\n", kmh);
+            sendSpeed = abs(floor(kmh));
+            sendSpeedDecimals = (kmh - sendSpeed) * 100;
         }
         else
         {

src/receiver.cpp

@@ -40,7 +40,7 @@ Servo esc2;
 SSD1306 display(0x3c, 5, 4);
 #endif
 
-OneWire oneWire(ONE_WIRE_BUS);
+// OneWire oneWire(ONE_WIRE_BUS);
 // DallasTemperature sensors(&oneWire); // one instance for all sensrs
 
 esp_now_peer_info_t remote;
@@ -49,12 +49,14 @@ float temperature = 0;
 float voltage = 0;
 float kmh = 0;
 
+#ifndef VARIANT_HOVER
 uint8_t sendTemperature = 0;
 uint8_t sendTemperatureDecimals = 0;
 uint8_t sendVoltage = 0;
 uint8_t sendVoltageDecimals = 0;
 uint8_t sendSpeed = 0;
 uint8_t sendSpeedDecimals = 0;
+#endif
 
 #include "rpm.h"
 
@@ -85,6 +87,9 @@ void setBoardOptions(uint8_t options)
   lightMode = options & 3;
   lightActive = (options >> 3) & 1;
   fanMode = (options >> 2) & 3;
+#ifdef VARIANT_HOVER
+  ctrl_mode = fanMode;
+#endif
 }
 
 // ESPNOW Functions ############################
@@ -287,6 +292,7 @@ bool manageRemote()
 
 // end ESPNOW functions
 
+#ifndef VARIANT_HOVER
 void checkTemperature()
 {
   // sensors.requestTemperatures(); // Send the command to get temperatures
@@ -338,6 +344,7 @@ void checkVoltage()
   // Serial.println(batteryVoltage);
   digitalWrite(voltageEnable, LOW); // change to low
 }
+#endif
 
 void setup()
 {

src/rpm.h

@@ -25,39 +25,38 @@ int direction[2] = {0, 0};
 long rpmTimer = 0;
 
 // Zero states are non existant
-int motorPosition[2][2][2] {
+int motorPosition[2][2][2]{
-  {
+    {{0, 1},
-    {0, 1},
+     {3, 2}},
-    {3, 2}
+    {{5, 6},
-  },
+     {4, 0}}};
-  {
-    {5, 6},
-    {4, 0}
-  }
-};
 // old state, new state
-int relativeLookup[6][6] {
+int relativeLookup[6][6]{
-  { 0, 1, 2, 3,-2,-1}, // 1
+    {0, 1, 2, 3, -2, -1},  // 1
-  {-1, 0, 1, 2, 3,-1}, // 2
+    {-1, 0, 1, 2, 3, -1},  // 2
-  {-2,-1, 0, 1, 2, 3}, // 3
+    {-2, -1, 0, 1, 2, 3},  // 3
-  {-3,-2,-1, 0, 1, 2}, // 4
+    {-3, -2, -1, 0, 1, 2}, // 4
-  { 1,-3,-2,-1, 0, 1}, // 5
+    {1, -3, -2, -1, 0, 1}, // 5
-  { 1, 2,-3,-2,-1, 0}  // 6
+    {1, 2, -3, -2, -1, 0}  // 6
 };
 
 #define PCNT_UNIT_LEFT PCNT_UNIT_0
 #define PCNT_UNIT_RIGHT PCNT_UNIT_1
 
-
+int getState(bool isLeft)
-int getState(bool isLeft) {
+{
-  if(isLeft) {
+  if (isLeft)
+  {
     return motorPosition[digitalRead(halBlueLeft)][digitalRead(halYellowLeft)][digitalRead(halGreenLeft)];
-  } else {
+  }
+  else
+  {
     return motorPosition[digitalRead(halBlueRight)][digitalRead(halYellowRight)][digitalRead(halGreenRight)];
   }
 }
 
-void initRPMPins() {
+void initRPMPins()
+{
   pinMode(halBlueLeft, INPUT_PULLUP);
   pinMode(halYellowLeft, INPUT_PULLUP);
   pinMode(halGreenLeft, INPUT_PULLUP);
@@ -77,8 +76,7 @@ void initRPMPins() {
       .counter_h_lim = 32767,
       .counter_l_lim = -1,
       .unit = PCNT_UNIT_LEFT,
-    .channel = PCNT_CHANNEL_0
+      .channel = PCNT_CHANNEL_0};
-  };
 
   pcnt_config_t pcnt_config_right = {
       .pulse_gpio_num = halBlueRight,
@@ -90,13 +88,11 @@ void initRPMPins() {
       .counter_h_lim = 32767,
       .counter_l_lim = -1,
       .unit = PCNT_UNIT_RIGHT,
-    .channel = PCNT_CHANNEL_0
+      .channel = PCNT_CHANNEL_0};
-  };
 
   pcnt_unit_config(&pcnt_config_left);
   pcnt_unit_config(&pcnt_config_right);
 
-
   pcnt_counter_pause(PCNT_UNIT_LEFT);
   pcnt_counter_pause(PCNT_UNIT_RIGHT);
 
@@ -105,13 +101,15 @@ void initRPMPins() {
 
   pcnt_counter_resume(PCNT_UNIT_LEFT);
   pcnt_counter_resume(PCNT_UNIT_RIGHT);
-
 }
 
-void measureRpm(void * parameter) {
+void measureRpm(void *parameter)
+{
 
-  while(true) {
+  while (true)
-    if(millis()-rpmTimer>250) {
+  {
+    if (millis() - rpmTimer > 250)
+    {
       // read counters
       pcnt_get_counter_value(PCNT_UNIT_LEFT, &counter[1]);
       pcnt_get_counter_value(PCNT_UNIT_RIGHT, &counter[0]);
@@ -119,11 +117,10 @@ void measureRpm(void * parameter) {
       pcnt_counter_clear(PCNT_UNIT_LEFT);
       pcnt_counter_clear(PCNT_UNIT_RIGHT);
 
-
+      rpmRight = (counter[0] / pulsesPerRotation) * 4; // LB: Actually only rps
-      rpmRight = (counter[0]/pulsesPerRotation)*4;
+      rpmLeft = (counter[1] / pulsesPerRotation) * 4;
-      rpmLeft = (counter[1]/pulsesPerRotation)*4;
+      kmh = (wheelCircumference / 100) * (rpmRight + rpmLeft) / 2 * 3600 / 1000;
-      kmh = (wheelCircumference/100) * (rpmRight+rpmLeft)/2 * 3600/1000;
+      // Serial.print(kmh); Serial.print(" "); Serial.println(direction[0]);
-      //Serial.print(kmh); Serial.print(" "); Serial.println(direction[0]);
       sendSpeed = abs(floor(kmh));
       sendSpeedDecimals = (kmh - sendSpeed) * 100;
       counter[true] = 0;
@@ -132,20 +129,28 @@ void measureRpm(void * parameter) {
     }
 
     int state = getState(0);
-    if(lastState[0] != state) {
+    if (lastState[0] != state)
-      if(relativeLookup[lastState[0]-1][state-1] > 0) {
+    {
+      if (relativeLookup[lastState[0] - 1][state - 1] > 0)
+      {
         direction[0] = FORWARD;
-      } else {
+      }
+      else
+      {
         direction[0] = BACKWARD;
       }
       lastState[0] = state;
     }
 
     state = getState(1);
-    if(lastState[1] != state) {
+    if (lastState[1] != state)
-      if(relativeLookup[lastState[1]-1][state-1] > 0) {
+    {
+      if (relativeLookup[lastState[1] - 1][state - 1] > 0)
+      {
         direction[1] = FORWARD;
-      } else {
+      }
+      else
+      {
         direction[1] = BACKWARD;
       }
 

src/settings.h

@@ -30,25 +30,23 @@ bool settingsChangeValueFlag = false;
 const char *settingPages[SETTINGS_COUNT] = {
     "Page Display",
     "LimitMode",
-  "FanOveride",
+    "CTRL Mode", // OLD FAN OVERRIDE
     "Wheel diameter",
     "Light Mode",
     "Throttle Min",
     "Throttle Center",
-  "Throttle Max"
+    "Throttle Max"};
-};
 
 // Setting rules format: default, min, max.
-int settingRules[SETTINGS_COUNT][3] {
+int settingRules[SETTINGS_COUNT][3]{
-  {0, 0, 3},   //page: 0 cycle, 1 speed, 2 volt, 3 temp
+    {0, 0, 3},    // page: 0 cycle, 1 speed, 2 volt, 3 temp
-  {0, 0, 1}, //limit default is no limit
+    {0, 0, 1},    // limit default is no limit
-  {0, 0, 2},   //fan 0 no, 1 on, 2 off
+    {0, 0, 2},    // fan 0 no, 1 on, 2 off // CTRL Mode 0 and 1 vlt 2 is torq
     {83, 0, 250}, // wheel mm
     {0, 0, 3},    // LightMode
     {HAL_MIN, 1000, 3000},
     {HAL_CENTER, 1000, 3000},
-  {HAL_MAX, 1000, 3000}
+    {HAL_MAX, 1000, 3000}};
-};
 /*
    const char *settingAliases[SETTINGS_COUNT] = {
    "Off", "ON"
@@ -60,24 +58,28 @@ int settingRules[SETTINGS_COUNT][3] {
    };
  */
 
-
 // Check if an integer is within a min and max value
-bool inRange(int val, int minimum, int maximum) {
+bool inRange(int val, int minimum, int maximum)
+{
   return ((minimum <= val) && (val <= maximum));
 }
 
-void updateBoardOptions() {
+void updateBoardOptions()
+{
   boardOptions = 0;
-  boardOptions |= (uint8_t) settings[fanOveride] << 2;
+  boardOptions |= (uint8_t)settings[fanOveride] << 2;
 
-  boardOptions |= boardOptions | (uint8_t) lightActive << 3;
+  boardOptions |= boardOptions | (uint8_t)lightActive << 3;
-  boardOptions |= boardOptions | (uint8_t) settings[lightMode];
+  boardOptions |= boardOptions | (uint8_t)settings[lightMode];
-  DEBUG_PRINT("board: "); DEBUG_PRINTLN(boardOptions);
+  DEBUG_PRINT("board: ");
+  DEBUG_PRINTLN(boardOptions);
 }
 
-void updateSettings() {
+void updateSettings()
+{
   preferences.begin("remote-set", false);
-  for (int i = 0; i < SETTINGS_COUNT; i++) {
+  for (int i = 0; i < SETTINGS_COUNT; i++)
+  {
     preferences.putInt(settingPages[i], settings[i]);
   }
 
@@ -86,48 +88,59 @@ void updateSettings() {
   preferences.begin("remote-set", false);
   DEBUG_PRINTLN(preferences.getInt(settingPages[0], 0));
   preferences.end();
-  //calculateRatios();
+  // calculateRatios();
   updateBoardOptions();
 }
 
-void setDefaultSettings() {
+void setDefaultSettings()
-  for (int i = 0; i < SETTINGS_COUNT; i++) {
+{
+  for (int i = 0; i < SETTINGS_COUNT; i++)
+  {
     settings[i] = settingRules[i][0];
   }
 
   updateSettings();
 }
 
-void loadSettings() {
+void loadSettings()
+{
   preferences.begin("remote-set", false);
-  for (int i = 0; i < SETTINGS_COUNT; i++) {
+  for (int i = 0; i < SETTINGS_COUNT; i++)
+  {
     settings[i] = preferences.getInt(settingPages[i], settingRules[i][0]); // retrieve default value
   }
   preferences.end();
 
   bool rewriteSettings = false;
   // Loop through all settings to check if everything is fine
-  for (int i = 0; i < SETTINGS_COUNT; i++) {
+  for (int i = 0; i < SETTINGS_COUNT; i++)
+  {
     int val = settings[i];
 
-    if (!inRange(val, settingRules[i][1], settingRules[i][2])) {
+    if (!inRange(val, settingRules[i][1], settingRules[i][2]))
+    {
       // Setting is damaged or never written. Rewrite default.
       rewriteSettings = true;
       settings[i] = settingRules[i][0];
     }
   }
-  if (rewriteSettings == true) {
+  if (rewriteSettings == true)
+  {
     updateSettings();
-  } else {
+  }
+  else
+  {
     // Calculate constants
-    //calculateRatios();
+    // calculateRatios();
     updateBoardOptions();
   }
 }
 
-void drawSettingsMenu() {
+void drawSettingsMenu()
+{
   // Position on OLED
-  int x = 0; int y = 10;
+  int x = 0;
+  int y = 10;
 
   // Draw setting title
   displayString = settingPages[currentSetting];
@@ -140,18 +153,23 @@ void drawSettingsMenu() {
 
   displayString = (String)val + ""; //+ settingPages[currentSetting][1];
   displayString.toCharArray(displayBuffer, displayString.length() + 1);
-  u8g2.setFont(u8g2_font_10x20_tr  );
+  u8g2.setFont(u8g2_font_10x20_tr);
 
-  if (changeSelectedSetting == true) {
+  if (changeSelectedSetting == true)
+  {
     u8g2.drawStr(x + 10, y + 20, displayBuffer);
-  } else {
+  }
+  else
+  {
     u8g2.drawStr(x, y + 20, displayBuffer);
   }
 }
 
-void drawSettingNumber() {
+void drawSettingNumber()
+{
   // Position on OLED
-  int x = 2; int y = 10;
+  int x = 2;
+  int y = 10;
 
   // Draw current setting number box
   u8g2.drawRFrame(x + 102, y - 10, 22, 32, 4);
@@ -176,55 +194,75 @@ void drawSettingNumber() {
    ratioPulseDistance = (gearRatio * (float)remoteSettings.wheelDiameter * 3.14156) / (((float)remoteSettings.motorPoles * 3) * 1000000); // Pulses to km travelled
    }
  */
-void controlSettingsMenu() {
+void controlSettingsMenu()
-  if (triggerActive()) {
+{
-    if (settingsChangeFlag == false) {
+  if (triggerActive())
+  {
+    if (settingsChangeFlag == false)
+    {
 
       // Save settings
-      if (changeSelectedSetting == true) {
+      if (changeSelectedSetting == true)
+      {
         updateSettings();
       }
 
       changeSelectedSetting = !changeSelectedSetting;
       settingsChangeFlag = true;
     }
-
+  }
-  } else {
+  else
+  {
     settingsChangeFlag = false;
   }
 
-  if (hallMeasurement >= (settings[maxHallValue] - 250) && settingsLoopFlag == false) {
+  if (hallMeasurement >= (settings[maxHallValue] - 250) && settingsLoopFlag == false)
+  {
     // Up
-    if (changeSelectedSetting == true) {
+    if (changeSelectedSetting == true)
+    {
       int val = settings[currentSetting] - 1;
 
-      if (inRange(val, settingRules[currentSetting][1], settingRules[currentSetting][2])) {
+      if (inRange(val, settingRules[currentSetting][1], settingRules[currentSetting][2]))
+      {
         settings[currentSetting] = val;
 
         settingsLoopFlag = true;
       }
-    } else {
+    }
-      if (currentSetting != 0) {
+    else
+    {
+      if (currentSetting != 0)
+      {
         currentSetting--;
         settingsLoopFlag = true;
       }
     }
-  } else if (hallMeasurement <= (settings[minHallValue] + 250) && settingsLoopFlag == false) {
+  }
+  else if (hallMeasurement <= (settings[minHallValue] + 250) && settingsLoopFlag == false)
+  {
     // Down
-    if (changeSelectedSetting == true) {
+    if (changeSelectedSetting == true)
+    {
       int val = settings[currentSetting] + 1;
 
-      if (inRange(val, settingRules[currentSetting][1], settingRules[currentSetting][2])) {
+      if (inRange(val, settingRules[currentSetting][1], settingRules[currentSetting][2]))
+      {
         settings[currentSetting] = val;
         settingsLoopFlag = true;
       }
-    } else {
+    }
-      if (currentSetting < (SETTINGS_COUNT - 1)) {
+    else
+    {
+      if (currentSetting < (SETTINGS_COUNT - 1))
+      {
         currentSetting++;
         settingsLoopFlag = true;
       }
     }
-  } else if (inRange(hallMeasurement, settings[centerHallValue] - 50, settings[centerHallValue] + 50)) {
+  }
+  else if (inRange(hallMeasurement, settings[centerHallValue] - 50, settings[centerHallValue] + 50))
+  {
     settingsLoopFlag = false;
   }
 }