From 4df275ddc028f512e648c44f94f19c25c1bbbc4d Mon Sep 17 00:00:00 2001 From: Alexander Klein Date: Fri, 2 Jul 2021 08:03:00 +0200 Subject: [PATCH] restart the work --- doc/classdiagramm.uxf | 18 ++++++------ src/debugMqtt.cpp | 4 ++- src/debugMqtt.h | 3 +- src/driveModi/manualControl.cpp | 52 +++++++++++++++++++++++++++------ src/driveModi/manualControl.h | 6 ++-- src/hardware/motorControl.cpp | 4 ++- src/main.cpp | 36 +++++++++++++++++++++-- src/moveControl.cpp | 6 ++-- 8 files changed, 99 insertions(+), 30 deletions(-) diff --git a/doc/classdiagramm.uxf b/doc/classdiagramm.uxf index 1d8fd99..023ccdf 100644 --- a/doc/classdiagramm.uxf +++ b/doc/classdiagramm.uxf @@ -4,8 +4,8 @@ UMLClass - 120 - 670 + 460 + 60 220 510 @@ -46,8 +46,8 @@ MotorControl UMLClass - 360 - 670 + 80 + 480 220 330 @@ -77,8 +77,8 @@ Speedometer UMLClass - 120 - 270 + 70 + 60 270 370 @@ -111,13 +111,13 @@ Speedometer UMLClass - 1110 - 670 + 700 + 60 220 330 <<driveModi>> -ManualContro +ManualControl -- - encoder: ESP32Encoder - debug: DebugMqtt* diff --git a/src/debugMqtt.cpp b/src/debugMqtt.cpp index 5d4fd6d..fb81f8c 100644 --- a/src/debugMqtt.cpp +++ b/src/debugMqtt.cpp @@ -41,7 +41,7 @@ void DebugMqtt::writeToInflux(String measurement_name, String field_set, float m unsigned long long int nanos = DebugMqtt::getUpdatedRealMillis() * 1000000; snprintf(DebugMqtt::msg, MQTT_BUFFER_SITE, "%s %s=%f %llu", measurement_name.c_str(), field_set.c_str(), measurement, nanos); - this->sendData(Loglevel::debug, DebugMqtt::msg); + this->sendData(Loglevel::influx, DebugMqtt::msg); } void DebugMqtt::sendTime() { @@ -93,6 +93,8 @@ String DebugMqtt::enum_to_string(Loglevel loglevel) { return "Rover/Info"; case Loglevel::debug : return "Rover/Debug"; + case Loglevel::influx : + return "Rover/Influx"; default: return "INVALID ENUM"; } diff --git a/src/debugMqtt.h b/src/debugMqtt.h index 84db080..316be20 100644 --- a/src/debugMqtt.h +++ b/src/debugMqtt.h @@ -9,7 +9,8 @@ enum Loglevel { none, error, warn, info, - debug}; + debug, + influx}; class DebugMqtt { public: diff --git a/src/driveModi/manualControl.cpp b/src/driveModi/manualControl.cpp index 0279a42..fcf78ac 100644 --- a/src/driveModi/manualControl.cpp +++ b/src/driveModi/manualControl.cpp @@ -19,19 +19,44 @@ void ManualControl::runManualControl() { if (millis() - this->last_millis < RUN_MANUALCONTROL_DELAY) { return; } + //Reset if this function was called a long time ago + if (millis() - this->last_millis > 1000) { + this->reset(); + controlMode = ControMode::halt; + debug->sendMsg(Loglevel::info, "Changed ControlMode to halt by Timereset"); + } this->last_millis = millis(); - switch (this->drive_mode) { - case DriveMode::stop: + //Change controlMode + if (this->controlMode == ControMode::halt && Ps3.data.button.l3) { + this->reset(); + controlMode = ControMode::joystick; + debug->sendMsg(Loglevel::info, "Changed ControlMode to Joystick by Gamepad"); + } else if (this->controlMode == ControMode::halt && Ps3.data.button.r3) { + this->reset(); + controlMode = ControMode::trigger; + debug->sendMsg(Loglevel::info, "Changed ControlMode to ShoulderTrigger by Gamepad"); + } else if (this->controlMode == ControMode::joystick && Ps3.data.button.r3) { + this->reset(); + controlMode = ControMode::trigger; + debug->sendMsg(Loglevel::info, "Changed ControlMode to ShoulderTrigger by Gamepad"); + } else if (this->controlMode == ControMode::trigger && Ps3.data.button.l3) { + this->reset(); + controlMode = ControMode::joystick; + debug->sendMsg(Loglevel::info, "Changed ControlMode to Joystick by Gamepad"); + } + + switch (this->controlMode) { + case ControMode::halt: this->reset(); break; - case DriveMode::joystick: + case ControMode::joystick: this->driveWithControllerJoystick(); this->moveControl->runMoveControl(); break; - case DriveMode::trigger: + case ControMode::trigger: this->driveWithControllerShoulderTrigger(); break; @@ -46,21 +71,30 @@ void ManualControl::driveWithControllerJoystick() { double value_per_step = MANUALCONTROL_MAX_SPEED * 2 / 256; this->moveControl->setSpeed((y * -1) * value_per_step); + char str[64]; + sprintf(str, "%f", (y * -1) * value_per_step); + debug->sendMsg(Loglevel::debug, str); value_per_step = MANUALCONTROL_MAX_ROTATION * 2 / 256; this->moveControl->setRotationspeed(x * value_per_step); } -void ManualControl::changeDriveMode(DriveMode drive_mode) { - this->drive_mode = drive_mode; +void ManualControl::changeDriveMode(ControMode drive_mode) { + this->reset(); + controlMode = drive_mode; + debug->sendMsg(Loglevel::info, "Changed ControlMode by function"); } void ManualControl::driveWithControllerShoulderTrigger() { uint8_t nowL = Ps3.data.analog.button.l2; - uint8_t nowR = Ps3.data.analog.button.r2; + uint8_t nowR = Ps3.data.analog.button.r2;; - map(nowL, 0, 255, 0, 100); - map(nowR, 0, 255, 0, 100); + nowL = map(nowL, 0, 255, 0, 100); + nowR = map(nowR, 0, 255, 0, 100); + + // char str[64]; + // sprintf(str, "nachher: nowL: %u, nowR: %u", nowL, nowR); + // debug->sendMsg(Loglevel::debug, str); if (nowL != this->oldL) { this->motorControlLeft->setTargetPower(nowL); diff --git a/src/driveModi/manualControl.h b/src/driveModi/manualControl.h index 468a8e3..83fb157 100644 --- a/src/driveModi/manualControl.h +++ b/src/driveModi/manualControl.h @@ -7,13 +7,13 @@ #include "hardware/motorControl.h" #include "debugMqtt.h" -enum DriveMode {stop, joystick, trigger}; +enum ControMode {halt, joystick, trigger}; class ManualControl { public: ManualControl(); void init(MoveControl *moveControl, MotorControl *motorControlLeft, MotorControl *motorControlRight); void runManualControl(); - void changeDriveMode(DriveMode drive_mode); + void changeDriveMode(ControMode drive_mode); private: void driveWithControllerJoystick(); @@ -25,7 +25,7 @@ class ManualControl { MotorControl *motorControlRight; DebugMqtt *debug; - DriveMode drive_mode = DriveMode::stop; + ControMode controlMode = ControMode::halt; uint32_t last_millis = 0; // Is needed for driveWithControllerShoulderTrigger() diff --git a/src/hardware/motorControl.cpp b/src/hardware/motorControl.cpp index 8408169..f936595 100644 --- a/src/hardware/motorControl.cpp +++ b/src/hardware/motorControl.cpp @@ -89,7 +89,9 @@ void MotorControl::setTargetPower(int8_t power) { sprintf(str, "target_power_%s", this->name.c_str()); debug->writeToInflux("motorControl", str, power); } else { - debug->sendMsg(Loglevel::error, "Invalid Argument in setTargetPower!"); + char str[64]; + sprintf(str, "Invalid Argument (power: %d) in setTargetPower!", power); + debug->sendMsg(Loglevel::error, str); } } diff --git a/src/main.cpp b/src/main.cpp index d288f9d..c61ec35 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -5,9 +5,13 @@ #include #include "config.h" + #include "hardware/motorControl.h" -#include "moveControl.h" #include "hardware/speedometer.h" +#include "driveModi/autopilot.h" +#include "driveModi/captureRoute.h" +#include "driveModi/manualControl.h" +#include "moveControl.h" #include "debugMqtt.h" void callbackControllerAction(); @@ -23,6 +27,8 @@ Speedometer speedometer_right; MoveControl moveController; DebugMqtt debugger("main"); +ManualControl manualControl; + IPAddress local_IP(WLAN_IP); IPAddress gateway(WLAN_GATEWAY); IPAddress subnet(WLAN_SUBNETMASK); @@ -32,6 +38,9 @@ WiFiClient wifi_client; PubSubClient mqtt_client(wifi_client); int controller_battery = -1; +enum DriveMode {autopilot_e, captureRoute_e, manualControl_e}; +DriveMode driveMode = manualControl_e; + void setup() { Serial.begin(115200); @@ -66,10 +75,11 @@ void setup() { Ps3.attach(callbackControllerAction); Ps3.attachOnConnect(callbackControllerConnect); - Ps3.attachOnDisconnect(callbackControllerDisconnect); + // Ps3.attachOnDisconnect(callbackControllerDisconnect); Serial.println("\nReady to connect a PS3 Controller... \n"); Ps3.begin(); + // Init hardware left_motor.init(M_PWM_1, PWM_CHANNEL_M1, M_DIR_11, M_DIR_12, "left"); right_motor.init(M_PWM_2, PWM_CHANNEL_M2, M_DIR_21, M_DIR_22, "right"); @@ -77,6 +87,9 @@ void setup() { speedometer_right.init(M_ENCODE_2A, M_ENCODE_2B, "right"); moveController.init(&left_motor, &right_motor, &speedometer_left, &speedometer_right); + + // Init driveModi + manualControl.init(&moveController, &left_motor, &right_motor); } void loop() { @@ -88,6 +101,23 @@ void loop() { right_motor.runMotorControl(); speedometer_left.runSpeedometer(); speedometer_right.runSpeedometer(); + + switch (driveMode) { + case manualControl_e: + manualControl.runManualControl(); + break; + + case captureRoute_e: + /* code */ + break; + + case autopilot_e: + /* code */ + break; + + default: + break; + } } void reconnectMqtt() { @@ -120,7 +150,7 @@ void callbackControllerAction() { void callbackControllerConnect() { Serial.println("Controller connected to ESP32"); - debugger.sendMsg(Loglevel::info, "Controller connected to ESP32") + debugger.sendMsg(Loglevel::info, "Controller connected to ESP32"); } void controllerPrintBattery() { diff --git a/src/moveControl.cpp b/src/moveControl.cpp index 98d6152..57bedba 100644 --- a/src/moveControl.cpp +++ b/src/moveControl.cpp @@ -41,9 +41,9 @@ void MoveControl::setRotationspeed(double speed) { } void MoveControl::calcWheelSpeed() { - // original formula: - // (1 / r) / 1 b \ / x \ = / Xl \ - // \ 1 -b / \ T / \ Xr / + /* original formula: + (1 / r) / 1 b \ / x \ = / Xl \ + \ 1 -b / \ T / \ Xr / */ // (1 / r) * 1 const static double A = 15.82278481;