From 9c4c82f8b38989de03ec7d38b5b11f3eb864b575 Mon Sep 17 00:00:00 2001 From: Alexander Klein Date: Sat, 29 Jan 2022 12:25:06 +0100 Subject: [PATCH] bug fix static in motor und speed --- .vscode/settings.json | 4 +- include/networkConfig.h | 26 +++----- lib/Menu/menu.cpp | 13 ++-- lib/Menu/menu.h | 4 -- lib/Menu/menuAction.cpp | 8 +++ lib/Menu/menuControl.h | 2 + lib/MotorControl/motorControl.cpp | 17 +++--- lib/MotorControl/motorControl.h | 5 +- lib/Speedometer/speedometer.cpp | 16 ++--- lib/Speedometer/speedometer.h | 6 +- src/SpecialMenus/menuManualDrive.cpp | 20 +++++++ src/SpecialMenus/menuManualDrive.h | 42 +++++++++++++ .../SpecialMenus}/menuPidSettings.cpp | 10 ++-- .../SpecialMenus}/menuPidSettings.h | 0 .../Modi/ManualControl/manualControl.cpp | 2 - src/driveModi/driveManager.cpp | 6 +- src/main.cpp | 56 +++++++++--------- src/moveControl.cpp | 59 ++++++++++++++----- 18 files changed, 197 insertions(+), 99 deletions(-) create mode 100644 src/SpecialMenus/menuManualDrive.cpp create mode 100644 src/SpecialMenus/menuManualDrive.h rename {lib/Menu => src/SpecialMenus}/menuPidSettings.cpp (78%) rename {lib/Menu => src/SpecialMenus}/menuPidSettings.h (100%) diff --git a/.vscode/settings.json b/.vscode/settings.json index 727da2e..17b9c29 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -74,6 +74,8 @@ "vector": "cpp", "fstream": "cpp", "istream": "cpp", - "streambuf": "cpp" + "streambuf": "cpp", + "functional": "cpp", + "cmath": "cpp" } } \ No newline at end of file diff --git a/include/networkConfig.h b/include/networkConfig.h index ebefa8e..d7f6b8b 100644 --- a/include/networkConfig.h +++ b/include/networkConfig.h @@ -1,36 +1,26 @@ -#define HW1_2 +#define HW1 //Network config RHEDE #ifdef RHEDE #define WLAN_SSID "LebennigHuus" #define WLAN_PASSWORD "Punica-699" -#define WLAN_IP 0x040BA8C0 //192.168.11.4 -#define WLAN_SUBNETMASK 0x0080FFFF //255.255.128.0 -#define WLAN_GATEWAY 0x0100a8c0 //192.168.0.1 -#define MQTT_SERVER 0x0701A8C0 //192.168.1.7 +#define WLAN_IP "192.168.11.4" +#define WLAN_SUBNETMASK "255.255.128.0" +#define WLAN_GATEWAY "192.168.0.1" +#define MQTT_SERVER "192.168.1.7" #define MQTT_PORT 1883 #define MQTT_AUTH #define MQTT_USER "kleiax" #define MQTT_PASSWORD "p?{$_~5%hBM7wrcFkr55KWr#" -#define NTP_SERVER 0x0100a8c0 //192.168.0.1 +#define NTP_SERVER "2.de.pool.ntp.org" #endif //RHEDE + + //Network config HW1 #ifdef HW1 #define WLAN_SSID "hw1_gast" #define WLAN_PASSWORD "KeineAhnung" -#define WLAN_IP 0x3400a8c0 //192.168.0.67 -#define WLAN_SUBNETMASK 0x00FFFFFF //255.255.255.0 -#define WLAN_GATEWAY 0x0100a8c0 //192.168.0.1 -#define MQTT_SERVER 0xD84016AC // 172.22.64.216 -#define MQTT_PORT 1883 -#define NTP_SERVER "2.de.pool.ntp.org" -#endif //HW1 - -//Network config HW1 -#ifdef HW1_2 -#define WLAN_SSID "hw1_gast" -#define WLAN_PASSWORD "KeineAhnung" #define WLAN_IP "192.168.0.67" #define WLAN_SUBNETMASK "255.255.255.0" #define WLAN_GATEWAY "192.168.0.1" diff --git a/lib/Menu/menu.cpp b/lib/Menu/menu.cpp index b2ad3fc..cbd1db3 100644 --- a/lib/Menu/menu.cpp +++ b/lib/Menu/menu.cpp @@ -20,18 +20,19 @@ void Menu::addEntry(MenuAction* entry) { // TODO: Same shit here. Why I get the first element after an increment. static uint8_t inc = 0; inc++; - if (inc == 2) { + if (inc == 1) { this->it = this->entrys.begin(); } } void Menu::printMenu() { - Serial.println("\n\n"); + std::cout << std::endl; for (std::list::iterator iter = this->entrys.begin(); iter != this->entrys.end(); iter++) { MenuAction* selectedEntry = *(iter); if (this->it == iter) - Serial.print(" "); - Serial.println(selectedEntry->getName()); + std::cout << " " << selectedEntry->getName() << std::endl; + else + std::cout << selectedEntry->getName() << std::endl; } this->inSubmenu = false; } @@ -62,11 +63,9 @@ void Menu::up() { std::list::iterator iter = this->entrys.end(); iter--; if (this->it != this->entrys.begin()) { - Serial.println("--"); this->it--; } else { - Serial.println("begin"); this->it = iter; } this->printMenu(); @@ -86,6 +85,8 @@ void Menu::right() { selectedEntry->getMenu()->setParentMenu(this); } selectedEntry->runAction(); + if (selectedEntry->getIsMenu()) + selectedEntry->getMenu()->down(); } void Menu::left() { diff --git a/lib/Menu/menu.h b/lib/Menu/menu.h index b63ac07..1cdd73a 100644 --- a/lib/Menu/menu.h +++ b/lib/Menu/menu.h @@ -13,10 +13,6 @@ #include -// Only for print in consol -// have to be deleted after display installation -#include - #include "menuAction.h" #include "menuControl.h" diff --git a/lib/Menu/menuAction.cpp b/lib/Menu/menuAction.cpp index fa8055c..1b4de3e 100644 --- a/lib/Menu/menuAction.cpp +++ b/lib/Menu/menuAction.cpp @@ -22,6 +22,14 @@ MenuAction::MenuAction(const char* name, void (*function) ()) { this->callback = nullptr; } +// MenuAction::MenuAction(const char* name, void (DriveManager:: *classFunction) ()) { +// this->name = name; +// this->classFunction = classFunction; +// this->classFunc = true; +// this->function = nullptr; +// this->callback = nullptr; +// } + MenuAction::MenuAction(const char* name, MenuControl* menu) { this->name = name; this->menu = menu; diff --git a/lib/Menu/menuControl.h b/lib/Menu/menuControl.h index 75eb0f4..162fc49 100644 --- a/lib/Menu/menuControl.h +++ b/lib/Menu/menuControl.h @@ -12,6 +12,8 @@ #ifndef MENU_CONTROLL_H #define MENU_CONTROLL_H +#include + class MenuControl { public: diff --git a/lib/MotorControl/motorControl.cpp b/lib/MotorControl/motorControl.cpp index e344118..4fb9fd7 100644 --- a/lib/MotorControl/motorControl.cpp +++ b/lib/MotorControl/motorControl.cpp @@ -34,23 +34,24 @@ void MotorControl::init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t dir_1, uin ledcWrite(this->pwm_channel, 0); } -void MotorControl::loop() { - static uint32_t last_millis = 0; +uint16_t MotorControl::loop() { uint32_t time = millis(); + uint16_t elapsed_time = time - this->last_millis; //Cancel if delay is not reached - if (time - last_millis < delay) { - return; - } + if (elapsed_time < delay) + return elapsed_time; + runMotorControl(); - last_millis = time; + this->last_millis = time; + return elapsed_time; } void MotorControl::runMotorControl() { - // Absolute difference between target_power and speed + // Absolute difference between target_power and power uint8_t abs_difference = abs(this->target_power - this->power); - // Difference between target_power and speed + // Difference between target_power and power int16_t difference = this->target_power - this->power; // Check that the target speed is close to 0 and that the abs_difference is lower than powersteps diff --git a/lib/MotorControl/motorControl.h b/lib/MotorControl/motorControl.h index d65f09e..42524c7 100644 --- a/lib/MotorControl/motorControl.h +++ b/lib/MotorControl/motorControl.h @@ -48,8 +48,9 @@ class MotorControl { * functions returns immediately. * @see runMotorControl() * @see DELAY + * @return time since the last call in Milliseconds */ - void loop(); + uint16_t loop(); /** * @brief Normaly called repeatedly by loop() to update the pwm signal. @@ -161,6 +162,8 @@ class MotorControl { uint8_t dir_2; uint8_t delay = DELAY; uint8_t powersteps = POWERSTEPS; + + uint32_t last_millis = 0; }; diff --git a/lib/Speedometer/speedometer.cpp b/lib/Speedometer/speedometer.cpp index a5da650..0e39653 100644 --- a/lib/Speedometer/speedometer.cpp +++ b/lib/Speedometer/speedometer.cpp @@ -29,24 +29,24 @@ void Speedometer::init(uint8_t pinA, uint8_t pinB, double diameter, uint16_t ste this->isInit = true; } -void Speedometer::loop() { - static uint32_t last_millis = 0; +uint16_t Speedometer::loop() { uint32_t time = millis(); + uint16_t elapsed_time = time - this->last_millis_loop; //Cancel if delay is not reached - if (time - last_millis < delay) { - return; + if (elapsed_time < delay) { + return elapsed_time; } runSpeedometer(); - last_millis = time; + this->last_millis_loop = time; + return elapsed_time; } void Speedometer::runSpeedometer() { - static uint32_t last_millis = 0; uint32_t time = millis(); - uint16_t elapsed_time = time - last_millis; - last_millis = time; + uint16_t elapsed_time = time - last_millis_calc; + last_millis_calc = time; int16_t count = encoder.getCount(); this->addValToBuf(count); diff --git a/lib/Speedometer/speedometer.h b/lib/Speedometer/speedometer.h index 3c82107..f888b8c 100644 --- a/lib/Speedometer/speedometer.h +++ b/lib/Speedometer/speedometer.h @@ -58,8 +58,9 @@ class Speedometer { * functions returns immediately. * @see runSpeedometer() * @see DELAY_SPEEDOMETER + * @return time since the last call in Milliseconds */ - void loop(); + uint16_t loop(); /** * @brief Noramly called repeatedly by loop() to calcluate new values. @@ -115,6 +116,9 @@ class Speedometer { uint8_t delay = DELAY_SPEEDOMETER; int16_t *buf; + + uint32_t last_millis_loop = 0; + uint32_t last_millis_calc = 0; }; #endif // SPEEDOMETER_H \ No newline at end of file diff --git a/src/SpecialMenus/menuManualDrive.cpp b/src/SpecialMenus/menuManualDrive.cpp new file mode 100644 index 0000000..e69dc5d --- /dev/null +++ b/src/SpecialMenus/menuManualDrive.cpp @@ -0,0 +1,20 @@ +#include "menuManualDrive.h" + +MenuManualControl::MenuManualControl(DriveManager* driveManager) { + this->driveManager = driveManager; +} + +void MenuManualControl::left() { + this->driveManager->changeModus(Modi::Off); + if (parentMenu) + this->parentMenu->printMenu(); +} + +void MenuManualControl::no() { + this->left(); +} + +void MenuManualControl::printMenu() { + this->driveManager->changeModus(Modi::ManualControl); + std::cout << "Jetzt kannst du\n fahren. :-)" << std::endl; +} \ No newline at end of file diff --git a/src/SpecialMenus/menuManualDrive.h b/src/SpecialMenus/menuManualDrive.h new file mode 100644 index 0000000..6762063 --- /dev/null +++ b/src/SpecialMenus/menuManualDrive.h @@ -0,0 +1,42 @@ +/** + * @file menuManualDrive.h + * @author Alexander Klein (alex@kleiax.de) + * @brief + * @version 0.1 + * @date 2022-01-20 + * + * @copyright Copyright (c) 2022 + * + */ + +#ifndef MENU_MANUAL_DRIVE_H +#define MENU_MANUAL_DRIVE_H + +#include "menuControl.h" +#include "driveModi/driveManager.h" + +// TODO: delete after installtion of display +#include + + + +class MenuManualControl : public MenuControl { + public: + MenuManualControl(DriveManager* driveManager); + + void down(){} + void up(){} + void right(){} + void left(); + void no(); + void yes(){} + + void printMenu(); + + private: + DriveManager* driveManager; + + +}; + +#endif // MENU_MANUAL_DRIVE_H \ No newline at end of file diff --git a/lib/Menu/menuPidSettings.cpp b/src/SpecialMenus/menuPidSettings.cpp similarity index 78% rename from lib/Menu/menuPidSettings.cpp rename to src/SpecialMenus/menuPidSettings.cpp index e155946..e2b433b 100644 --- a/lib/Menu/menuPidSettings.cpp +++ b/src/SpecialMenus/menuPidSettings.cpp @@ -62,25 +62,23 @@ void MenuPidSettings::yes() { } void MenuPidSettings::printMenu() { - Serial.println(" P I D "); - char pos[16]; + std::cout << " P I D " << std::endl; switch (this->curPos) { case 0: - sprintf(pos, "_%d.3_ %d.3 %d.3 ", this->values[0], this->values[1], this->values[2]); + printf("_%3.d_ %3.d %3.d \n", this->values[0], this->values[1], this->values[2]); break; case 1: - sprintf(pos, " %d.3 _%d.3_ %d.3 ", this->values[0], this->values[1], this->values[2]); + printf(" %3.d _%3.d_ %3.d \n", this->values[0], this->values[1], this->values[2]); break; case 2: - sprintf(pos, " %d.3 %d.3 _%d.3_", this->values[0], this->values[1], this->values[2]); + printf(" %3.d %3.d _%3.d_\n", this->values[0], this->values[1], this->values[2]); break; default: break; } - Serial.println(pos); } diff --git a/lib/Menu/menuPidSettings.h b/src/SpecialMenus/menuPidSettings.h similarity index 100% rename from lib/Menu/menuPidSettings.h rename to src/SpecialMenus/menuPidSettings.h diff --git a/src/driveModi/Modi/ManualControl/manualControl.cpp b/src/driveModi/Modi/ManualControl/manualControl.cpp index 561b98c..31d9767 100644 --- a/src/driveModi/Modi/ManualControl/manualControl.cpp +++ b/src/driveModi/Modi/ManualControl/manualControl.cpp @@ -24,8 +24,6 @@ void ManualControl::init(MoveControl *moveControl) { } void ManualControl::loop() { - this->moveControl->loop(); - static uint32_t last_millis = 0; if (millis() - last_millis < delay) { return; diff --git a/src/driveModi/driveManager.cpp b/src/driveModi/driveManager.cpp index 0827950..e2ee626 100644 --- a/src/driveModi/driveManager.cpp +++ b/src/driveModi/driveManager.cpp @@ -16,6 +16,7 @@ DriveManager::DriveManager(MoveControl *moveControl) { } void DriveManager::loop() { + this->moveControl->loop(); if (currentModus) this->currentModus->loop(); } @@ -31,11 +32,12 @@ void DriveManager::nextModus() { void DriveManager::changeModus(Modi modus) { //Set moveControl to a safe state + delete this->currentModus; + this->moveControl->setSpeed(0); this->moveControl->setRotationspeed(0); this->moveControl->setDrivingStatus(DrivingStatus::stop); - delete this->currentModus; - + switch (modus) { case Modi::Off: this->currentModus = nullptr; diff --git a/src/main.cpp b/src/main.cpp index a9ec1b8..7d20d0b 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,5 +1,6 @@ #include #include +#include #include "config.h" @@ -11,26 +12,26 @@ #include "menu.h" #include "menuAction.h" -#include "menuPidSettings.h" - -// Platzhalter, muss irgendwann weg -void dummy(){Serial.println("Dummy in Action");} - -void callbackControllerAction(); -void callbackControllerConnect(); -void controllerPrintBattery(); -void p_f(); -void i_f(); -void d_f(); -int32_t displaySelectValue(); +#include "SpecialMenus/menuPidSettings.h" +#include "SpecialMenus/menuManualDrive.h" MoveControl moveController; DriveManager driveManager(&moveController); Menu* main_m; +// TODO: Platzhalter, muss irgendwann weg +void dummy(){std::cout << "Dummy in Action" <addEntry(autopilot_e); mode_m->addEntry(captureRoute_e); @@ -82,7 +84,8 @@ void loop() { Network::checkMQTT(); driveManager.loop(); while (Serial1.available() > 0) - Serial.print(Serial1.read()); + std::cout << "In while von gps in loop in main.cpp" << std::endl; + // Serial.print(Serial1.read()); } bool isDelayReached() { @@ -112,16 +115,15 @@ void callbackControllerAction() { else if (Ps3.data.button.circle) main_m->yes(); else if (Ps3.data.button.cross) - main_m->no(); - else if (Ps3.data.button.select) - main_m->printMenu(); + main_m->no(); else if (Ps3.data.button.ps) controllerPrintBattery(); } } void callbackControllerConnect() { - Serial.println("Controller connected to ESP32"); + std::cout << "Controller connected to ESP32" << std::endl; + main_m->printMenu(); } void controllerPrintBattery() { @@ -131,11 +133,11 @@ void controllerPrintBattery() { } Serial.print("The controller battery is "); - if( controller_battery == ps3_status_battery_charging ) Serial.println("charging"); - else if( controller_battery == ps3_status_battery_full ) Serial.println("FULL"); - else if( controller_battery == ps3_status_battery_high ) Serial.println("HIGH"); - else if( controller_battery == ps3_status_battery_low) Serial.println("LOW"); - else if( controller_battery == ps3_status_battery_dying ) Serial.println("DYING"); - else if( controller_battery == ps3_status_battery_shutdown ) Serial.println("SHUTDOWN"); - else Serial.println("UNDEFINED"); + if( controller_battery == ps3_status_battery_charging ) printf("charging\n"); + else if( controller_battery == ps3_status_battery_full ) printf("FULL\n"); + else if( controller_battery == ps3_status_battery_high ) printf("HIGH\n"); + else if( controller_battery == ps3_status_battery_low) printf("LOW\n"); + else if( controller_battery == ps3_status_battery_dying ) printf("DYING\n"); + else if( controller_battery == ps3_status_battery_shutdown ) printf("SHUTDOWN\n"); + else printf("UNDEFINED\n"); } diff --git a/src/moveControl.cpp b/src/moveControl.cpp index 8bd31d2..16c60bf 100644 --- a/src/moveControl.cpp +++ b/src/moveControl.cpp @@ -38,10 +38,17 @@ MoveControl::~MoveControl() { } void MoveControl::loop() { - this->left_motor->loop(); - this->right_motor->loop(); - this->left_speedometer->loop(); - this->right_speedometer->loop(); + uint16_t left_motor_time = this->left_motor->loop(); + uint16_t right_motor_time = this->right_motor->loop(); + uint16_t left_speed_time = this->left_speedometer->loop(); + uint16_t right_speed_time = this->right_speedometer->loop(); + + if (left_motor_time > 50 + || right_motor_time > 50 + || left_speed_time > 50 + || right_speed_time > 50) { + Serial.printf("left M: %d, right M %d, left S %d, right S %d in moveControl::loop\n", left_motor_time, right_motor_time, left_speed_time, right_speed_time); + } static uint64_t last_millis = 0; if (millis() - last_millis < delay) @@ -63,6 +70,19 @@ void MoveControl::runMoveControl() { void MoveControl::setDrivingStatus(DrivingStatus status) { this->driving_status = status; + // switch (this->driving_status) { + // case DrivingStatus::stop : + // Serial.println("New drivingState = stop in MoveControl::setDrivingStatus"); + // break; + + // case DrivingStatus::drive : + // Serial.println("New drivingState = drive in MoveControl::setDrivingStatus"); + // break; + + // default: + // Serial.println("Wrong drivingState in MoveControl::regulateMotors"); + // break; + // } } void MoveControl::setSpeed(double speed) { @@ -114,19 +134,28 @@ void MoveControl::calcTargetWheelSpeed() { void MoveControl::regulateMotors() { switch (this->driving_status) { - case DrivingStatus::stop : - this->left_motor->setTargetPower(0); - this->right_motor->setTargetPower(0); - break; + case DrivingStatus::stop : + this->left_motor->setTargetPower(0); + this->right_motor->setTargetPower(0); + // Serial.println("in stop MoveControl::regulateMotors"); + // if (abs(left_speedometer->getSpeed()) > 0.2) { + // Serial.printf("Left Motor Target: %d, Ist: %d\n", this->left_motor->getTargetPower(), this->left_motor->getPower()); + // Serial.printf("Left Speedometer speed: %f\n", this->left_speedometer->getSpeed()); + // } + // if (abs(right_speedometer->getSpeed()) > 0.2) { + // Serial.printf("Right Motor Target: %d, Ist: %d\n", this->right_motor->getTargetPower(), this->right_motor->getPower()); + // Serial.printf("Right Speedometer speed: %f\n", this->right_speedometer->getSpeed()); + // } + break; - case DrivingStatus::drive : - this->left_motor->setTargetPower( (int8_t) this->left_pid_out); - this->right_motor->setTargetPower( (int8_t) this->right_pid_out); - break; + case DrivingStatus::drive : + this->left_motor->setTargetPower( (int8_t) this->left_pid_out); + this->right_motor->setTargetPower( (int8_t) this->right_pid_out); + break; - default: - Serial.println("Wrong drivingState in MoveControl::regulateMotors"); - break; + default: + Serial.println("Wrong drivingState in MoveControl::regulateMotors"); + break; } }